From 05eef039c949c98886654f8bb6ec6a20f6e63e66 Mon Sep 17 00:00:00 2001 From: lwvmobile Date: Mon, 20 Feb 2023 12:41:20 -0500 Subject: [PATCH] DMR - LRRP tweaks; Add LOCN PDU. --- src/dmr_pdu.c | 245 +++++++++++++++++++++++++++++++++++++------------- 1 file changed, 182 insertions(+), 63 deletions(-) diff --git a/src/dmr_pdu.c b/src/dmr_pdu.c index 091a653..de13b8f 100644 --- a/src/dmr_pdu.c +++ b/src/dmr_pdu.c @@ -44,21 +44,20 @@ void dmr_pdu (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t DMR uint8_t padding = state->data_header_padding[slot]; uint8_t lrrp_conf = 0; - //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); + //check for more available flag info, etc on these prior to running + if (DMR_PDU[0] == 0x01) dmr_locn (opts, state, block_len, DMR_PDU); + else 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 - state->data_header_format[slot] = 0; //zero out so we don't keep ascii printing } //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 (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t DMR_PDU[]) +void dmr_lrrp (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t DMR_PDU[]) { int i, j; uint16_t message_len = 0; @@ -83,12 +82,15 @@ uint8_t dmr_lrrp (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t //lat-long-radius-alt uint32_t lat = 0; - uint8_t lat_sign = 0; uint32_t lon = 0; - uint16_t rad = 0; + uint16_t rad = 0; //or Azimuth in Circle 3D? uint8_t alt = 0; double lat_unit = (double)180/(double)4294967295; double lon_unit = (double)360/(double)4294967295; + double lat_fin = 0.0; //calculated values + double lon_fin = 0.0; //calculated values + int lat_sign = 1; //positive 1, or negative 1 + int lon_sign = 1; //positive 1, or negative 1 //speed/velocity uint16_t vel = 0; @@ -97,28 +99,19 @@ uint8_t dmr_lrrp (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t //track, direction, degrees uint8_t degrees = 0; - uint8_t deg_set = 0; + uint8_t deg_set = 0; + + char deg_glyph[4]; + sprintf (deg_glyph, "%s", "°"); + // sprintf (deg_glyph, "%s", "d"); //triggered information report uint8_t report = 0; uint8_t pot_report = 0; //potential report by finding 0x0D and backtracking a few bytes - uint8_t is_p_head = 0; - //shim for getting LRRP out of some prop head data blocks - if (state->data_p_head[slot] == 1) - { - if (DMR_PDU[1] != 0x10) goto LRRP_END; //check to see if FID is 0x10 - i = 0; - message_len = blocks * block_len; - source = state->dmr_lrrp_source[state->currentslot]; - is_p_head = 1; - } - else i = 12; - - //start looking for tokens (using my best understanding of the ok-dmr library xml and python files) - for (i; i < ( (blocks*block_len) - (padding+4) ); i++) + for (i = 0; i < ( (blocks*block_len) - (padding+4) ); i++) { switch(DMR_PDU[i]){ @@ -175,20 +168,22 @@ uint8_t dmr_lrrp (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t } break; //RESULT TOKEN or Request ID! + case 0x22: //same comment as below, observed followed the message len case 0x23: //this value has been seen after the 0x0D message len, appears to be a 2-byte value, was tripping a false on time stamp i += 2; break; //answer and report tokens case 0x51: //circle-2d case 0x54: //circle-3d - case 0x55: //circle-3d <- false positives on one sample, so either an earlier token occurred (that we don't have) or just random data? + case 0x55: //circle-3d if (message_len > 0 && lat == 0) { - if (DMR_PDU[i+1] & 0x80) lat_sign = 1; - lat = ( ( ((DMR_PDU[i+1] & 0x7F ) << 24 ) + (DMR_PDU[i+2] << 16) + (DMR_PDU[i+3] << 8) + DMR_PDU[i+4]) * 1 ); + // lat = ( ( ((DMR_PDU[i+1] & 0x7F ) << 24 ) + (DMR_PDU[i+2] << 16) + (DMR_PDU[i+3] << 8) + DMR_PDU[i+4]) * 1 ); + // lon = ( ( ((DMR_PDU[i+5] & 0x7F ) << 24 ) + (DMR_PDU[i+6] << 16) + (DMR_PDU[i+7] << 8) + DMR_PDU[i+8]) * 1 ); + lat = ( ( (DMR_PDU[i+1] << 24 ) + (DMR_PDU[i+2] << 16) + (DMR_PDU[i+3] << 8) + DMR_PDU[i+4]) * 1 ); lon = ( ( (DMR_PDU[i+5] << 24 ) + (DMR_PDU[i+6] << 16) + (DMR_PDU[i+7] << 8) + DMR_PDU[i+8]) * 1 ); rad = (DMR_PDU[i+9] << 8) + DMR_PDU[i+10]; - i += 10; //double check + i += 10; if (lat > 0 && lon > 0) lrrp_confidence++; //would be better to set by absolute boundary (180 and 90?) else lat = 0; } @@ -211,13 +206,14 @@ uint8_t dmr_lrrp (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t } break; - case 0x66: //point-2d + case 0x66: //point-2d if (message_len > 0 && lat == 0) { - if (DMR_PDU[i+1] & 0x80) lat_sign = 1; - lat = ( ( ((DMR_PDU[i+1] & 0x7F ) << 24 ) + (DMR_PDU[i+2] << 16) + (DMR_PDU[i+3] << 8) + DMR_PDU[i+4]) * 1 ); + // lat = ( ( ((DMR_PDU[i+1] & 0x7F ) << 24 ) + (DMR_PDU[i+2] << 16) + (DMR_PDU[i+3] << 8) + DMR_PDU[i+4]) * 1 ); + // lon = ( ( ((DMR_PDU[i+5] & 0x7F ) << 24 ) + (DMR_PDU[i+6] << 16) + (DMR_PDU[i+7] << 8) + DMR_PDU[i+8]) * 1 ); + lat = ( ( (DMR_PDU[i+1] << 24 ) + (DMR_PDU[i+2] << 16) + (DMR_PDU[i+3] << 8) + DMR_PDU[i+4]) * 1 ); lon = ( ( (DMR_PDU[i+5] << 24 ) + (DMR_PDU[i+6] << 16) + (DMR_PDU[i+7] << 8) + DMR_PDU[i+8]) * 1 ); - i += 8; //check + i += 8; lrrp_confidence++; } break; @@ -225,12 +221,13 @@ uint8_t dmr_lrrp (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t case 0x6A: //point-3d if (message_len > 0 && lat == 0) { - if (DMR_PDU[i+1] & 0x80) lat_sign = 1; - lat = ( ( ((DMR_PDU[i+1] & 0x7F ) << 24 ) + (DMR_PDU[i+2] << 16) + (DMR_PDU[i+3] << 8) + DMR_PDU[i+4]) * 1 ); + // lat = ( ( ((DMR_PDU[i+1] & 0x7F ) << 24 ) + (DMR_PDU[i+2] << 16) + (DMR_PDU[i+3] << 8) + DMR_PDU[i+4]) * 1 ); + // lon = ( ( ((DMR_PDU[i+5] & 0x7F ) << 24 ) + (DMR_PDU[i+6] << 16) + (DMR_PDU[i+7] << 8) + DMR_PDU[i+8]) * 1 ); + lat = ( ( (DMR_PDU[i+1] << 24 ) + (DMR_PDU[i+2] << 16) + (DMR_PDU[i+3] << 8) + DMR_PDU[i+4]) * 1 ); lon = ( ( (DMR_PDU[i+5] << 24 ) + (DMR_PDU[i+6] << 16) + (DMR_PDU[i+7] << 8) + DMR_PDU[i+8]) * 1 ); alt = DMR_PDU[i+9]; - i += 9; //check - if (lat > 0 && lon > 0) lrrp_confidence++; //would be better to set by absolute boundary (180 and 90?) + i += 9; + if (lat > 0 && lon > 0) lrrp_confidence++; else lat = 0; } break; @@ -274,7 +271,7 @@ uint8_t dmr_lrrp (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t { fprintf (stderr, "%s", KYEL); fprintf (stderr, "\n LRRP Confidence: %d - Message Len: %d Octets", lrrp_confidence, message_len); - if (lrrp_confidence >= 4) //find the sweet magical number + if (lrrp_confidence >= 3) //find the sweet magical number { //now we can open our lrrp file and write to it as well FILE * pFile; //file pointer @@ -286,9 +283,10 @@ uint8_t dmr_lrrp (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t if (!year) fprintf (pFile, "%s\t", getDateL() ); //current date, only add this IF no included timestamp in LRRP data? if (!year) fprintf (pFile, "%s\t", getTimeL() ); //current timestamp, only add this IF no included timestamp in LRRP data? if (year) fprintf (pFile, "%04d/%02d/%02d\t%02d:%02d:%02d\t", year, month, day, hour, minute, second); //add timestamp from decoded audio if available - //write data header source (or destination?) if not available in lrrp data + //write data header source if not available in lrrp data if (!source) fprintf (pFile, "%08lld\t", state->dmr_lrrp_source[state->currentslot]); //source address from data header if (source) fprintf (pFile, "%08d\t", source); //add source form decoded audio if available, else its from the header + } if (pot_report) @@ -328,31 +326,45 @@ uint8_t dmr_lrrp (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t if (lat) { fprintf (stderr, "\n"); - if (lat_sign) //print a '-' in front of the value - fprintf (stderr, " LRRP - Lat: -%.5lf", (double)lat * lat_unit); - else fprintf (stderr, " LRRP - Lat: %.5lf", (double)lat * lat_unit); - fprintf (stderr, " Lon: %.5lf", (double)lon * lon_unit); - if (lat_sign) - fprintf (stderr, " (-%.5lf, %.5lf)", (double)lat * lat_unit , (double)lon * lon_unit); - else fprintf (stderr, " (%.5lf, %.5lf)", (double)lat * lat_unit , (double)lon * lon_unit); + //need to check these new calcs for accuracy accross the globe, both lat and lon + + //two's compliment-ish testing on these bytes + if (lat & 0x80000000) //8 + { + lat = lat & 0x7FFFFFFF; + lat_sign = -1; + // lat = 0x80000000 - lat; //not sure why this doesn't work here like it does on lon, extra bit? + } + if (lon & 0x80000000) + { + lon = lon & 0x7FFFFFFF; + lon_sign = -1; + lon = 0x80000000 - lon; + } + + lat_fin = (double)lat * lat_unit * lat_sign; + lon_fin = (double)lon * lon_unit * lon_sign; + + fprintf (stderr, " LRRP - Lat: %.5lf", lat_fin); + fprintf (stderr, " Lon: %.5lf", lon_fin); + fprintf (stderr, " (%.5lf, %.5lf)", lat_fin , lon_fin); if (opts->lrrp_file_output == 1) { - if (lat_sign) fprintf (pFile, "-"); - fprintf (pFile, "%.5lf\t", (double)lat * lat_unit); - fprintf (pFile, "%.5lf\t", (double)lon * lon_unit); + fprintf (pFile, "%.5lf\t", lat_fin); + fprintf (pFile, "%.5lf\t", lon_fin); } } if (rad) { fprintf (stderr, "\n"); - fprintf (stderr, " LRRP - Radius: %d", rad); //unsure of 'units' or calculation for radius (meters?) + fprintf (stderr, " LRRP - Radius: %dm", rad); //unsure of 'units' or calculation for radius (meters?) } if (alt) { fprintf (stderr, "\n"); - fprintf (stderr, " LRRP - Altitude: %d", alt); //unsure of 'units' or calculation for alt (meters?) + fprintf (stderr, " LRRP - Altitude: %dm", alt); //unsure of 'units' or calculation for alt (meters?) } if (vel_set) { @@ -363,7 +375,7 @@ uint8_t dmr_lrrp (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t if (deg_set) { fprintf (stderr, "\n"); - fprintf (stderr, " LRRP - Track: %d Degrees", degrees); + fprintf (stderr, " LRRP - Track: %d%s", degrees, deg_glyph); if (opts->lrrp_file_output == 1) fprintf (pFile, "%d\t",degrees); } @@ -374,20 +386,17 @@ uint8_t dmr_lrrp (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t fclose (pFile); } - //save to array for ncurses + //save to string for ncurses if (!source) source = state->dmr_lrrp_source[state->currentslot]; - char sign[8]; char velstr[20]; char degstr[20]; char lrrpstr[100]; sprintf (lrrpstr, "%s", ""); sprintf (velstr, "%s", ""); sprintf (degstr, "%s", ""); - if (lat_sign) sprintf (sign, "%s", "-"); - else sprintf (sign, "%s", ""); - if (lat) sprintf (lrrpstr, "LRRP %0d (%s%lf, %lf)", source, sign, (double)lat * lat_unit, (double)lon * lon_unit); + if (lat) sprintf (lrrpstr, "LRRP %0d (%lf, %lf)", source, lat_fin, lon_fin); if (vel_set) sprintf (velstr, " %.4lf km/h", velocity * 3.6); - if (deg_set) sprintf (degstr, " %d deg", degrees); + if (deg_set) sprintf (degstr, " %d%s ", degrees, deg_glyph); sprintf (state->dmr_lrrp_gps[slot], "%s%s%s", lrrpstr, velstr, degstr); } @@ -400,16 +409,126 @@ uint8_t dmr_lrrp (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t 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); - // } - LRRP_END: fprintf (stderr, "%s", KNRM); - - return (lrrp_confidence); +} + +void dmr_locn (dsd_opts * opts, dsd_state * state, uint8_t block_len, uint8_t DMR_PDU[]) +{ + int i, j; + uint8_t slot = state->currentslot; + uint8_t blocks = state->data_header_blocks[slot]; + uint8_t padding = state->data_header_padding[slot]; + uint8_t source = state->dmr_lrrp_source[slot]; + + //flags if certain data type is present + uint8_t time = 0; + uint8_t lat = 0; + uint8_t lon = 0; + + //date-time variables + uint8_t hour = 0; + uint8_t minute = 0; + uint8_t second = 0; + uint8_t year = 0; + uint8_t month = 0; + uint8_t day = 0; + + //need to experiment best way to do this portion + uint8_t lat_deg = 0; + uint8_t lat_min = 0; + uint16_t lat_sec = 0; + + uint8_t lon_deg = 0; + uint8_t lon_min = 0; + uint16_t lon_sec = 0; + + char lat_ord[2]; + char lon_ord[2]; + sprintf (lat_ord, "%s", "N"); + sprintf (lon_ord, "%s", "E"); + + char deg_glyph[4]; + sprintf (deg_glyph, "%s", "°"); + // sprintf (deg_glyph, "%s", "d"); + + //more strings... + char locnstr[50]; + char latstr[75]; + char lonstr[75]; + sprintf (locnstr, "%s", " "); + sprintf (latstr, "%s", " "); + sprintf (lonstr, "%s", " "); + + //TODO: conversion to decimal format + //DD = d + (min/60) + (sec/3600) + //N is positive, E is positive?? Assuming its like a 2D plane + int lat_sign = 1; //positive 1 or negative 1 + int lon_sign = 1; //positive 1 or negative 1 + double lat_fin = 0; + double lon_fin = 0; + + //start looking for specific bytes corresponding to 'letters' A (time), NSEW (ordinal directions), etc + for (i = 0; i < ( (blocks*block_len) - (padding+4) ); i++) + { + switch(DMR_PDU[i]){ + case 0x41: //A -- time and date + time = 1; + hour = ((DMR_PDU[i+1] - 0x30) << 4) | (DMR_PDU[i+2] - 0x30); + minute = ((DMR_PDU[i+3] - 0x30) << 4) | (DMR_PDU[i+4] - 0x30); + second = ((DMR_PDU[i+5] - 0x30) << 4) | (DMR_PDU[i+6] - 0x30); + //think this is in day, mon, year format + day = ((DMR_PDU[i+7] - 0x30) << 4) | (DMR_PDU[i+8] - 0x30); + month = ((DMR_PDU[i+9] - 0x30) << 4) | (DMR_PDU[i+10] - 0x30); + year = ((DMR_PDU[i+11] - 0x30) << 4) | (DMR_PDU[i+12] - 0x30); + i += 12; + break; + + case 0x53: //S -- South + sprintf (lat_ord, "%s", "S"); + lat_sign = -1; + case 0x4E: //N -- North + lat = 1; + lat_deg = ((DMR_PDU[i+1] - 0x30) << 4) | (DMR_PDU[i+2] - 0x30); + lat_min = ((DMR_PDU[i+3] - 0x30) << 4) | (DMR_PDU[i+4] - 0x30); + lat_sec = ((DMR_PDU[i+6] - 0x30) << 12) | ((DMR_PDU[i+7] - 0x30) << 8) | ((DMR_PDU[i+8] - 0x30) << 4) | ((DMR_PDU[i+9] - 0x30) << 0); + i += 8; + break; + + case 0x57: //W -- West + sprintf (lon_ord, "%s", "W"); + lon_sign = -1; + case 0x45: //E -- East + lon = 1; + lon_deg = ((DMR_PDU[i+1] - 0x30) << 8) | ((DMR_PDU[i+2] - 0x30) << 4) | ((DMR_PDU[i+3] - 0x30) << 0); + lon_min = ((DMR_PDU[i+4] - 0x30) << 4) | (DMR_PDU[i+5] - 0x30); + lon_sec = ((DMR_PDU[i+7] - 0x30) << 12) | ((DMR_PDU[i+8] - 0x30) << 8) | ((DMR_PDU[i+9] - 0x30) << 4) | ((DMR_PDU[i+10] - 0x30) << 0); + i += 8; + break; + + default: + //do nothing + break; + + } //end switch + + } //for i + + if (lat && lon) + { + fprintf (stderr, "%s", KYEL); + fprintf (stderr, "\n LOCN Report - Source: [%d]\n", source); + if (time) fprintf (stderr, " 20%02X/%02X/%02X %02X:%02X:%02X ", year, month, day, hour, minute, second); + fprintf (stderr, "Lat: %s %02X%s%02X\"%04X' Lon: %s %02X%s%02X\"%04X' ", lat_ord, lat_deg, deg_glyph, lat_min, lat_sec, lon_ord, lon_deg, deg_glyph, lon_min, lon_sec); + + //string manip for ncurses terminal display + sprintf (locnstr, "LOCN %d ", source); + sprintf (latstr, "%s %02X%s%02X\"%04X' ", lat_ord, lat_deg, deg_glyph, lat_min, lat_sec); + sprintf (lonstr, "%s %02X%s%02X\"%04X' ", lon_ord, lon_deg, deg_glyph, lon_min, lon_sec); + sprintf (state->dmr_lrrp_gps[slot], "%s%s%s", locnstr, latstr, lonstr); + + } + } \ No newline at end of file