From a9417afd674e30cee2dd0e499a1b8de224be4ad2 Mon Sep 17 00:00:00 2001 From: lwvmobile Date: Wed, 7 Dec 2022 04:26:11 -0500 Subject: [PATCH] DMR EGPS, LRRP, Data Block Tweaks; --- include/dsd.h | 6 ++--- src/dmr_block.c | 19 +++++++++++--- src/dmr_bs.c | 46 ++++++++++++++++++-------------- src/dmr_data.c | 1 + src/dmr_flco.c | 70 ++++++++++++++++++++++++++++++++++++------------- src/dmr_pdu.c | 49 +++++++++++++++++++++++----------- 6 files changed, 133 insertions(+), 58 deletions(-) diff --git a/include/dsd.h b/include/dsd.h index a59f796..73a6035 100644 --- a/include/dsd.h +++ b/include/dsd.h @@ -873,9 +873,9 @@ void dmrMSBootstrap (dsd_opts * opts, dsd_state * state); //dmr data header and multi block types (header, 1/2, 3/4, 1, Unified) void dmr_dheader (dsd_opts * opts, dsd_state * state, uint8_t dheader[], uint32_t CRCCorrect, uint32_t IrrecoverableErrors); void dmr_block_assembler (dsd_opts * opts, dsd_state * state, uint8_t block_bytes[], uint8_t block_len, uint8_t databurst, uint8_t type); -void dmr_pdu (dsd_opts * opts, dsd_state * state, uint8_t DMR_PDU[]); -void dmr_lrrp (dsd_opts * opts, dsd_state * state, uint8_t DMR_PDU[]); -uint8_t dmr_lrrp_check (dsd_opts * opts, dsd_state * state, uint8_t DMR_PDU[]); +void dmr_pdu (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t DMR_PDU[]); +void dmr_reset_blocks (dsd_opts * opts, dsd_state * state); +uint8_t dmr_lrrp (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t DMR_PDU[]); //dmr alg stuff void dmr_alg_reset (dsd_opts * opts, dsd_state * state); diff --git a/src/dmr_block.c b/src/dmr_block.c index 68d0cd0..02f91b1 100644 --- a/src/dmr_block.c +++ b/src/dmr_block.c @@ -190,7 +190,7 @@ void dmr_dheader (dsd_opts * opts, dsd_state * state, uint8_t dheader[], uint32_ fprintf (stderr, "\n Proprietary Data Header: SAP %01X - Format %01X - MFID %02X", (dheader[0] & 0xF0) >> 4, dheader[0] & 0xF, dheader[1] ); - state->data_p_head[slot] = 0; + //state->data_p_head[slot] = 0; //disabling this, we can flag it off after processing pdu instead state->data_header_sap[slot] = 0; //reset this to zero so we don't trip the condition below state->data_block_counter[slot]++; //increment the counter since this counts against the data blocks @@ -255,7 +255,7 @@ void dmr_block_assembler (dsd_opts * opts, dsd_state * state, uint8_t block_byte if (block_len == 0) block_len = 18; if (block_len > 24) block_len = 24; - //TODO: add CRC/FEC validation to each block and each confirmed data block as well - type 1 + //TODO: add CRC/FEC validation to each data header and each confirmed data block as well - type 1 if (type == 1) { //type 1 data block, shuffle method @@ -275,7 +275,7 @@ void dmr_block_assembler (dsd_opts * opts, dsd_state * state, uint8_t block_byte if (state->data_block_counter[slot] == state->data_header_blocks[slot]) { //TODO: CRC32 on completed messages - dmr_pdu (opts, state, state->dmr_pdu_sf[slot]); + dmr_pdu (opts, state, block_len, state->dmr_pdu_sf[slot]); } } @@ -370,4 +370,17 @@ void dmr_block_assembler (dsd_opts * opts, dsd_state * state, uint8_t block_byte } +} + +//failsafe to clear old data header and block info in case of tact/emb/slottype failures and we +//can no longer verify accurate data block reporting +void dmr_reset_blocks (dsd_opts * opts, dsd_state * state) +{ + memset (state->data_p_head, 0, sizeof(state->data_p_head)); + memset (state->data_conf_data, 0, sizeof(state->data_conf_data)); + memset (state->dmr_pdu_sf, 0, sizeof(state->dmr_pdu_sf)); + memset (state->data_block_counter, 0, sizeof(state->data_block_counter)); + memset (state->data_header_blocks, 0, sizeof(state->data_header_blocks)); + memset (state->data_block_crc_valid, 0, sizeof(state->data_block_crc_valid)); + memset (state->dmr_lrrp_source, 0, sizeof(state->dmr_lrrp_source)); } \ No newline at end of file diff --git a/src/dmr_bs.c b/src/dmr_bs.c index 9cdaaa1..2079f24 100644 --- a/src/dmr_bs.c +++ b/src/dmr_bs.c @@ -399,25 +399,29 @@ void dmrBS (dsd_opts * opts, dsd_state * state) //if we have a tact or emb err, then produce sync pattern/err message if (tact_okay != 1 || emb_ok != 1) { - fprintf (stderr,"%s ", getTime()); - fprintf (stderr,"Sync: DMR "); - fprintf (stderr, "%s", KRED); - fprintf (stderr, "| VOICE CACH/EMB ERR"); - fprintf (stderr, "%s", KNRM); - fprintf (stderr, "\n"); - //run LFSR if either slot had an active MI in it. - if (state->payload_algid >= 0x21) - { - state->currentslot = 0; - LFSR(state); - fprintf (stderr, "\n"); - } - if (state->payload_algidR >= 0x21) - { - state->currentslot = 1; - LFSR(state); - fprintf (stderr, "\n"); - } + + fprintf (stderr,"%s ", getTime()); + fprintf (stderr,"Sync: DMR "); + fprintf (stderr, "%s", KRED); + fprintf (stderr, "| VOICE CACH/EMB ERR"); + fprintf (stderr, "%s", KNRM); + fprintf (stderr, "\n"); + //run LFSR if either slot had an active MI in it. + if (state->payload_algid >= 0x21) + { + state->currentslot = 0; + LFSR(state); + fprintf (stderr, "\n"); + } + if (state->payload_algidR >= 0x21) + { + state->currentslot = 1; + LFSR(state); + fprintf (stderr, "\n"); + } + + //failsafe to reset all data header and blocks when bad tact or emb + dmr_reset_blocks (opts, state); } @@ -657,6 +661,10 @@ void dmrBSBootstrap (dsd_opts * opts, dsd_state * state) LFSR(state); fprintf (stderr, "\n"); } + + //failsafe to reset all data header and blocks when bad tact or emb + dmr_reset_blocks (opts, state); } + } diff --git a/src/dmr_data.c b/src/dmr_data.c index f3e6ca1..ab24dd2 100644 --- a/src/dmr_data.c +++ b/src/dmr_data.c @@ -306,6 +306,7 @@ dmr_data_sync (dsd_opts * opts, dsd_state * state) fprintf (stderr, "| CACH/Burst FEC ERR"); fprintf (stderr, "%s", KNRM); fprintf (stderr, "\n"); + dmr_reset_blocks (opts, state); //failsafe to reset all data header and blocks when bad tact or slottype } // Skip cach (24 bit = 12 dibit) and next slot 1st half (98 + 10 bit = 49 + 5 dibit) diff --git a/src/dmr_flco.c b/src/dmr_flco.c index e1a7b20..ba14a23 100644 --- a/src/dmr_flco.c +++ b/src/dmr_flco.c @@ -47,9 +47,10 @@ void dmr_flco (dsd_opts * opts, dsd_state * state, uint8_t lc_bits[], uint32_t C source = (uint32_t)ConvertBitIntoBytes(&lc_bits[56], 16); } - //Embedded Talker Alias Header Only (format and len storage) + if (IrrecoverableErrors == 0) { + //Embedded Talker Alias Header Only (format and len storage) if (type == 3 && flco == 0x04) { dmr_embedded_alias_header(opts, state, lc_bits); @@ -63,10 +64,10 @@ void dmr_flco (dsd_opts * opts, dsd_state * state, uint8_t lc_bits[], uint32_t C } //Embedded GPS - if (type == 3 && flco == 0x08) + if (type == 3 && flco == 0x08) { is_gps = 1; - dmr_embedded_gps(opts, state, lc_bits); + if (fid == 0) dmr_embedded_gps(opts, state, lc_bits); //issues with fid 0x10 and flco 0x08 } } @@ -596,7 +597,7 @@ void dmr_embedded_alias_blocks (dsd_opts * opts, dsd_state * state, uint8_t lc_b void dmr_embedded_gps (dsd_opts * opts, dsd_state * state, uint8_t lc_bits[]) { fprintf (stderr, "%s", KYEL); - fprintf (stderr, " Embedded GPS"); + fprintf (stderr, " Embedded GPS:"); uint8_t slot = state->currentslot; uint8_t pf = lc_bits[0]; uint8_t res_a = lc_bits[1]; @@ -605,25 +606,58 @@ void dmr_embedded_gps (dsd_opts * opts, dsd_state * state, uint8_t lc_bits[]) uint32_t lon = (uint32_t)ConvertBitIntoBytes(&lc_bits[23], 25); uint32_t lat = (uint32_t)ConvertBitIntoBytes(&lc_bits[48], 24); - double lat_unit = (double)180/(double)16777216; //180 divided by 2^24 - double lon_unit = (double)360/(double)33554432; //360 divided by 2^25 + 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 - //run calculations and print - //7.2.16 and 7.2.17 - double latitude = (double)-90 + ((double)lat * lat_unit); - double longitude = (double)-180 + ((double)lon * lon_unit); + uint32_t lat_sign = lat >> 23; + uint32_t lon_sign = lon >> 24; + char latstr[3]; + char lonstr[3]; + sprintf (latstr, "%s", "N"); + sprintf (lonstr, "%s", "E"); - fprintf (stderr, " Lat: %.5lf Lon: %.5lf", latitude, longitude); + //run calculations and print (still cannot verify as accurate) + //7.2.16 and 7.2.17 (two's compliment) - //7.2.15 Position Error - uint16_t position_error = pow(2, pos_err); //2^pos_err = 2 meters to the pos_err power - if (pos_err == 0x7 ) fprintf (stderr, " Position Error: Not Known or Location Invalid"); - else fprintf (stderr, " Position Error: Less than %d meters", position_error); + double latitude = 0; + double longitude = 0; - //save to array for ncurses - if (pos_err != 0x7) + if (pf) fprintf (stderr, " Protected"); + else { - sprintf (state->dmr_embedded_gps[slot], "E-GPS (%lf %lf) Error: %dm", latitude, longitude, position_error); + if (lat_sign) + { + lat = 0x800001 - (lat & 0x7FFFFF); + sprintf (latstr, "%s", "S"); + } + latitude = ((double)lat * lat_unit); + + if (lon_sign) + { + lon = 0x1000001 - (lon & 0xFFFFFF); + sprintf (lonstr, "%s", "W"); + } + longitude = ((double)lon * lon_unit); + + //sanity check + if (abs (latitude) < 90 && abs(longitude) < 180) + { + fprintf (stderr, " Lat: %.5lf %s Lon: %.5lf %s", latitude, latstr, longitude, lonstr); + + //7.2.15 Position Error + uint16_t position_error = 2 * pow(10, pos_err); //2 * 10^pos_err + if (pos_err == 0x7 ) fprintf (stderr, " Position Error: Unknown or Invalid"); + else fprintf (stderr, " Position Error: Less than %dm", position_error); + + //save to array for ncurses + if (pos_err != 0x7) + { + sprintf (state->dmr_embedded_gps[slot], "E-GPS (%lf %s %lf %s) Err: %dm", latitude, latstr, longitude, lonstr, position_error); + } + + } + + } fprintf (stderr, "%s", KNRM); diff --git a/src/dmr_pdu.c b/src/dmr_pdu.c index 4f97a29..64b8c0d 100644 --- a/src/dmr_pdu.c +++ b/src/dmr_pdu.c @@ -6,7 +6,6 @@ * 2022-12 DSD-FME Florida Man Edition *-----------------------------------------------------------------------------*/ -//unified DMR multiblock PDU message handler (LRRP, etc) #include "dsd.h" char * getTimeL(void) //get pretty hh:mm:ss timestamp @@ -36,31 +35,34 @@ char * getDateL(void) { return curr2; } -void dmr_pdu (dsd_opts * opts, dsd_state * state, uint8_t DMR_PDU[]) +void dmr_pdu (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t DMR_PDU[]) { - int message_legnth = 0; + uint8_t message_len = 0; uint8_t slot = state->currentslot; - uint8_t blocks = state->data_header_blocks[slot] - 1; //subtract 1 for the relevant value in the calc below + uint8_t blocks = state->data_header_blocks[slot]; uint8_t padding = state->data_header_padding[slot]; uint8_t lrrp_conf = 0; - lrrp_conf = dmr_lrrp_check (opts, state, DMR_PDU); + //consider disabling on prop_head data blocks if bad/false decodes + //if (state->data_p_head[slot] == 0) + lrrp_conf = dmr_lrrp (opts, state, block_len, DMR_PDU); //maybe one day we will have more things to do here state->data_conf_data[slot] = 0; //flag off confirmed data after processing it + state->data_p_head[slot] = 0; //flag off prop_head data after processing it } //The contents of this function are mostly my reversed engineered efforts by observing DSDPlus output and matching data bytes //combined with a few external sources such as OK-DMR for some token values and extra data values (rad and alt) //this is by no means an extensive LRRP list and is prone to error (unless somebody has the manual or something) -uint8_t dmr_lrrp_check (dsd_opts * opts, dsd_state * state, uint8_t DMR_PDU[]) +uint8_t dmr_lrrp (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t DMR_PDU[]) { int i, j; - uint8_t message_len = 0; + uint16_t message_len = 0; uint8_t slot = state->currentslot; - uint8_t blocks = state->data_header_blocks[slot] - 1; + uint8_t blocks = state->data_header_blocks[slot]; uint8_t padding = state->data_header_padding[slot]; uint8_t lrrp_confidence = 0; //variable to increment based on number of tokens found, the more, the higher the confidence level @@ -98,11 +100,19 @@ uint8_t dmr_lrrp_check (dsd_opts * opts, dsd_state * state, uint8_t DMR_PDU[]) //triggered information report uint8_t report = 0; - uint8_t pot_report = 0; //potential report by findind 0x0D and backtracking a few bytes + uint8_t pot_report = 0; //potential report by finding 0x0D and backtracking a few bytes - i = 0; //watch DMR_PDU[10] closely for a 'report' token + //shim for getting LRRP out of some prop head data blocks + if (state->data_p_head[slot] == 1) + { + i = 0; + message_len = blocks * block_len; + } + else i = 12; + + //start looking for tokens (using my best understanding of the ok-dmr library xml and python files) - for (i; i < ( ((blocks+1)*16) - (padding+4) ); i++) //need to change the 16, or entire thing? or who cares lol just keep looking + for (i; i < ( (blocks*block_len) - (padding+4) ); i++) { switch(DMR_PDU[i]){ @@ -122,6 +132,7 @@ uint8_t dmr_lrrp_check (dsd_opts * opts, dsd_state * state, uint8_t DMR_PDU[]) //case 0x1D: //ARRP_TriggeredInformationAnswer_NCDT //case 0x1E: //ARRP_TriggeredInformationReport = FALSE? //case 0x25: //ARRP_UnsolicitedInformationReport_NCDT = (0x25, True, "") + case 0x26: //ARRP_UnsolicitedInformationReport_NCDT case 0x13: //LRRP_UnsolicitedLocationReport_NCDT case 0x15: //LRRP_LocationProtocolReport_NCDT case 0x21: //ARRP_TriggeredInformationStopRequest_NCDT @@ -188,7 +199,7 @@ uint8_t dmr_lrrp_check (dsd_opts * opts, dsd_state * state, uint8_t DMR_PDU[]) second = (DMR_PDU[i+5] & 0x3F); i += 5; //sanity check - if (year > 2000 && year < 2025) lrrp_confidence++; + if (year > 2000 && year <= 2025) lrrp_confidence++; if (year > 2025 || year < 2000) year = 0; //needs future proofing } break; @@ -254,7 +265,7 @@ uint8_t dmr_lrrp_check (dsd_opts * opts, dsd_state * state, uint8_t DMR_PDU[]) if (report && message_len > 0) { fprintf (stderr, "%s", KYEL); - fprintf (stderr, "\n LRRP Confidence: %d - Message Len: %d", lrrp_confidence, message_len); + fprintf (stderr, "\n LRRP Confidence: %d - Message Len: %d Octets", lrrp_confidence, message_len); if (lrrp_confidence >= 3) //find the sweet magical number { //now we can open our lrrp file and write to it as well @@ -281,13 +292,14 @@ uint8_t dmr_lrrp_check (dsd_opts * opts, dsd_state * state, uint8_t DMR_PDU[]) { fprintf (stderr, "\n"); fprintf (stderr, " Report: 0x%02X", report); - if (report == 0x1F) fprintf (stderr, " ARRP_TriggeredInformationReport_NCDT "); //customize later when more is learned + if (report == 0x1F) fprintf (stderr, " ARRP_TriggeredInformationReport_NCDT "); //customize later when more is learned if (report == 0x21) fprintf (stderr, " ARRP_TriggeredInformationStopRequest_NCDT "); if (report == 0x22) fprintf (stderr, " ARRP_TriggeredInformationStopAnswer "); if (report == 0x25) fprintf (stderr, " ARRP_UnsolicitedInformationReport_NCDT "); if (report == 0x26) fprintf (stderr, " ARRP_UnsolicitedInformationReport_NCDT "); if (report == 0x27) fprintf (stderr, " ARRP_InformationProtocolRequest_NCDT "); if (report == 0x13) fprintf (stderr, " LRRP_UnsolicitedLocationReport_NCDT "); + if (report == 0x15) fprintf (stderr, " LRRP_UnsolicitedLocationReport_NCDT "); } if (source) @@ -368,11 +380,18 @@ uint8_t dmr_lrrp_check (dsd_opts * opts, dsd_state * state, uint8_t DMR_PDU[]) } - if (pot_report) + else if (pot_report) { fprintf (stderr, "\n"); fprintf (stderr, " Potential ARRP/LRRP Report (Debug): 0x%02X", report); } + + // else //debug only, disabled + // { + // fprintf (stderr, "\n"); + // fprintf (stderr, "\n LRRP Confidence: %d - Message Len: %d Octets", lrrp_confidence, message_len); + // } + fprintf (stderr, "%s", KNRM);