From 5a050119920415aaafc9ed6c769f0bf5b9c34a07 Mon Sep 17 00:00:00 2001 From: lwvmobile Date: Mon, 18 Dec 2023 18:35:53 -0500 Subject: [PATCH] DMR - Inplement LIP; Reorganize Various GPS Reports; --- include/dsd.h | 6 +- src/dmr_block.c | 101 ------- src/dmr_dburst.c | 14 +- src/dmr_flco.c | 105 ------- src/dsd_gps.c | 695 +++++++++++++++++++++++++++++++++++++++++++++++ src/p25_lcw.c | 106 -------- src/p25p2_vpdu.c | 245 ----------------- 7 files changed, 707 insertions(+), 565 deletions(-) create mode 100644 src/dsd_gps.c diff --git a/include/dsd.h b/include/dsd.h index 3a8a5fa..ee4cad4 100644 --- a/include/dsd.h +++ b/include/dsd.h @@ -1111,11 +1111,15 @@ uint32_t dmr_34(uint8_t * input, uint8_t treturn[18]); //simplier trellis decode void beeper (dsd_opts * opts, dsd_state * state, int lr); //the tone beeper function void dmr_gateway_identifier (uint32_t source, uint32_t target); //translate special addresses -//Embedded Alias and GPS +//Embedded Alias and GPS reports void dmr_embedded_alias_header (dsd_opts * opts, dsd_state * state, uint8_t lc_bits[]); void dmr_embedded_alias_blocks (dsd_opts * opts, dsd_state * state, uint8_t lc_bits[]); 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); //"DMR STEREO" void dmrBSBootstrap (dsd_opts * opts, dsd_state * state); diff --git a/src/dmr_block.c b/src/dmr_block.c index e454789..5e9ed11 100644 --- a/src/dmr_block.c +++ b/src/dmr_block.c @@ -380,107 +380,6 @@ void dmr_dheader (dsd_opts * opts, dsd_state * state, uint8_t dheader[], uint8_t } -void nmea_iec_61162_1 (dsd_opts * opts, dsd_state * state, uint8_t * input, uint32_t src, int type) -{ - int slot = state->currentslot; - - //NOTE: The Only difference between Short (type == 1) and Long Format (type == 2) - //is the utc_ss3 on short vs utc_ss6 and inclusion of COG value on long - - //NOTE: MFID Specific Formats are not handled here, unknown, could be added if worked out - //they could share most of the same elements and use the large spare bits in block 2 for extra - // uint8_t mfid = (uint8_t)ConvertBitIntoBytes(&input[88], 8); //on type 3, last octet of first block carries MFID - - // uint8_t nmea_c = input[0]; //encrypted -- checked before we get here - uint8_t nmea_ns = input[1]; //north/south (lat sign) - uint8_t nmea_ew = input[2]; //east/west (lon sign) - uint8_t nmea_q = input[3]; //Quality Indicator (no fix or fix valid) - uint8_t nmea_speed = (uint8_t)ConvertBitIntoBytes(&input[4], 7); //speed in knots (127 = greater than 126 knots) - - //Latitude Bits - uint8_t nmea_ndeg = (uint8_t)ConvertBitIntoBytes(&input[11], 7); //Latitude Degrees - uint8_t nmea_nmin = (uint8_t)ConvertBitIntoBytes(&input[18], 6); //Latitude Minutes - uint16_t nmea_nminf = (uint16_t)ConvertBitIntoBytes(&input[24], 14); //Latitude Fractions of Minutes - - //Longitude Bits - uint8_t nmea_edeg = (uint8_t)ConvertBitIntoBytes(&input[38], 8); //Longitude Degrees - uint8_t nmea_emin = (uint8_t)ConvertBitIntoBytes(&input[46], 6); //Longitude Minutes - uint16_t nmea_eminf = (uint16_t)ConvertBitIntoBytes(&input[52], 14); //Longitude Fractions of Minutes - - //UTC Time and COG - uint8_t nmea_utc_hh = (uint8_t)ConvertBitIntoBytes(&input[66], 5); - uint8_t nmea_utc_mm = (uint8_t)ConvertBitIntoBytes(&input[71], 6); - //seconds and the addition of COG is the difference between short and long formats - uint8_t nmea_utc_ss3 = (uint8_t)ConvertBitIntoBytes(&input[77], 3); //seconds in 10s - uint8_t nmea_utc_ss6 = (uint8_t)ConvertBitIntoBytes(&input[77], 6); //seconds in 1s - uint16_t nmea_cog = (uint16_t)ConvertBitIntoBytes(&input[103], 9); //course over ground in degrees - - //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; //unit to convert min into decimal value - (1/60)*60 minutes = 1 degree - // float mm_unit = 1.0f / 10000.0f; //unit to convert minf into decimal value - (0000 - 9999) 0.0001×9999 = .9999 minutes, so its sub 1 minute decimal - - //testing w/ Harris NMEA like values (ran tests over there with this code, this seems to work on those values) - float m_unit = 1.0f / 60.0f; //unit to convert min into decimal value - (1/60)*60 minutes = 1 degree - float mm_unit = 1.0f / 600000.0f; //unit to convert minf into decimal value - (0000 - 9999) 0.0001×9999 = .9999 minutes, so its sub 1 minute decimal - - //speed conversion - 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; - - //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) ); - - if (!nmea_ns) latitude *= -1.0f; //0 is South, 1 is North - if (!nmea_ew) longitude *= -1.0f; //0 is West, 1 is East - - fprintf (stderr, " GPS: %f%s, %f%s;", latitude, deg_glyph, longitude, deg_glyph); - - //Speed in Knots - if (nmea_speed > 126) - fprintf (stderr, " SPD > 126 knots or %f kph;", fkph); - else - fprintf (stderr, " SPD: %d knots; %f kph;", nmea_speed, fkph); - - //Print UTC Time and COG according to Format Type - if (type == 1) - fprintf (stderr, " FIX: %d; %02d:%02d:%02d UTC; Short Format;", nmea_q, nmea_utc_hh, nmea_utc_mm, nmea_utc_ss3); - if (type == 2) - fprintf (stderr, " FIX: %d; %02d:%02d:%02d UTC; COG: %d%s; Long Format;", nmea_q, nmea_utc_hh, nmea_utc_mm, nmea_utc_ss6, nmea_cog, deg_glyph); - - //save to ncurses string - sprintf (state->dmr_embedded_gps[slot], "GPS: (%f%s, %f%s)", latitude, deg_glyph, longitude, deg_glyph); - - //save to LRRP report for mapping/logging - FILE * pFile; //file pointer - if (opts->lrrp_file_output == 1) - { - int s = (int)fkph; //rounded interger format for the log report - int a = 0; - if (type == 2) - a = nmea_cog; //long format only - //open file by name that is supplied in the ncurses terminal, or cli - pFile = fopen (opts->lrrp_out_file, "a"); - fprintf (pFile, "%s\t", getDateL() ); - fprintf (pFile, "%s\t", getTimeL() ); //could switch to UTC time if desired, but would require local user offset - 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); - - } - -} - void dmr_udt_decoder (dsd_opts * opts, dsd_state * state, uint8_t * block_bytes, uint32_t CRCCorrect) { //TODO: double check end rep values to Text Format Modes vs padnibs/uab values diff --git a/src/dmr_dburst.c b/src/dmr_dburst.c index f6f83ac..bb3710c 100644 --- a/src/dmr_dburst.c +++ b/src/dmr_dburst.c @@ -6,13 +6,13 @@ * Source: https://github.com/LouisErigHerve/dsd/blob/master/src/dmr_sync.c * * LWVMOBILE - * 2022-12 DSD-FME Florida Man Edition + * 2023-12 DSD-FME Florida Man Edition *-----------------------------------------------------------------------------*/ -//WIP: Unified Single Block Data - USBD -- TODO: USBD LIP Protocol Handling -//WIP: UDT Header and Blocks (Assembly Working Now, Need UDT Protocol Handling Function) -//WIP: UDT Full Message CRC16 (Seems to work, but need to test more, could be just sample w/ errors in it) -//WIP: CRC9/CRC32 on Rate 1 Data (need samples) +//TODO: Test USBD LIP Decoder with Real World Samples (if/when available) +//TODO: Test UDT NMEA and LIP Decoders with Real World Samples (if/when available) +//WIP: Move all extra decoders for location, etc, to a new file and add prototypes to dsd.h +//TODO: Test CRC9/CRC32 on Rate 1 Data with Real World Samples (if/when available) //TODO: Address areas that require reading of ISO7, ISO8, and UTF-16 string formats #include "dsd.h" @@ -575,8 +575,8 @@ void dmr_data_burst_handler(dsd_opts * opts, dsd_state * state, uint8_t info[196 //TODO: provide more robust handling at a later date -- ETSI TS 102 361-4 V1.11.1 (2021-01) 6.6.11.3 usbd_st = (uint8_t)ConvertBitIntoBytes(&DMR_PDU_bits[0], 4); fprintf (stderr, "%s\n", KYEL); - fprintf (stderr, " Unified Single Block Data - "); - if (usbd_st == 0) fprintf (stderr, "Location Information Protocol"); + fprintf (stderr, " USBD - "); + if (usbd_st == 0) lip_protocol_decoder (opts, state, DMR_PDU_bits); else if (usbd_st > 8) fprintf (stderr, "Manufacturer Specific Service %d ", usbd_st); else fprintf (stderr, "Reserved %d ", usbd_st); } diff --git a/src/dmr_flco.c b/src/dmr_flco.c index 2c54880..80f9eed 100644 --- a/src/dmr_flco.c +++ b/src/dmr_flco.c @@ -1212,108 +1212,3 @@ void dmr_embedded_alias_blocks (dsd_opts * opts, dsd_state * state, uint8_t lc_b fprintf (stderr, "%s", KNRM); } - -//externalize embedded GPS - Confirmed working now on NE, NW, SE, and SW coordinates -void dmr_embedded_gps (dsd_opts * opts, dsd_state * state, uint8_t lc_bits[]) -{ - UNUSED(opts); - - fprintf (stderr, "%s", KYEL); - fprintf (stderr, " Embedded GPS:"); - uint8_t slot = state->currentslot; - uint8_t pf = lc_bits[0]; - uint8_t res_a = lc_bits[1]; - uint8_t res_b = (uint8_t)ConvertBitIntoBytes(&lc_bits[16], 4); - uint8_t pos_err = (uint8_t)ConvertBitIntoBytes(&lc_bits[20], 3); - UNUSED2(res_a, res_b); - - char deg_glyph[4]; - sprintf (deg_glyph, "%s", "°"); - - uint32_t lon_sign = lc_bits[23]; - uint32_t lon = (uint32_t)ConvertBitIntoBytes(&lc_bits[24], 24); - uint32_t lat_sign = lc_bits[48]; - uint32_t lat = (uint32_t)ConvertBitIntoBytes(&lc_bits[49], 23); - double lon_sf = 1.0f; //float value we can multiple longitude with - double lat_sf = 1.0f; //float value we can multiple latitude with - - double lat_unit = (double)180/ pow (2.0, 24); //180 divided by 2^24 - double lon_unit = (double)360/ pow (2.0, 25); //360 divided by 2^25 - - char latstr[3]; - char lonstr[3]; - sprintf (latstr, "%s", "N"); - sprintf (lonstr, "%s", "E"); - - //run calculations and print - //7.2.16 and 7.2.17 (two's compliment) - - double latitude = 0; - double longitude = 0; - - if (pf) fprintf (stderr, " Protected"); - else - { - if (lat_sign) - { - lat = 0x800001 - lat; - sprintf (latstr, "%s", "S"); - lat_sf = -1.0f; - } - latitude = ((double)lat * lat_unit); - - if (lon_sign) - { - lon = 0x1000001 - lon; - sprintf (lonstr, "%s", "W"); - lon_sf = -1.0f; - } - longitude = ((double)lon * lon_unit); - - //sanity check - if (abs (latitude) < 90 && abs(longitude) < 180) - { - fprintf (stderr, " Lat: %.5lf%s%s Lon: %.5lf%s%s (%.5lf, %.5lf)", latitude, deg_glyph, latstr, longitude, deg_glyph, lonstr, lat_sf * latitude, lon_sf * longitude); - - //7.2.15 Position Error - uint16_t position_error = 2 * pow(10, pos_err); //2 * 10^pos_err - if (pos_err == 0x7 ) fprintf (stderr, "\n Position Error: Unknown or Invalid"); - else fprintf (stderr, "\n Position Error: Less than %dm", position_error); - - //save to array for ncurses - if (pos_err != 0x7) - { - sprintf (state->dmr_embedded_gps[slot], "GPS: %.5lf%s%s %.5lf%s%s Err: %dm", latitude, deg_glyph, latstr, longitude, deg_glyph, lonstr, position_error); - } - else sprintf (state->dmr_embedded_gps[slot], "GPS: %.5lf%s%s %.5lf%s%s Unknown Pos Err", latitude, deg_glyph, latstr, longitude, deg_glyph, lonstr); - - //save to LRRP report for mapping/logging - FILE * pFile; //file pointer - if (opts->lrrp_file_output == 1) - { - int src = 0; - if (slot == 0) src = state->lasttg; - if (slot == 1) src = state->lasttgR; - - //open file by name that is supplied in the ncurses terminal, or cli - pFile = fopen (opts->lrrp_out_file, "a"); - fprintf (pFile, "%s\t", getDateL() ); - fprintf (pFile, "%s\t", getTimeL() ); - fprintf (pFile, "%08d\t", src); - fprintf (pFile, "%.5lf\t", latitude); - fprintf (pFile, "%.5lf\t", longitude); - fprintf (pFile, "0\t " ); //zero for velocity - fprintf (pFile, "0\t " ); //zero for azimuth - fprintf (pFile, "\n"); - fclose (pFile); - - } - - } - - - - } - - fprintf (stderr, "%s", KNRM); -} diff --git a/src/dsd_gps.c b/src/dsd_gps.c new file mode 100644 index 0000000..144f717 --- /dev/null +++ b/src/dsd_gps.c @@ -0,0 +1,695 @@ +/*------------------------------------------------------------------------------- + * dsd_gps.c + * GPS Handling Functions for Various Protocols + * + * LWVMOBILE + * 2023-12 DSD-FME Florida Man Edition + *-----------------------------------------------------------------------------*/ + +#include "dsd.h" + +void lip_protocol_decoder (dsd_opts * opts, dsd_state * state, uint8_t * input) +{ + //NOTE: This is defined in ETSI TS 102 361-4 V1.12.1 (2023-07) p208 + //6.6.11.3.2 USBD Polling Service Poll Response PDU for LIP (That's a mouthful) + + int slot = state->currentslot; + + //NOTE: This format is pretty much the same as DMR EMB GPS, but has a few extra elements, + //so I got lazy and just lifted most of the code from there, also assuming same lat/lon calcs + //since those have been tested to work in DMR EMB GPS and units are same in LIP + + fprintf (stderr, "Location Information Protocol; "); + + // uint8_t service_type = (uint8_t)ConvertBitIntoBytes(&input[0], 4); //checked before arrival here + uint8_t time_elapsed = (uint8_t)ConvertBitIntoBytes(&input[6], 2); + uint8_t lon_sign = input[8]; + uint32_t lon = (uint32_t)ConvertBitIntoBytes(&input[9], 24); //8, 25 + uint8_t lat_sign = input[33]; + uint32_t lat = (uint32_t)ConvertBitIntoBytes(&input[34], 23); //33, 24 + uint8_t pos_err = (uint8_t)ConvertBitIntoBytes(&input[57], 2); + uint8_t hor_vel = (uint8_t)ConvertBitIntoBytes(&input[59], 7); + uint8_t dir_tra = (uint8_t)ConvertBitIntoBytes(&input[66], 4); + uint8_t reason = (uint8_t)ConvertBitIntoBytes(&input[70], 3); + uint8_t add_hash = (uint8_t)ConvertBitIntoBytes(&input[73], 8); //MS Source Address Hash + + //NOTE: May need to use double instead of float to avoid rounding errors + double latitude, longitude = 0.0f; + double lat_unit = (double)180/ pow (2.0, 24); //180 divided by 2^24 -- 6.3.30 + double lon_unit = (double)360/ pow (2.0, 25); //360 divided by 2^25 -- 6.3.50 + double lon_sf = 1.0f; //float value we can multiple longitude with + double lat_sf = 1.0f; //float value we can multiple latitude with + + char latstr[3]; + char lonstr[3]; + sprintf (latstr, "%s", "N"); + sprintf (lonstr, "%s", "E"); + + char deg_glyph[4]; + sprintf (deg_glyph, "%s", "°"); + + //lat and lon calculations + if (lat_sign) + { + lat = 0x800001 - lat; + sprintf (latstr, "%s", "S"); + lat_sf = -1.0f; + } + latitude = ((double)lat * lat_unit); + + if (lon_sign) + { + lon = 0x1000001 - lon; + sprintf (lonstr, "%s", "W"); + lon_sf = -1.0f; + } + longitude = ((double)lon * lon_unit); + + //6.3.63 Position Error + //6.3.17 Horizontal velocity + /* + Horizontal velocity shall be encoded for speeds 0 km/h to 28 km/h in 1 km/h steps and + from 28 km/h onwards using equation: v = C × (1 + x)^(K-A) + B where: + */ + float v = 0.0f; + float C = 16.0f; + float x = 0.038f; + float A = 13.0f; + float K = (float)hor_vel; + float B = 0.0f; + + if (hor_vel > 28) + v = C * (pow(1+x, K-A)) + B; //I think this pow function is set up correctly + else v = (float)hor_vel; + + float dir = (((float)dir_tra + 11.25f)/22.5f); //page 68, Table 6.45 + + //truncated and rounded forms + int vt = (int)v; + int dt = (int)dir; + + //sanity check + if (abs (latitude) < 90 && abs(longitude) < 180) + { + fprintf (stderr, "Src(Hash); %03d; Lat: %.5lf%s%s Lon: %.5lf%s%s (%.5lf, %.5lf); Spd: %d km/h; Dir: %d%s",add_hash, latitude, deg_glyph, latstr, longitude, deg_glyph, lonstr, lat_sf * latitude, lon_sf * longitude, vt, dt, deg_glyph); + + //6.3.63 Position Error + uint16_t position_error = 2 * pow(10, pos_err); //2 * 10^pos_err + if (pos_err == 0x7 ) fprintf (stderr, "\n Position Error: Unknown or Invalid;"); + else fprintf (stderr, "\n Position Error: Less than %dm;", position_error); + + //Reason For Sending 6.6.11.3.3 Table 6.80 + if (reason) fprintf (stderr, " Reserved: %d;", reason); + else fprintf (stderr, " Request Response; "); + + //6.3.78 Time elapsed + if (time_elapsed == 0) fprintf (stderr, " TE: < 5s;"); + if (time_elapsed == 1) fprintf (stderr, " TE: < 5m;"); + if (time_elapsed == 2) fprintf (stderr, " TE: < 30m;"); + if (time_elapsed == 3) fprintf (stderr, " TE: NA or UNK;"); //not applicable or unknown + + //save to array for ncurses + if (pos_err != 0x7) + { + sprintf (state->dmr_embedded_gps[slot], "%03d; LIP: %.5lf%s%s %.5lf%s%s; Err: %dm; Spd: %d km/h; Dir: %d%s", add_hash, latitude, deg_glyph, latstr, longitude, deg_glyph, lonstr, position_error, vt, dt, deg_glyph); + } + else sprintf (state->dmr_embedded_gps[slot], "%03d; LIP: %.5lf%s%s %.5lf%s%s Unknown Pos Err; Spd: %d km/h; Dir %d%s", add_hash, latitude, deg_glyph, latstr, longitude, deg_glyph, lonstr, vt, dt, deg_glyph); + + //save to LRRP report for mapping/logging + FILE * pFile; //file pointer + if (opts->lrrp_file_output == 1) + { + //open file by name that is supplied in the ncurses terminal, or cli + pFile = fopen (opts->lrrp_out_file, "a"); + fprintf (pFile, "%s\t", getDateL() ); + fprintf (pFile, "%s\t", getTimeL() ); + fprintf (pFile, "%08d\t", add_hash); + fprintf (pFile, "%.5lf\t", latitude); + fprintf (pFile, "%.5lf\t", longitude); + fprintf (pFile, "%d\t", vt); //speed in km/h + fprintf (pFile, "%d\t", dt); //direction of travel + fprintf (pFile, "\n"); + fclose (pFile); + + } + + } + else fprintf (stderr, " Position Calculation Error;"); + +} + +void nmea_iec_61162_1 (dsd_opts * opts, dsd_state * state, uint8_t * input, uint32_t src, int type) +{ + int slot = state->currentslot; + + //NOTE: The Only difference between Short (type == 1) and Long Format (type == 2) + //is the utc_ss3 on short vs utc_ss6 and inclusion of COG value on long + + //NOTE: MFID Specific Formats are not handled here, unknown, could be added if worked out + //they could share most of the same elements and use the large spare bits in block 2 for extra + // uint8_t mfid = (uint8_t)ConvertBitIntoBytes(&input[88], 8); //on type 3, last octet of first block carries MFID + + // uint8_t nmea_c = input[0]; //encrypted -- checked before we get here + uint8_t nmea_ns = input[1]; //north/south (lat sign) + uint8_t nmea_ew = input[2]; //east/west (lon sign) + uint8_t nmea_q = input[3]; //Quality Indicator (no fix or fix valid) + uint8_t nmea_speed = (uint8_t)ConvertBitIntoBytes(&input[4], 7); //speed in knots (127 = greater than 126 knots) + + //Latitude Bits + uint8_t nmea_ndeg = (uint8_t)ConvertBitIntoBytes(&input[11], 7); //Latitude Degrees + uint8_t nmea_nmin = (uint8_t)ConvertBitIntoBytes(&input[18], 6); //Latitude Minutes + uint16_t nmea_nminf = (uint16_t)ConvertBitIntoBytes(&input[24], 14); //Latitude Fractions of Minutes + + //Longitude Bits + uint8_t nmea_edeg = (uint8_t)ConvertBitIntoBytes(&input[38], 8); //Longitude Degrees + uint8_t nmea_emin = (uint8_t)ConvertBitIntoBytes(&input[46], 6); //Longitude Minutes + uint16_t nmea_eminf = (uint16_t)ConvertBitIntoBytes(&input[52], 14); //Longitude Fractions of Minutes + + //UTC Time and COG + uint8_t nmea_utc_hh = (uint8_t)ConvertBitIntoBytes(&input[66], 5); + uint8_t nmea_utc_mm = (uint8_t)ConvertBitIntoBytes(&input[71], 6); + //seconds and the addition of COG is the difference between short and long formats + uint8_t nmea_utc_ss3 = (uint8_t)ConvertBitIntoBytes(&input[77], 3); //seconds in 10s + uint8_t nmea_utc_ss6 = (uint8_t)ConvertBitIntoBytes(&input[77], 6); //seconds in 1s + uint16_t nmea_cog = (uint16_t)ConvertBitIntoBytes(&input[103], 9); //course over ground in degrees + + //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; //unit to convert min into decimal value - (1/60)*60 minutes = 1 degree + // float mm_unit = 1.0f / 10000.0f; //unit to convert minf into decimal value - (0000 - 9999) 0.0001×9999 = .9999 minutes, so its sub 1 minute decimal + + //testing w/ Harris NMEA like values (ran tests over there with this code, this seems to work on those values) + float m_unit = 1.0f / 60.0f; //unit to convert min into decimal value - (1/60)*60 minutes = 1 degree + float mm_unit = 1.0f / 600000.0f; //unit to convert minf into decimal value - (0000 - 9999) 0.0001×9999 = .9999 minutes, so its sub 1 minute decimal + + //speed conversion + 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; + + //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) ); + + if (!nmea_ns) latitude *= -1.0f; //0 is South, 1 is North + if (!nmea_ew) longitude *= -1.0f; //0 is West, 1 is East + + fprintf (stderr, " GPS: %f%s, %f%s;", latitude, deg_glyph, longitude, deg_glyph); + + //Speed in Knots + if (nmea_speed > 126) + fprintf (stderr, " SPD > 126 knots or %f kph;", fkph); + else + fprintf (stderr, " SPD: %d knots; %f kph;", nmea_speed, fkph); + + //Print UTC Time and COG according to Format Type + if (type == 1) + fprintf (stderr, " FIX: %d; %02d:%02d:%02d UTC; Short Format;", nmea_q, nmea_utc_hh, nmea_utc_mm, nmea_utc_ss3); + if (type == 2) + fprintf (stderr, " FIX: %d; %02d:%02d:%02d UTC; COG: %d%s; Long Format;", nmea_q, nmea_utc_hh, nmea_utc_mm, nmea_utc_ss6, nmea_cog, deg_glyph); + + //save to ncurses string + sprintf (state->dmr_embedded_gps[slot], "GPS: (%f%s, %f%s)", latitude, deg_glyph, longitude, deg_glyph); + + //save to LRRP report for mapping/logging + FILE * pFile; //file pointer + if (opts->lrrp_file_output == 1) + { + int s = (int)fkph; //rounded interger format for the log report + int a = 0; + if (type == 2) + a = nmea_cog; //long format only + //open file by name that is supplied in the ncurses terminal, or cli + pFile = fopen (opts->lrrp_out_file, "a"); + fprintf (pFile, "%s\t", getDateL() ); + fprintf (pFile, "%s\t", getTimeL() ); //could switch to UTC time if desired, but would require local user offset + 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); + + } + +} + +//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) +{ + + //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 + + 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 + + //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 + + //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 + + //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 + + //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 + + //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; + + //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; + + //speed conversion + 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); + + //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) ); + + //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 + + fprintf (stderr, "\n"); + + 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 (nmea_speed > 126) + fprintf (stderr, " SPD > 126 knots or %f MPH;", fmph); //using MPH here, its Harris, they're in the U.S. + else + fprintf (stderr, " SPD: %d knots; %f MPH;", nmea_speed, fmph); + + //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], "GPS: (%f%s, %f%s)", latitude, deg_glyph, longitude, deg_glyph); + + //save to LRRP report for mapping/logging + FILE * pFile; //file pointer + if (opts->lrrp_file_output == 1) + { + //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", getDateL() ); + fprintf (pFile, "%s\t", getTimeL() ); //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); + + } + + //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) + { + //open file by name that is supplied in the ncurses terminal, or cli + pFile = fopen (opts->lrrp_out_file, "a"); + fprintf (pFile, "%s\t", getDateL() ); + fprintf (pFile, "%s\t", getTimeL() ); + fprintf (pFile, "%08d\t", src); + fprintf (pFile, "%.6lf\t", lat_dec); + fprintf (pFile, "%.6lf\t", lon_dec); + fprintf (pFile, "%d\t ", s); + fprintf (pFile, "%d\t ", a); + fprintf (pFile, "\n"); + fclose (pFile); + + } + + //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 +} + +//externalize embedded GPS - Confirmed working now on NE, NW, SE, and SW coordinates +void dmr_embedded_gps (dsd_opts * opts, dsd_state * state, uint8_t lc_bits[]) +{ + UNUSED(opts); + + fprintf (stderr, "%s", KYEL); + fprintf (stderr, " Embedded GPS:"); + uint8_t slot = state->currentslot; + uint8_t pf = lc_bits[0]; + uint8_t res_a = lc_bits[1]; + uint8_t res_b = (uint8_t)ConvertBitIntoBytes(&lc_bits[16], 4); + uint8_t pos_err = (uint8_t)ConvertBitIntoBytes(&lc_bits[20], 3); + UNUSED2(res_a, res_b); + + char deg_glyph[4]; + sprintf (deg_glyph, "%s", "°"); + + uint32_t lon_sign = lc_bits[23]; + uint32_t lon = (uint32_t)ConvertBitIntoBytes(&lc_bits[24], 24); + uint32_t lat_sign = lc_bits[48]; + uint32_t lat = (uint32_t)ConvertBitIntoBytes(&lc_bits[49], 23); + double lon_sf = 1.0f; //float value we can multiple longitude with + double lat_sf = 1.0f; //float value we can multiple latitude with + + double lat_unit = (double)180/ pow (2.0, 24); //180 divided by 2^24 + double lon_unit = (double)360/ pow (2.0, 25); //360 divided by 2^25 + + char latstr[3]; + char lonstr[3]; + sprintf (latstr, "%s", "N"); + sprintf (lonstr, "%s", "E"); + + //run calculations and print + //7.2.16 and 7.2.17 (two's compliment) + + double latitude = 0; + double longitude = 0; + + if (pf) fprintf (stderr, " Protected"); + else + { + if (lat_sign) + { + lat = 0x800001 - lat; + sprintf (latstr, "%s", "S"); + lat_sf = -1.0f; + } + latitude = ((double)lat * lat_unit); + + if (lon_sign) + { + lon = 0x1000001 - lon; + sprintf (lonstr, "%s", "W"); + lon_sf = -1.0f; + } + longitude = ((double)lon * lon_unit); + + //sanity check + if (abs (latitude) < 90 && abs(longitude) < 180) + { + fprintf (stderr, " Lat: %.5lf%s%s Lon: %.5lf%s%s (%.5lf, %.5lf)", latitude, deg_glyph, latstr, longitude, deg_glyph, lonstr, lat_sf * latitude, lon_sf * longitude); + + //7.2.15 Position Error + uint16_t position_error = 2 * pow(10, pos_err); //2 * 10^pos_err + if (pos_err == 0x7 ) fprintf (stderr, "\n Position Error: Unknown or Invalid"); + else fprintf (stderr, "\n Position Error: Less than %dm", position_error); + + //save to array for ncurses + if (pos_err != 0x7) + { + sprintf (state->dmr_embedded_gps[slot], "GPS: %.5lf%s%s %.5lf%s%s Err: %dm", latitude, deg_glyph, latstr, longitude, deg_glyph, lonstr, position_error); + } + else sprintf (state->dmr_embedded_gps[slot], "GPS: %.5lf%s%s %.5lf%s%s Unknown Pos Err", latitude, deg_glyph, latstr, longitude, deg_glyph, lonstr); + + //save to LRRP report for mapping/logging + FILE * pFile; //file pointer + if (opts->lrrp_file_output == 1) + { + int src = 0; + if (slot == 0) src = state->lasttg; + if (slot == 1) src = state->lasttgR; + + //open file by name that is supplied in the ncurses terminal, or cli + pFile = fopen (opts->lrrp_out_file, "a"); + fprintf (pFile, "%s\t", getDateL() ); + fprintf (pFile, "%s\t", getTimeL() ); + fprintf (pFile, "%08d\t", src); + fprintf (pFile, "%.5lf\t", latitude); + fprintf (pFile, "%.5lf\t", longitude); + fprintf (pFile, "0\t " ); //zero for velocity + fprintf (pFile, "0\t " ); //zero for azimuth + fprintf (pFile, "\n"); + fclose (pFile); + + } + + } + + + + } + + fprintf (stderr, "%s", KNRM); +} + +//This Function has been redone and variables fixed to mirror SDRTrunk +//my earlier assumption that this was the same format as DMR Embedded GPS was incorrect +void apx_embedded_gps (dsd_opts * opts, dsd_state * state, uint8_t lc_bits[]) +{ + + fprintf (stderr, "%s", KYEL); + fprintf (stderr, " GPS:"); + uint8_t slot = state->currentslot; + uint8_t pf = lc_bits[0]; + uint8_t res_a = lc_bits[1]; + uint8_t res_b = (uint8_t)ConvertBitIntoBytes(&lc_bits[16], 7); + uint8_t expired = lc_bits[23]; //this bit seems to indicate that the GPS coordinates are out of date or fresh + + char deg_glyph[4]; + sprintf (deg_glyph, "%s", "°"); + + uint32_t lon_sign = lc_bits[48]; + uint32_t lon = (uint32_t)ConvertBitIntoBytes(&lc_bits[49], 23); + uint32_t lat_sign = lc_bits[24]; + uint32_t lat = (uint32_t)ConvertBitIntoBytes(&lc_bits[25], 23); + //todo: acquire more samples for additional validation + double lat_unit = 90.0f / 0x7FFFFF; + double lon_unit = 180.0f / 0x7FFFFF; + double lon_sf = 1.0f; //float value we can multiple longitude with + double lat_sf = 1.0f; //float value we can multiple latitude with + + char latstr[3]; + char lonstr[3]; + char valid[9]; + sprintf (latstr, "%s", "N"); + sprintf (lonstr, "%s", "E"); + sprintf (valid, "%s", "Current"); + + double latitude = 0; + double longitude = 0; + + if (pf) fprintf (stderr, " Protected"); + else + { + if (lat_sign) + { + sprintf (latstr, "%s", "S"); + lat_sf = -1.0f; + } + latitude = ((double)lat * lat_unit); + + if (lon_sign) + { + sprintf (lonstr, "%s", "W"); + lon_sf = -1.0f; + } + longitude = ((double)lon * lon_unit); + + //sanity check + if (abs (latitude) < 90 && abs(longitude) < 180) + { + fprintf (stderr, " Lat: %.5lf%s%s Lon: %.5lf%s%s (%.5lf, %.5lf) ", latitude, deg_glyph, latstr, longitude, deg_glyph, lonstr, latitude * lat_sf, longitude * lon_sf); + + if (expired) + { + fprintf (stderr, "Expired; "); + sprintf (valid, "%s", "Expired"); + } + else if (!expired) + fprintf (stderr, "Current; "); + + if (res_a) + fprintf (stderr, "RES_A: %d; ", res_a); + + if (res_b) + fprintf (stderr, "RES_B: %02X; ", res_b); + + //save to array for ncurses + sprintf (state->dmr_embedded_gps[slot], "GPS: %lf%s%s %lf%s%s %s", latitude, deg_glyph, latstr, longitude, deg_glyph, lonstr, valid); + + //save to LRRP report for mapping/logging + FILE * pFile; //file pointer + if (opts->lrrp_file_output == 1) + { + int src = 0; + if (slot == 0) src = state->lastsrc; + if (slot == 1) src = state->lastsrcR; + + //open file by name that is supplied in the ncurses terminal, or cli + pFile = fopen (opts->lrrp_out_file, "a"); + fprintf (pFile, "%s\t", getDateL() ); + fprintf (pFile, "%s\t", getTimeL() ); + fprintf (pFile, "%08d\t", src); + fprintf (pFile, "%.5lf\t", latitude); + fprintf (pFile, "%.5lf\t", longitude); + fprintf (pFile, "0\t " ); //zero for velocity + fprintf (pFile, "0\t " ); //zero for azimuth + fprintf (pFile, "\n"); + fclose (pFile); + + } + + } + + + + } + + fprintf (stderr, "%s", KNRM); +} \ No newline at end of file diff --git a/src/p25_lcw.c b/src/p25_lcw.c index 8b57899..cd93f5f 100644 --- a/src/p25_lcw.c +++ b/src/p25_lcw.c @@ -448,110 +448,4 @@ void p25_lcw (dsd_opts * opts, dsd_state * state, uint8_t LCW_bits[], uint8_t ir } -//This Function has been redone and variables fixed to mirror SDRTrunk -//my earlier assumption that this was the same format as DMR Embedded GPS was incorrect -void apx_embedded_gps (dsd_opts * opts, dsd_state * state, uint8_t lc_bits[]) -{ - - fprintf (stderr, "%s", KYEL); - fprintf (stderr, " GPS:"); - uint8_t slot = state->currentslot; - uint8_t pf = lc_bits[0]; - uint8_t res_a = lc_bits[1]; - uint8_t res_b = (uint8_t)ConvertBitIntoBytes(&lc_bits[16], 7); - uint8_t expired = lc_bits[23]; //this bit seems to indicate that the GPS coordinates are out of date or fresh - - char deg_glyph[4]; - sprintf (deg_glyph, "%s", "°"); - - uint32_t lon_sign = lc_bits[48]; - uint32_t lon = (uint32_t)ConvertBitIntoBytes(&lc_bits[49], 23); - uint32_t lat_sign = lc_bits[24]; - uint32_t lat = (uint32_t)ConvertBitIntoBytes(&lc_bits[25], 23); - //todo: acquire more samples for additional validation - double lat_unit = 90.0f / 0x7FFFFF; - double lon_unit = 180.0f / 0x7FFFFF; - double lon_sf = 1.0f; //float value we can multiple longitude with - double lat_sf = 1.0f; //float value we can multiple latitude with - - char latstr[3]; - char lonstr[3]; - char valid[9]; - sprintf (latstr, "%s", "N"); - sprintf (lonstr, "%s", "E"); - sprintf (valid, "%s", "Current"); - - double latitude = 0; - double longitude = 0; - - if (pf) fprintf (stderr, " Protected"); - else - { - if (lat_sign) - { - sprintf (latstr, "%s", "S"); - lat_sf = -1.0f; - } - latitude = ((double)lat * lat_unit); - - if (lon_sign) - { - sprintf (lonstr, "%s", "W"); - lon_sf = -1.0f; - } - longitude = ((double)lon * lon_unit); - - //sanity check - if (abs (latitude) < 90 && abs(longitude) < 180) - { - fprintf (stderr, " Lat: %.5lf%s%s Lon: %.5lf%s%s (%.5lf, %.5lf) ", latitude, deg_glyph, latstr, longitude, deg_glyph, lonstr, latitude * lat_sf, longitude * lon_sf); - - if (expired) - { - fprintf (stderr, "Expired; "); - sprintf (valid, "%s", "Expired"); - } - else if (!expired) - fprintf (stderr, "Current; "); - - if (res_a) - fprintf (stderr, "RES_A: %d; ", res_a); - - if (res_b) - fprintf (stderr, "RES_B: %02X; ", res_b); - - //save to array for ncurses - sprintf (state->dmr_embedded_gps[slot], "GPS: %lf%s%s %lf%s%s %s", latitude, deg_glyph, latstr, longitude, deg_glyph, lonstr, valid); - - //save to LRRP report for mapping/logging - FILE * pFile; //file pointer - if (opts->lrrp_file_output == 1) - { - int src = 0; - if (slot == 0) src = state->lastsrc; - if (slot == 1) src = state->lastsrcR; - - //open file by name that is supplied in the ncurses terminal, or cli - pFile = fopen (opts->lrrp_out_file, "a"); - fprintf (pFile, "%s\t", getDateL() ); - fprintf (pFile, "%s\t", getTimeL() ); - fprintf (pFile, "%08d\t", src); - fprintf (pFile, "%.5lf\t", latitude); - fprintf (pFile, "%.5lf\t", longitude); - fprintf (pFile, "0\t " ); //zero for velocity - fprintf (pFile, "0\t " ); //zero for azimuth - fprintf (pFile, "\n"); - fclose (pFile); - - } - - } - - - - } - - fprintf (stderr, "%s", KNRM); -} - diff --git a/src/p25p2_vpdu.c b/src/p25p2_vpdu.c index ae0053e..468b71a 100644 --- a/src/p25p2_vpdu.c +++ b/src/p25p2_vpdu.c @@ -34,251 +34,6 @@ static const uint8_t mac_msg_len[256] = { //TODO: Check for Non standard MFIDs first MAC[1], then set len on the MAC[2] if //the result from the len table is 0 (had to manually enter a few observed values from Harris) - -//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) -{ - - //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 - - 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 - - //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 - - //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 - - //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 - - //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 - - //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; - - //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; - - //speed conversion - 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); - - //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) ); - - //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 - - fprintf (stderr, "\n"); - - 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 (nmea_speed > 126) - fprintf (stderr, " SPD > 126 knots or %f MPH;", fmph); //using MPH here, its Harris, they're in the U.S. - else - fprintf (stderr, " SPD: %d knots; %f MPH;", nmea_speed, fmph); - - //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], "GPS: (%f%s, %f%s)", latitude, deg_glyph, longitude, deg_glyph); - - //save to LRRP report for mapping/logging - FILE * pFile; //file pointer - if (opts->lrrp_file_output == 1) - { - //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", getDateL() ); - fprintf (pFile, "%s\t", getTimeL() ); //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); - - } - - //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) - { - //open file by name that is supplied in the ncurses terminal, or cli - pFile = fopen (opts->lrrp_out_file, "a"); - fprintf (pFile, "%s\t", getDateL() ); - fprintf (pFile, "%s\t", getTimeL() ); - fprintf (pFile, "%08d\t", src); - fprintf (pFile, "%.6lf\t", lat_dec); - fprintf (pFile, "%.6lf\t", lon_dec); - fprintf (pFile, "%d\t ", s); - fprintf (pFile, "%d\t ", a); - fprintf (pFile, "\n"); - fclose (pFile); - - } - - //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 -} - void process_MAC_VPDU(dsd_opts * opts, dsd_state * state, int type, unsigned long long int MAC[24]) { //handle variable content MAC PDUs (Active, Idle, Hangtime, or Signal)