From 5818d58dba92b2336725d82073ce1713754147e7 Mon Sep 17 00:00:00 2001 From: Max Date: Tue, 4 Jan 2022 15:29:01 -0500 Subject: [PATCH] p25p2 pdu updates, new 'sync' plot mode --- op25/gr-op25_repeater/apps/gr_gnuplot.py | 131 ++++++++++- op25/gr-op25_repeater/apps/multi_rx.py | 9 + op25/gr-op25_repeater/apps/rx.py | 6 + op25/gr-op25_repeater/apps/trunking.py | 84 ++++++- .../include/op25_repeater/gardner_costas_cc.h | 1 + .../lib/gardner_costas_cc_impl.cc | 95 +++++++- .../lib/gardner_costas_cc_impl.h | 10 + op25/gr-op25_repeater/lib/p25p2_tdma.cc | 219 +++--------------- op25/gr-op25_repeater/lib/p25p2_tdma.h | 1 - 9 files changed, 345 insertions(+), 211 deletions(-) diff --git a/op25/gr-op25_repeater/apps/gr_gnuplot.py b/op25/gr-op25_repeater/apps/gr_gnuplot.py index c3fcb9f..2d349f4 100644 --- a/op25/gr-op25_repeater/apps/gr_gnuplot.py +++ b/op25/gr-op25_repeater/apps/gr_gnuplot.py @@ -24,13 +24,15 @@ import os import time import subprocess import json +import threading +import glob from gnuradio import gr, gru, eng_notation from gnuradio import blocks, audio from gnuradio.eng_option import eng_option import numpy as np from gnuradio import gr -from math import pi +from math import pi, sin, cos _def_debug = 0 _def_sps = 10 @@ -55,6 +57,14 @@ def limit(a,lim): return lim return a +def ensure_str(s): # for python 2/3 + if isinstance(s[0], str): + return s + ns = '' + for i in range(len(s)): + ns += chr(s[i]) + return ns + PSEQ = 0 class wrap_gp(object): @@ -127,6 +137,9 @@ class wrap_gp(object): def set_output_dir(self, v): self.output_dir = v + def set_sps(self, sps): + self.sps = sps + def plot(self, buf, bufsz, mode='eye'): BUFSZ = bufsz consumed = min(len(buf), BUFSZ-len(self.buf)) @@ -203,6 +216,25 @@ class wrap_gp(object): s += '%f\n' % (b) s += 'e\n' plots.append('"-" with lines') + elif mode == 'sync': + s_abs = np.abs(self.buf) + sums = np.zeros(self.sps) + for i in range(self.sps): + sums[i] = np.sum(s_abs[range(i, len(self.buf), self.sps)]) + am = np.argmax(sums) + samples = self.buf[am:] + + a1 = -np.angle(samples[0]) + rz = cos(a1) + 1j * sin(a1) + + while len(samples) >= self.sps+1: + for i in range(self.sps+1): + z = samples[i] * rz + s += '%f\t%f\n' % (z.real, z.imag) + s += 'e\n' + plots.append('"-" with linespoints') + samples = samples[self.sps:] + self.buf = np.array([]) # FFT processing needs to be completed to maintain the weighted average buckets @@ -285,6 +317,12 @@ class wrap_gp(object): h+= 'set yrange [-4:4]\n' h+= 'set title "Datascope %s" %s\n' % (self.title, label_color) plot_color = '' + elif mode == 'sync': + h += 'set object 1 rect from screen 0,0 to screen 1,1 %s behind\n' % (background_color) + h += 'set size square\n' + h += 'set xtics %s\n' % (tic_color) + h += 'set ytics %s\n' % (tic_color) + h += 'set border %s\n' % (border_color) elif mode == 'symbol': h+= background h+= 'set yrange [-4:4]\n' @@ -319,6 +357,8 @@ class wrap_gp(object): title = self.title h+= 'set yrange [-1.1:1.1]\n' h+= 'set title "%s" %s\n' % (title, label_color) + if self.output_dir: + s += 'set output\n' ## flush output png dat = '%s%splot %s %s\n%s' % (h0, h, ','.join(plots), plot_color, s) if self.logfile is not None: with open(self.logfile, 'a') as fd: @@ -328,7 +368,11 @@ class wrap_gp(object): self.gp.poll() if self.gp.returncode is None: # make sure gnuplot is still running try: - self.gp.stdin.write(dat) + rc = self.gp.stdin.write(dat) + except (IOError, ValueError): + pass + try: + self.gp.stdin.flush() except (IOError, ValueError): pass if filename: @@ -486,6 +530,89 @@ class mixer_sink_c(gr.sync_block): def kill(self): self.gnuplot.kill() +class sync_plot(threading.Thread): + """ + """ + def __init__(self, debug = _def_debug, block = None, **kwds): + threading.Thread.__init__ (self, **kwds) + self.setDaemon(1) + self.SLEEP_TIME = 3 ## TODO - make more configurable + self.sleep_until = time.time() + self.SLEEP_TIME + self.last_file_time = time.time() + self.keep_running = True + self.debug = debug + self.warned = False + + block.enable_sync_plot(True) # block must refer to a gardner/costas instance + self.blk_id = block.unique_id() + + self.gnuplot = wrap_gp(sps = _def_sps) + self.start() + + def run(self): + while self.keep_running == True: + curr_time = time.time() + if curr_time < self.sleep_until: + time.sleep(1.0) + if self.keep_running == False: + break + else: + self.sleep_until = time.time() + self.SLEEP_TIME + self.check_update() + + def read_raw_file(self, fn): + s = open(fn, 'rb').read() + s_msg = ensure_str(s) + p = s_msg.find('\n') + if p < 1 or p > 24: + return None # error + hdrline = s_msg[:p] + rest = s[p+1:] + params = hdrline.split() + params = [int(p) for p in params] #idx, p1p2, sps, error + idx = params[0] + p1p2 = params[1] + sps = params[2] + error_amt = params[3] + self.gnuplot.set_sps(sps) + if error_amt != 0: + self.set_title("Tuning Error %d" % error_amt) + else: + self.set_title("") + samples = np.frombuffer(rest, dtype=np.complex64) + samples2 = np.concatenate((samples[idx:], samples[:idx])) + needed = sps * 25 if p1p2 == 1 else sps * 21 + if len(samples2) < needed: + if not self.warned: + self.warned = True + sys.stderr.write('read_raw_file: insufficient samples %d, needed %d\n' % (needed, len(samples2))) + elif len(samples2) > needed: + trim = len(samples2) - needed + samples2 = samples2[trim:] + return samples2 # return trimmed buf in np.complex64 format + + def check_update(self): + patt = 'sample-%d*.dat' % (self.blk_id) + names = glob.glob(patt) + if len(names) < 1: # no files to work with + return + d = {n: os.stat(n).st_mtime for n in names} + ds = sorted(d.items(), key=lambda x:x[1], reverse = True)[0] + if ds[1] <= self.last_file_time: + return + self.last_file_time = ds[1] + dat = self.read_raw_file(ds[0]) + self.gnuplot.plot(dat, len(dat), mode='sync') + + def kill(self): + self.keep_running = False + + def set_title(self, title): + self.gnuplot.set_title(title) + + def kill(self): + self.gnuplot.kill() + class symbol_sink_f(gr.sync_block): """ """ diff --git a/op25/gr-op25_repeater/apps/multi_rx.py b/op25/gr-op25_repeater/apps/multi_rx.py index 1969cd6..0ac4bf8 100755 --- a/op25/gr-op25_repeater/apps/multi_rx.py +++ b/op25/gr-op25_repeater/apps/multi_rx.py @@ -49,6 +49,7 @@ from gr_gnuplot import mixer_sink_c from gr_gnuplot import symbol_sink_f from gr_gnuplot import eye_sink_f from gr_gnuplot import setup_correlation +from gr_gnuplot import sync_plot from nxdn_trunking import cac_message @@ -311,6 +312,14 @@ class channel(object): sinks = setup_correlation(sps, self.name, self.demod.connect_bb) self.kill_sink += sinks self.sinks += sinks + elif plot == 'sync': + assert config['demod_type'] == 'cqpsk' ## sync plot requires cqpsk demod type + i = len(self.sinks) + sink = sync_plot(block = self.demod.clock) + sink.set_title(self.name) + self.sinks.append(sink) + self.kill_sink.append(self.sinks[i]) + # does not issue self.connect() else: sys.stderr.write('unrecognized plot type %s\n' % plot) return diff --git a/op25/gr-op25_repeater/apps/rx.py b/op25/gr-op25_repeater/apps/rx.py index 34fcc04..5e5a09d 100755 --- a/op25/gr-op25_repeater/apps/rx.py +++ b/op25/gr-op25_repeater/apps/rx.py @@ -66,6 +66,7 @@ from gr_gnuplot import symbol_sink_f from gr_gnuplot import eye_sink_f from gr_gnuplot import mixer_sink_c from gr_gnuplot import setup_correlation +from gr_gnuplot import sync_plot from terminal import op25_terminal from sockaudio import audio_thread @@ -328,6 +329,11 @@ class p25_rx_block (gr.top_block): elif plot_mode == 'correlation': assert self.options.demod_type == 'fsk4' ## correlation plot requires fsk4 demod type self.plot_sinks += setup_correlation(sps, "", self.demod.connect_bb) + elif plot_mode == 'sync': + assert self.options.demod_type == 'cqpsk' ## sync plot requires cqpsk demod-type + sink = sync_plot(block = self.demod.clock) + self.plot_sinks.append(sink) + # no flowgraph connection in this plot else: raise ValueError('unsupported plot type: %s' % plot_mode) if self.is_http_term(): diff --git a/op25/gr-op25_repeater/apps/trunking.py b/op25/gr-op25_repeater/apps/trunking.py index 00f5ae4..dba0b2c 100644 --- a/op25/gr-op25_repeater/apps/trunking.py +++ b/op25/gr-op25_repeater/apps/trunking.py @@ -524,11 +524,70 @@ class trunked_system (object): def decode_tdma_blk(self, blk): self.stats['tsbks'] += 1 - op = (blk[0] >> 6) & 3 - moc = blk[0] & 0x3f + msg0 = get_ordinals(blk[:1]) + op = (msg0 >> 6) & 3 + moc = msg0 & 0x3f if self.debug > 1: - sys.stderr.write('tdma_cc: decode_blk: op %x moc %x\n' % (op, moc)) - if op == 1 and moc == 0x3c: # adjacent + sys.stderr.write('tdma_cc: decode_blk: op %x moc %x len %d msg0 0x%x\n' % (op, moc, len(blk), msg0)) + if op == 0 and moc == 0: # null msg + if self.debug > 10: + sys.stderr.write('tdma_cc: null block\n') + return -1; + if op == 0 and moc == 8: # null msg zero bias + if self.debug > 10: + sys.stderr.write('tdma_cc: null block zero bias\n') + return -1; + + if op == 1 and moc == 0x38: # system service broadcast + if self.debug > 10: + sys.stderr.write('tdma_cc: system service broadcast\n') + msglen = 9 + elif op == 3 and moc == 0x16: # SNDCP data ch + if self.debug > 10: + sys.stderr.write('tdma_cc: SNDCP data channel announce\n') + msglen = 9 + elif op == 0 and moc == 1: # group voice channel user + msglen = 7 + msg = get_ordinals(blk[:msglen]) + options = (msg >> 40) & 0xff + ga1 = (msg >> 24) & 0xffff + sa1 = (msg ) & 0xffffff + if self.debug > 1: + sys.stderr.write('decode_blk: group voice channel user opts %x ga %x sa %x\n' % (options, ga1, sa1)) + elif op == 0 and moc == 0x30: # power control sq + msglen = 5 + if self.debug > 1: + sys.stderr.write('decode_blk: power control sq\n') + elif op == 1 and moc == 0x30: # sync broadcast + msglen = 9 + msg = get_ordinals(blk[:msglen]) + year = (msg >> 33) & 0x7f + month = (msg >> 29) & 0xf + day = (msg >> 24) & 0x1f + if self.debug > 1: + sys.stderr.write('decode_blk: synchronization broadcast %d %d %d\n' % (year, month, day)) + elif op == 1 and moc == 2: # group voice channel grant update + msglen = 9 + msg = get_ordinals(blk[:msglen]) + ch1 = (msg >> 48) & 0xffff + ga1 = (msg >> 32) & 0xffff + ch2 = (msg >> 16) & 0xffff + ga2 = (msg ) & 0xffff + f1 = self.channel_id_to_frequency(ch1) + f2 = self.channel_id_to_frequency(ch2) + if self.debug > 1: + sys.stderr.write('decode_blk: group voice channel grant update ga1 %d ch1 %s ga2 %d ch2 %s\n' % (ga1, f1, ga2, f2)) + opcode = 2 # (p25p1 opcode) + d = {'cc_event': 'grp_v_ch_grant_updt', 'mfrid': 0, 'frequency1': f1, 'group1': self.mk_tg_dict(ga1), 'opcode': opcode, 'tdma_slot': self.get_tdma_slot(ch1) } + self.update_voice_frequency(f1, tgid=ga1, tdma_slot=self.get_tdma_slot(ch1)) + if f1 != f2: + self.update_voice_frequency(f2, tgid=ga2, tdma_slot=self.get_tdma_slot(ch2)) + d['frequency2'] = f2 + d['group2'] = self.mk_tg_dict(ga2) + self.post_event(d) + if self.debug > 10: + sys.stderr.write('phase 2 tsbk02 grant update: chan %s %d %s %d\n' %(self.channel_id_to_string(ch1), ga1, self.channel_id_to_string(ch2), ga2)) + elif op == 1 and moc == 0x3c: # adjacent msglen = 9 msg = get_ordinals(blk[:msglen]) syid = (msg >> 40) & 0xfff @@ -644,12 +703,17 @@ class trunked_system (object): else: msglen = -1 if self.debug > 0: - sys.stderr.write ('tdma_cc unknown request: %x %x %02x %02x %02x\n' % (op, moc, blk[0], blk[1], blk[2] )) + sys.stderr.write ('tdma_cc unknown request: %x %x %x\n' % (op, moc, get_ordinals(blk))) return msglen def decode_tdma_cc(self, blk): - rc = self.decode_tdma_blk(blk) - # TODO: Attempt to decode remaining half? + while True: + rc = self.decode_tdma_blk(blk) + if rc < 1: + break + if rc >= len(blk): + break + blk = blk[rc:] def decode_tsbk_harris(self, tsbk, opcode, mfrid): HARRIS_SGS_EXPIRES = 5.0 # sec. @@ -1361,14 +1425,12 @@ class rx_ctl (object): # nac is always 1st two bytes nac = get_ordinals(msgtext[:2]) msgtext = msgtext[2:] + if self.debug > 1: + sys.stderr.write('tdma_cc message nac 0x%x text 0x%x len %d\n' % (nac, get_ordinals(msgtext), len(msgtext))) if nac not in self.trunked_systems.keys(): sys.stderr.write('tdma_cc received from unexpected NAC 0x%x\n' % nac) return tsys = self.trunked_systems[nac] - m1 = msgtext[1] - b1 = (m1 >> 7) & 1 - b2 = (m1 >> 6) & 1 - moc = m1 & 0x3f tsys.decode_tdma_cc(msgtext[1:]) return elif mtype < 0: diff --git a/op25/gr-op25_repeater/include/op25_repeater/gardner_costas_cc.h b/op25/gr-op25_repeater/include/op25_repeater/gardner_costas_cc.h index 524c5bc..07c0d3f 100644 --- a/op25/gr-op25_repeater/include/op25_repeater/gardner_costas_cc.h +++ b/op25/gr-op25_repeater/include/op25_repeater/gardner_costas_cc.h @@ -54,6 +54,7 @@ namespace gr { virtual bool is_muted(void) = 0; virtual void set_tdma(bool) = 0; virtual bool is_tdma(void) = 0; + virtual void enable_sync_plot(bool) = 0; }; } // namespace op25_repeater diff --git a/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc b/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc index dc36b74..2d770bf 100644 --- a/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc +++ b/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc @@ -35,6 +35,7 @@ #include #include #include +#include #include "p25_frame.h" #include "p25p2_framer.h" @@ -50,6 +51,8 @@ static const int NUM_COMPLEX=100; static const int FM_COUNT=500; // number of samples per measurement frame +static const int N_FILES = 4; // number of filenames to cycle through + namespace gr { namespace op25_repeater { @@ -66,6 +69,49 @@ static inline std::complex sgn(std::complexc) { d_event_type = c; \ } +static inline bool is_future(struct timeval*t) { + struct timeval current_t; + gettimeofday(¤t_t,0); + if (t->tv_sec > current_t.tv_sec) + return true; + else if (t->tv_sec < current_t.tv_sec) + return false; + else if (t->tv_usec > current_t.tv_usec) + return true; + else + return false; +} + +void gardner_costas_cc_impl::dump_samples(int error_amt) { + // TODO = disk I/O from inside a gr flow graph block work function (tsk tsk) + char tmp_filename[256]; + char filename[256]; + char line[64]; + FILE *fp1; + if (!d_enable_sync_plot) + return; + if (d_prev_sample == NULL) + return; + if (is_future(&d_next_sample_time)) + return; + gettimeofday(&d_next_sample_time,0); + d_next_sample_time.tv_sec += 5; + sprintf(filename, "sample-%ld-%d.dat", unique_id(), d_sample_file_id); + d_sample_file_id ++; + d_sample_file_id = d_sample_file_id % N_FILES; + sprintf(line, "%u %d %d %d\n", d_prev_sample_p, (d_is_tdma) ? 2 : 1, (int) (d_omega + 0.5), error_amt); + strcpy(tmp_filename, filename); + strcat(tmp_filename, ".tmp"); + fp1 = fopen(tmp_filename, "wb"); + if (!fp1) + return; + fwrite (line, 1, strlen(line), fp1); + fwrite (d_prev_sample, 1, d_n_prev_sample * sizeof(gr_complex), fp1); + fclose(fp1); + + rename(tmp_filename, filename); +} + uint8_t gardner_costas_cc_impl::slicer(float sym) { uint8_t dibit = 0; static const float PI_4 = M_PI / 4.0; @@ -89,36 +135,44 @@ uint8_t gardner_costas_cc_impl::slicer(float sym) { if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ P25_FRAME_SYNC_MAGIC, 0, 48)) { // fprintf(stderr, "P25P1 Framing detect\n"); UPDATE_COUNT(' ') + dump_samples(0); } else if(check_frame_sync((nid_accum & P25P2_FRAME_SYNC_MASK) ^ P25P2_FRAME_SYNC_MAGIC, 0, 40)) { // fprintf(stderr, "P25P2 Framing detect\n"); UPDATE_COUNT(' ') + dump_samples(0); } if (d_is_tdma) { if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0x000104015155LL, 0, 40)) { fprintf(stderr, "TDMA: channel %d tuning error -1200\n", -1); UPDATE_COUNT('-') + dump_samples(-1200); } else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xfefbfeaeaaLL, 0, 40)) { fprintf(stderr, "TDMA: channel %d tuning error +1200\n", -1); UPDATE_COUNT('+') + dump_samples(1200); } else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xa8a2a80800LL, 0, 40)) { fprintf(stderr, "TDMA: channel %d tuning error +/- 2400\n", -1); UPDATE_COUNT('|') + dump_samples(2400); } } else { if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0x001050551155LL, 0, 48)) { // fprintf(stderr, "tuning error -1200\n"); UPDATE_COUNT('-') + dump_samples(-1200); } else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xFFEFAFAAEEAALL, 0, 48)) { // fprintf(stderr, "tuning error +1200\n"); UPDATE_COUNT('+') + dump_samples(1200); } else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xAA8A0A008800LL, 0, 48)) { // fprintf(stderr, "tuning error +/- 2400\n"); UPDATE_COUNT('|') + dump_samples(2400); } } if (d_event_type == ' ' || d_event_count < 5) { @@ -165,11 +219,15 @@ uint8_t gardner_costas_cc_impl::slicer(float sym) { d_event_count(0), d_event_type(' '), d_symbol_seq(samples_per_symbol * 4800), d_update_request(0), - d_fm(0), d_fm_accum(0), d_fm_count(0), d_muted(false), d_is_tdma(false) + d_fm(0), d_fm_accum(0), d_fm_count(0), d_muted(false), d_is_tdma(false), + d_enable_sync_plot(false), + d_prev_sample(NULL), d_n_prev_sample(0), d_prev_sample_p(0), + d_sample_file_id(0) { set_omega(samples_per_symbol); set_relative_rate (1.0 / d_omega); set_history(d_twice_sps); // ensure extra input is available + gettimeofday(&d_next_sample_time, 0); } /* @@ -177,6 +235,10 @@ uint8_t gardner_costas_cc_impl::slicer(float sym) { */ gardner_costas_cc_impl::~gardner_costas_cc_impl() { + if (d_prev_sample != NULL) { + free(d_prev_sample); + d_prev_sample = NULL; + } delete [] d_dl; delete d_interp; } @@ -245,21 +307,20 @@ gardner_costas_cc_impl::phase_error_tracking(gr_complex sample) phase_error = phase_error_detector_qpsk(sample); d_freq += d_beta*phase_error*abs(sample); // adjust frequency based on error - d_phase += d_freq + d_alpha*phase_error*abs(sample); // adjust phase based on error + d_phase += d_alpha*phase_error*abs(sample); // adjust phase based on error - // Make sure we stay within +-2pi - while(d_phase > M_TWOPI) + // Make sure we stay within +-pi + while(d_phase > M_PI) d_phase -= M_TWOPI; - while(d_phase < -M_TWOPI) + while(d_phase < -M_PI) d_phase += M_TWOPI; // Limit the frequency range d_freq = gr::branchless_clip(d_freq, d_max_freq); #if VERBOSE_COSTAS - printf("cl: phase_error: %f phase: %f freq: %f sample: %f+j%f constellation: %f+j%f\n", - phase_error, d_phase, d_freq, sample.real(), sample.imag(), - d_constellation[d_current_const_point].real(), d_constellation[d_current_const_point].imag()); + fprintf(stderr, "COSTAS\t%f\t%f\t%f\t%f+j%f\n", + phase_error, d_phase, d_freq, sample.real(), sample.imag()); #endif } @@ -277,21 +338,33 @@ gardner_costas_cc_impl::general_work (int noutput_items, gr_complex interp_samp, interp_samp_mid, diffdec; float error_real, error_imag, symbol_error; + if (d_enable_sync_plot && d_prev_sample == NULL) { + d_n_prev_sample = (int) (d_omega + 0.5); // sps + d_n_prev_sample *= (d_is_tdma) ? 32 : 25; // enough for p25p1 or p25p2 sync + d_prev_sample = (gr_complex *) calloc(d_n_prev_sample, sizeof(gr_complex)); + } + while((o < noutput_items) && (i < ninput_items[0])) { while((d_mu > 1.0) && (i < ninput_items[0])) { d_mu --; d_phase += d_freq; // Keep phase clamped and not walk to infinity - while(d_phase > M_TWOPI) + while(d_phase > M_PI) d_phase -= M_TWOPI; - while(d_phase < -M_TWOPI) + while(d_phase < -M_PI) d_phase += M_TWOPI; nco = gr_expj(d_phase+d_theta); // get the NCO value for derotating the curr symbol = in[i]; sample = nco*symbol; // get the downconverted symbol + if (d_enable_sync_plot && d_prev_sample != NULL) { + d_prev_sample[d_prev_sample_p] = sample; + d_prev_sample_p ++; + d_prev_sample_p %= d_n_prev_sample; + } + d_dl[d_dl_index] = sample; d_dl[d_dl_index + d_twice_sps] = sample; d_dl_index ++; @@ -345,7 +418,7 @@ gardner_costas_cc_impl::general_work (int noutput_items, d_omega = d_omega + d_gain_omega * symbol_error * abs(interp_samp); // update omega based on loop error d_omega = d_omega_mid + gr::branchless_clip(d_omega-d_omega_mid, d_omega_rel); // make sure we don't walk away #if VERBOSE_GARDNER - printf("%f\t%f\t%f\t%f\t%f\n", symbol_error, d_mu, d_omega, error_real, error_imag); + fprintf(stderr, "%f\t%f\t%f\t%f\t%f\n", symbol_error, d_mu, d_omega, error_real, error_imag); #endif } else { symbol_error = 0; diff --git a/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.h b/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.h index c413557..e731ed4 100644 --- a/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.h +++ b/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.h @@ -78,6 +78,7 @@ namespace gr { inline bool is_muted(void) { return d_muted; } inline void set_tdma(bool v) { d_is_tdma = v; } inline bool is_tdma(void) { return d_is_tdma; } + inline void enable_sync_plot(bool v) { d_enable_sync_plot = v; } protected: bool input_sample0(gr_complex, gr_complex& outp); @@ -121,9 +122,18 @@ protected: int d_fm_count; bool d_muted; bool d_is_tdma; + bool d_enable_sync_plot; + + gr_complex *d_prev_sample; + unsigned int d_n_prev_sample; + unsigned int d_prev_sample_p; + + struct timeval d_next_sample_time; + int d_sample_file_id; float phase_error_detector_qpsk(gr_complex sample); void phase_error_tracking(gr_complex sample); + void dump_samples(int); }; } // namespace op25_repeater diff --git a/op25/gr-op25_repeater/lib/p25p2_tdma.cc b/op25/gr-op25_repeater/lib/p25p2_tdma.cc index 020bfa1..b9211c9 100644 --- a/op25/gr-op25_repeater/lib/p25p2_tdma.cc +++ b/op25/gr-op25_repeater/lib/p25p2_tdma.cc @@ -1,4 +1,4 @@ -// P25 TDMA Decoder (C) Copyright 2013, 2014 Max H. Parke KA1RBI +// P25 TDMA Decoder (C) Copyright 2013, 2014, 2021 Max H. Parke KA1RBI // Copyright 2017 Graham J. Norbury (modularization rewrite) // // This file is part of OP25 @@ -38,6 +38,7 @@ static const int BURST_SIZE = 180; static const int SUPERFRAME_SIZE = (12*BURST_SIZE); +static const int which_slot[] = {0,1,0,1,0,1,0,1,0,1,1,0}; static uint16_t crc12(const uint8_t bits[], unsigned int len) { uint16_t crc=0; @@ -70,24 +71,6 @@ static bool crc12_ok(const uint8_t bits[], unsigned int len) { return (crc == crc12(bits,len)); } -static const uint8_t mac_msg_len[256] = { - 0, 7, 8, 7, 0, 16, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 14, 15, 0, 0, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 5, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 9, 7, 9, 0, 9, 8, 9, 0, 0, 0, 9, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 9, 7, 0, 0, 0, 0, 7, 0, 0, 8, 14, 7, - 9, 9, 0, 0, 9, 0, 0, 9, 0, 0, 7, 0, 0, 7, 0, 0, - 0, 0, 0, 9, 9, 9, 0, 0, 9, 9, 9, 11, 9, 9, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 11, 0, 0, 8, 15, 12, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 9, 0, 0, 0, 11, 0, 0, 0, 0, 11, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 11, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 13, 11, 0, 0, 0 }; - p25p2_tdma::p25p2_tdma(const op25_audio& udp, int slotid, int debug, bool do_msgq, gr::msg_queue::sptr queue, std::deque &qptr, bool do_audio_output, int msgq_id) : // constructor op25audio(udp), write_bufp(0), @@ -122,6 +105,7 @@ bool p25p2_tdma::rx_sym(uint8_t sym) void p25p2_tdma::set_slotid(int slotid) { + memset(tdma_xormask, 0, SUPERFRAME_SIZE); assert (slotid == 0 || slotid == 1); d_slotid = slotid; } @@ -141,15 +125,33 @@ int p25p2_tdma::process_mac_pdu(const uint8_t byte_buf[], const unsigned int len { unsigned int opcode = (byte_buf[0] >> 5) & 0x7; unsigned int offset = (byte_buf[0] >> 2) & 0x7; + bool my_slot = (which_slot[sync.tdma_slotid()] == d_slotid); if (d_debug >= 10) { - fprintf(stderr, "%s process_mac_pdu: opcode %d len %d\n", logts.get(), opcode, len); + fprintf(stderr, "%s process_mac_pdu: opcode %d len %d buf %02x %02x %02x\n", logts.get(), opcode, len, byte_buf[0], byte_buf[1], byte_buf[2]); } + if (opcode == 2) { // MAC_END_PTT + uint16_t colorcd = ((byte_buf[1] & 0x0f) << 8) + byte_buf[2]; + if (colorcd != d_nac && d_debug > 0) + fprintf(stderr, "p25p2_tdma_ process_mac_pdu: MAC_END_PTT color code 0x%x does not match d_nac 0x%x channel %d\n", colorcd, d_nac, d_msgq_id); + } + + if (opcode == 3 || opcode == 4 || opcode == 6) { // send msg for MAC_IDLE, MAC_ACTIVE, MAC_HANGTIME + char nac_color[2]; + nac_color[0] = d_nac >> 8; + nac_color[1] = d_nac & 0xff; + send_msg(std::string(nac_color, 2) + std::string((const char *)byte_buf, len), -6); + } + + if (opcode != 0 && !my_slot) // for all except MAC_SIGNAL, ignore if on oppo. slot + return -1; + switch (opcode) { case 0: // MAC_SIGNAL handle_mac_signal(byte_buf, len); + return -1; break; case 1: // MAC_PTT @@ -161,20 +163,22 @@ int p25p2_tdma::process_mac_pdu(const uint8_t byte_buf[], const unsigned int len break; case 3: // MAC_IDLE - handle_mac_idle(byte_buf, len); + op25audio.send_audio_flag(op25_audio::DRAIN); break; case 4: // MAC_ACTIVE - handle_mac_active(byte_buf, len); break; case 6: // MAC_HANGTIME - handle_mac_hangtime(byte_buf, len); op25audio.send_audio_flag(op25_audio::DRAIN); break; + default: + if (d_debug > 0) + fprintf(stderr, "p25p2_tdma_ process_mac_pdu: unrecognized opcode 0x%x channel %d\n", opcode, d_msgq_id); + break; } // maps sacch opcodes into phase I duid values - static const int opcode_map[8] = {3, 5, 15, 15, 5, 3, 3, 3}; + static const int opcode_map[8] = {7, 5, 15, 15, 5, 3, 3, 3}; return opcode_map[opcode]; } @@ -233,162 +237,6 @@ void p25p2_tdma::handle_mac_end_ptt(const uint8_t byte_buf[], const unsigned int op25audio.send_audio_flag(op25_audio::DRAIN); } -void p25p2_tdma::handle_mac_idle(const uint8_t byte_buf[], const unsigned int len) -{ - if (d_debug >= 10) - fprintf(stderr, "%s MAC_IDLE: ", logts.get()); - - decode_mac_msg(byte_buf, len); - op25audio.send_audio_flag(op25_audio::DRAIN); - - if (d_debug >= 10) - fprintf(stderr, "\n"); -} - -void p25p2_tdma::handle_mac_active(const uint8_t byte_buf[], const unsigned int len) -{ - if (d_debug >= 10) - fprintf(stderr, "%s MAC_ACTIVE: ", logts.get()); - - decode_mac_msg(byte_buf, len); - - if (d_debug >= 10) - fprintf(stderr, "\n"); -} - -void p25p2_tdma::handle_mac_hangtime(const uint8_t byte_buf[], const unsigned int len) -{ - if (d_debug >= 10) - fprintf(stderr, "%s MAC_HANGTIME: ", logts.get()); - - decode_mac_msg(byte_buf, len); - op25audio.send_audio_flag(op25_audio::DRAIN); - - if (d_debug >= 10) - fprintf(stderr, "\n"); -} - - -void p25p2_tdma::decode_mac_msg(const uint8_t byte_buf[], const unsigned int len) -{ - std::string s; - uint8_t b1b2, cfva, mco, lra, rfss, site_id, ssc, svcopts[3], msg_ptr, msg_len; - uint16_t chan[3], ch_t[2], ch_r[2], colorcd, grpaddr[3], sys_id; - uint32_t srcaddr, wacn_id; - - for (msg_ptr = 1; msg_ptr < len; ) - { - b1b2 = byte_buf[msg_ptr] >> 6; - mco = byte_buf[msg_ptr] & 0x3f; - msg_len = mac_msg_len[(b1b2 << 6) + mco]; - if (d_debug >= 10) - fprintf(stderr, "mco=%01x/%02x", b1b2, mco); - - switch(byte_buf[msg_ptr]) - { - case 0x00: // Null message - break; - case 0x40: // Group Voice Channel Grant Abbreviated - svcopts[0] = (byte_buf[msg_ptr+1] ) ; - chan[0] = (byte_buf[msg_ptr+2] << 8) + byte_buf[msg_ptr+3]; - grpaddr[0] = (byte_buf[msg_ptr+4] << 8) + byte_buf[msg_ptr+5]; - srcaddr = (byte_buf[msg_ptr+6] << 16) + (byte_buf[msg_ptr+7] << 8) + byte_buf[msg_ptr+8]; - if (d_debug >= 10) - fprintf(stderr, ", svcopts=0x%02x, ch=%u, grpaddr=%u, srcaddr=%u", svcopts[0], chan[0], grpaddr[0], srcaddr); - break; - case 0xc0: // Group Voice Channel Grant Extended - svcopts[0] = (byte_buf[msg_ptr+1] ) ; - ch_t[0] = (byte_buf[msg_ptr+2] << 8) + byte_buf[msg_ptr+3]; - ch_r[0] = (byte_buf[msg_ptr+4] << 8) + byte_buf[msg_ptr+5]; - grpaddr[0] = (byte_buf[msg_ptr+6] << 8) + byte_buf[msg_ptr+7]; - srcaddr = (byte_buf[msg_ptr+8] << 16) + (byte_buf[msg_ptr+9] << 8) + byte_buf[msg_ptr+10]; - if (d_debug >= 10) - fprintf(stderr, ", svcopts=0x%02x, ch_t=%u, ch_t=%u, grpaddr=%u, srcaddr=%u", svcopts[0], ch_t[0], ch_r[0], grpaddr[0], srcaddr); - break; - case 0x01: // Group Voice Channel User Message Abbreviated - grpaddr[0] = (byte_buf[msg_ptr+2] << 8) + byte_buf[msg_ptr+3]; - srcaddr = (byte_buf[msg_ptr+4] << 16) + (byte_buf[msg_ptr+5] << 8) + byte_buf[msg_ptr+6]; - if (d_debug >= 10) - fprintf(stderr, ", grpaddr=%u, srcaddr=%u", grpaddr[0], srcaddr); - s = "{\"srcaddr\" : " + std::to_string(srcaddr) + ", \"grpaddr\": " + std::to_string(grpaddr[0]) + ", \"nac\" : " + std::to_string(d_nac) + "}"; - send_msg(s, -3); - break; - case 0x42: // Group Voice Channel Grant Update - chan[0] = (byte_buf[msg_ptr+1] << 8) + byte_buf[msg_ptr+2]; - grpaddr[0] = (byte_buf[msg_ptr+3] << 8) + byte_buf[msg_ptr+4]; - chan[1] = (byte_buf[msg_ptr+5] << 8) + byte_buf[msg_ptr+6]; - grpaddr[1] = (byte_buf[msg_ptr+7] << 8) + byte_buf[msg_ptr+8]; - if (d_debug >= 10) - fprintf(stderr, ", ch_1=%u, grpaddr1=%u, ch_2=%u, grpaddr2=%u", chan[0], grpaddr[0], chan[1], grpaddr[1]); - break; - case 0xc3: // Group Voice Channel Grant Update Explicit - svcopts[0] = (byte_buf[msg_ptr+1] ) ; - ch_t[0] = (byte_buf[msg_ptr+2] << 8) + byte_buf[msg_ptr+3]; - ch_r[0] = (byte_buf[msg_ptr+4] << 8) + byte_buf[msg_ptr+5]; - grpaddr[0] = (byte_buf[msg_ptr+6] << 8) + byte_buf[msg_ptr+7]; - if (d_debug >= 10) - fprintf(stderr, ", svcopts=0x%02x, ch_t=%u, ch_r=%u, grpaddr=%u", svcopts[0], ch_t[0], ch_r[0], grpaddr[0]); - break; - case 0x05: // Group Voice Channel Grant Update Multiple - svcopts[0] = (byte_buf[msg_ptr+ 1] ) ; - chan[0] = (byte_buf[msg_ptr+ 2] << 8) + byte_buf[msg_ptr+ 3]; - grpaddr[0] = (byte_buf[msg_ptr+ 4] << 8) + byte_buf[msg_ptr+ 5]; - svcopts[1] = (byte_buf[msg_ptr+ 6] ) ; - chan[1] = (byte_buf[msg_ptr+ 7] << 8) + byte_buf[msg_ptr+ 8]; - grpaddr[1] = (byte_buf[msg_ptr+ 9] << 8) + byte_buf[msg_ptr+10]; - svcopts[2] = (byte_buf[msg_ptr+11] ) ; - chan[2] = (byte_buf[msg_ptr+12] << 8) + byte_buf[msg_ptr+13]; - grpaddr[2] = (byte_buf[msg_ptr+14] << 8) + byte_buf[msg_ptr+15]; - if (d_debug >= 10) - fprintf(stderr, ", svcopt1=0x%02x, ch_1=%u, grpaddr1=%u, svcopt2=0x%02x, ch_2=%u, grpaddr2=%u, svcopt3=0x%02x, ch_3=%u, grpaddr3=%u", svcopts[0], chan[0], grpaddr[0], svcopts[1], chan[1], grpaddr[1], svcopts[2], chan[2], grpaddr[2]); - break; - case 0x25: // Group Voice Channel Grant Update Multiple Explicit - svcopts[0] = (byte_buf[msg_ptr+ 1] ) ; - ch_t[0] = (byte_buf[msg_ptr+ 2] << 8) + byte_buf[msg_ptr+ 3]; - ch_r[0] = (byte_buf[msg_ptr+ 4] << 8) + byte_buf[msg_ptr+ 5]; - grpaddr[0] = (byte_buf[msg_ptr+ 6] << 8) + byte_buf[msg_ptr+ 7]; - svcopts[1] = (byte_buf[msg_ptr+ 8] ) ; - ch_t[1] = (byte_buf[msg_ptr+ 9] << 8) + byte_buf[msg_ptr+10]; - ch_r[1] = (byte_buf[msg_ptr+11] << 8) + byte_buf[msg_ptr+12]; - grpaddr[1] = (byte_buf[msg_ptr+13] << 8) + byte_buf[msg_ptr+14]; - if (d_debug >= 10) - fprintf(stderr, ", svcopt1=0x%02x, ch_t1=%u, ch_r1=%u, grpaddr1=%u, svcopt2=0x%02x, ch_t2=%u, ch_r2=%u, grpaddr2=%u", svcopts[0], ch_t[0], ch_r[0], grpaddr[0], svcopts[1], ch_t[1], ch_r[1], grpaddr[1]); - break; - case 0x7b: // Network Status Broadcast Abbreviated - lra = byte_buf[msg_ptr+1]; - wacn_id = (byte_buf[msg_ptr+2] << 12) + (byte_buf[msg_ptr+3] << 4) + (byte_buf[msg_ptr+4] >> 4); - sys_id = ((byte_buf[msg_ptr+4] & 0x0f) << 8) + byte_buf[msg_ptr+5]; - chan[0] = (byte_buf[msg_ptr+6] << 8) + byte_buf[msg_ptr+7]; - ssc = byte_buf[msg_ptr+8]; - colorcd = ((byte_buf[msg_ptr+9] & 0x0f) << 8) + byte_buf[msg_ptr+10]; - if (d_debug >= 10) - fprintf(stderr, ", lra=0x%02x, wacn_id=0x%05x, sys_id=0x%03x, ch=%u, ssc=0x%02x, colorcd=%03x", lra, wacn_id, sys_id, chan[0], ssc, colorcd); - break; - case 0x7c: // Adjacent Status Broadcast Abbreviated - lra = byte_buf[msg_ptr+1]; - cfva = (byte_buf[msg_ptr+2] >> 4); - sys_id = ((byte_buf[msg_ptr+2] & 0x0f) << 8) + byte_buf[msg_ptr+3]; - rfss = byte_buf[msg_ptr+4]; - site_id = byte_buf[msg_ptr+5]; - chan[0] = (byte_buf[msg_ptr+6] << 8) + byte_buf[msg_ptr+7]; - ssc = byte_buf[msg_ptr+8]; - if (d_debug >= 10) - fprintf(stderr, ", lra=0x%02x, cfva=0x%01x, sys_id=0x%03x, rfss=%u, site=%u, ch=%u, ssc=0x%02x", lra, cfva, sys_id, rfss, site_id, chan[0], ssc); - break; - case 0xfc: // Adjacent Status Broadcast Extended - break; - case 0xfb: // Network Status Broadcast Extended - colorcd = ((byte_buf[msg_ptr+11] & 0x0f) << 8) + byte_buf[msg_ptr+12]; - if (d_debug >= 10) - fprintf(stderr, ", colorcd=%03x", colorcd); - break; - } - msg_ptr = (msg_len == 0) ? len : (msg_ptr + msg_len); // TODO: handle variable length messages - if ((d_debug >= 10) && (msg_ptr < len)) - fprintf(stderr,", "); - } -} - int p25p2_tdma::handle_acch_frame(const uint8_t dibits[], bool fast, bool is_lcch) { int i, j, rc; @@ -472,15 +320,14 @@ int p25p2_tdma::handle_acch_frame(const uint8_t dibits[], bool fast, bool is_lcc bool crc_ok = (is_lcch) ? (crc16(bits, len) == 0) : crc12_ok(bits, len); int olen = (is_lcch) ? 23 : len/8; + if (d_debug >= 1) + fprintf(stderr, "p25p2_tdma: crc%d result: %s, length %d\n", (is_lcch) ? 16 : 12, (crc_ok) ? "ok" : "failed", olen); rc = -1; if (crc_ok) { // TODO: rewrite crc12 so we don't have to do so much bit manipulation - // fprintf(stderr, "crc12 ok\n"); for (int i=0; i