p25p2 pdu updates, new 'sync' plot mode
This commit is contained in:
parent
8a948a3bee
commit
5818d58dba
|
@ -24,13 +24,15 @@ import os
|
||||||
import time
|
import time
|
||||||
import subprocess
|
import subprocess
|
||||||
import json
|
import json
|
||||||
|
import threading
|
||||||
|
import glob
|
||||||
|
|
||||||
from gnuradio import gr, gru, eng_notation
|
from gnuradio import gr, gru, eng_notation
|
||||||
from gnuradio import blocks, audio
|
from gnuradio import blocks, audio
|
||||||
from gnuradio.eng_option import eng_option
|
from gnuradio.eng_option import eng_option
|
||||||
import numpy as np
|
import numpy as np
|
||||||
from gnuradio import gr
|
from gnuradio import gr
|
||||||
from math import pi
|
from math import pi, sin, cos
|
||||||
|
|
||||||
_def_debug = 0
|
_def_debug = 0
|
||||||
_def_sps = 10
|
_def_sps = 10
|
||||||
|
@ -55,6 +57,14 @@ def limit(a,lim):
|
||||||
return lim
|
return lim
|
||||||
return a
|
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
|
PSEQ = 0
|
||||||
|
|
||||||
class wrap_gp(object):
|
class wrap_gp(object):
|
||||||
|
@ -127,6 +137,9 @@ class wrap_gp(object):
|
||||||
def set_output_dir(self, v):
|
def set_output_dir(self, v):
|
||||||
self.output_dir = v
|
self.output_dir = v
|
||||||
|
|
||||||
|
def set_sps(self, sps):
|
||||||
|
self.sps = sps
|
||||||
|
|
||||||
def plot(self, buf, bufsz, mode='eye'):
|
def plot(self, buf, bufsz, mode='eye'):
|
||||||
BUFSZ = bufsz
|
BUFSZ = bufsz
|
||||||
consumed = min(len(buf), BUFSZ-len(self.buf))
|
consumed = min(len(buf), BUFSZ-len(self.buf))
|
||||||
|
@ -203,6 +216,25 @@ class wrap_gp(object):
|
||||||
s += '%f\n' % (b)
|
s += '%f\n' % (b)
|
||||||
s += 'e\n'
|
s += 'e\n'
|
||||||
plots.append('"-" with lines')
|
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([])
|
self.buf = np.array([])
|
||||||
|
|
||||||
# FFT processing needs to be completed to maintain the weighted average buckets
|
# 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 yrange [-4:4]\n'
|
||||||
h+= 'set title "Datascope %s" %s\n' % (self.title, label_color)
|
h+= 'set title "Datascope %s" %s\n' % (self.title, label_color)
|
||||||
plot_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':
|
elif mode == 'symbol':
|
||||||
h+= background
|
h+= background
|
||||||
h+= 'set yrange [-4:4]\n'
|
h+= 'set yrange [-4:4]\n'
|
||||||
|
@ -319,6 +357,8 @@ class wrap_gp(object):
|
||||||
title = self.title
|
title = self.title
|
||||||
h+= 'set yrange [-1.1:1.1]\n'
|
h+= 'set yrange [-1.1:1.1]\n'
|
||||||
h+= 'set title "%s" %s\n' % (title, label_color)
|
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)
|
dat = '%s%splot %s %s\n%s' % (h0, h, ','.join(plots), plot_color, s)
|
||||||
if self.logfile is not None:
|
if self.logfile is not None:
|
||||||
with open(self.logfile, 'a') as fd:
|
with open(self.logfile, 'a') as fd:
|
||||||
|
@ -328,7 +368,11 @@ class wrap_gp(object):
|
||||||
self.gp.poll()
|
self.gp.poll()
|
||||||
if self.gp.returncode is None: # make sure gnuplot is still running
|
if self.gp.returncode is None: # make sure gnuplot is still running
|
||||||
try:
|
try:
|
||||||
self.gp.stdin.write(dat)
|
rc = self.gp.stdin.write(dat)
|
||||||
|
except (IOError, ValueError):
|
||||||
|
pass
|
||||||
|
try:
|
||||||
|
self.gp.stdin.flush()
|
||||||
except (IOError, ValueError):
|
except (IOError, ValueError):
|
||||||
pass
|
pass
|
||||||
if filename:
|
if filename:
|
||||||
|
@ -486,6 +530,89 @@ class mixer_sink_c(gr.sync_block):
|
||||||
def kill(self):
|
def kill(self):
|
||||||
self.gnuplot.kill()
|
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):
|
class symbol_sink_f(gr.sync_block):
|
||||||
"""
|
"""
|
||||||
"""
|
"""
|
||||||
|
|
|
@ -49,6 +49,7 @@ from gr_gnuplot import mixer_sink_c
|
||||||
from gr_gnuplot import symbol_sink_f
|
from gr_gnuplot import symbol_sink_f
|
||||||
from gr_gnuplot import eye_sink_f
|
from gr_gnuplot import eye_sink_f
|
||||||
from gr_gnuplot import setup_correlation
|
from gr_gnuplot import setup_correlation
|
||||||
|
from gr_gnuplot import sync_plot
|
||||||
|
|
||||||
from nxdn_trunking import cac_message
|
from nxdn_trunking import cac_message
|
||||||
|
|
||||||
|
@ -311,6 +312,14 @@ class channel(object):
|
||||||
sinks = setup_correlation(sps, self.name, self.demod.connect_bb)
|
sinks = setup_correlation(sps, self.name, self.demod.connect_bb)
|
||||||
self.kill_sink += sinks
|
self.kill_sink += sinks
|
||||||
self.sinks += 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:
|
else:
|
||||||
sys.stderr.write('unrecognized plot type %s\n' % plot)
|
sys.stderr.write('unrecognized plot type %s\n' % plot)
|
||||||
return
|
return
|
||||||
|
|
|
@ -66,6 +66,7 @@ from gr_gnuplot import symbol_sink_f
|
||||||
from gr_gnuplot import eye_sink_f
|
from gr_gnuplot import eye_sink_f
|
||||||
from gr_gnuplot import mixer_sink_c
|
from gr_gnuplot import mixer_sink_c
|
||||||
from gr_gnuplot import setup_correlation
|
from gr_gnuplot import setup_correlation
|
||||||
|
from gr_gnuplot import sync_plot
|
||||||
|
|
||||||
from terminal import op25_terminal
|
from terminal import op25_terminal
|
||||||
from sockaudio import audio_thread
|
from sockaudio import audio_thread
|
||||||
|
@ -328,6 +329,11 @@ class p25_rx_block (gr.top_block):
|
||||||
elif plot_mode == 'correlation':
|
elif plot_mode == 'correlation':
|
||||||
assert self.options.demod_type == 'fsk4' ## correlation plot requires fsk4 demod type
|
assert self.options.demod_type == 'fsk4' ## correlation plot requires fsk4 demod type
|
||||||
self.plot_sinks += setup_correlation(sps, "", self.demod.connect_bb)
|
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:
|
else:
|
||||||
raise ValueError('unsupported plot type: %s' % plot_mode)
|
raise ValueError('unsupported plot type: %s' % plot_mode)
|
||||||
if self.is_http_term():
|
if self.is_http_term():
|
||||||
|
|
|
@ -524,11 +524,70 @@ class trunked_system (object):
|
||||||
|
|
||||||
def decode_tdma_blk(self, blk):
|
def decode_tdma_blk(self, blk):
|
||||||
self.stats['tsbks'] += 1
|
self.stats['tsbks'] += 1
|
||||||
op = (blk[0] >> 6) & 3
|
msg0 = get_ordinals(blk[:1])
|
||||||
moc = blk[0] & 0x3f
|
op = (msg0 >> 6) & 3
|
||||||
|
moc = msg0 & 0x3f
|
||||||
if self.debug > 1:
|
if self.debug > 1:
|
||||||
sys.stderr.write('tdma_cc: decode_blk: op %x moc %x\n' % (op, moc))
|
sys.stderr.write('tdma_cc: decode_blk: op %x moc %x len %d msg0 0x%x\n' % (op, moc, len(blk), msg0))
|
||||||
if op == 1 and moc == 0x3c: # adjacent
|
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
|
msglen = 9
|
||||||
msg = get_ordinals(blk[:msglen])
|
msg = get_ordinals(blk[:msglen])
|
||||||
syid = (msg >> 40) & 0xfff
|
syid = (msg >> 40) & 0xfff
|
||||||
|
@ -644,12 +703,17 @@ class trunked_system (object):
|
||||||
else:
|
else:
|
||||||
msglen = -1
|
msglen = -1
|
||||||
if self.debug > 0:
|
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
|
return msglen
|
||||||
|
|
||||||
def decode_tdma_cc(self, blk):
|
def decode_tdma_cc(self, blk):
|
||||||
rc = self.decode_tdma_blk(blk)
|
while True:
|
||||||
# TODO: Attempt to decode remaining half?
|
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):
|
def decode_tsbk_harris(self, tsbk, opcode, mfrid):
|
||||||
HARRIS_SGS_EXPIRES = 5.0 # sec.
|
HARRIS_SGS_EXPIRES = 5.0 # sec.
|
||||||
|
@ -1361,14 +1425,12 @@ class rx_ctl (object):
|
||||||
# nac is always 1st two bytes
|
# nac is always 1st two bytes
|
||||||
nac = get_ordinals(msgtext[:2])
|
nac = get_ordinals(msgtext[:2])
|
||||||
msgtext = 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():
|
if nac not in self.trunked_systems.keys():
|
||||||
sys.stderr.write('tdma_cc received from unexpected NAC 0x%x\n' % nac)
|
sys.stderr.write('tdma_cc received from unexpected NAC 0x%x\n' % nac)
|
||||||
return
|
return
|
||||||
tsys = self.trunked_systems[nac]
|
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:])
|
tsys.decode_tdma_cc(msgtext[1:])
|
||||||
return
|
return
|
||||||
elif mtype < 0:
|
elif mtype < 0:
|
||||||
|
|
|
@ -54,6 +54,7 @@ namespace gr {
|
||||||
virtual bool is_muted(void) = 0;
|
virtual bool is_muted(void) = 0;
|
||||||
virtual void set_tdma(bool) = 0;
|
virtual void set_tdma(bool) = 0;
|
||||||
virtual bool is_tdma(void) = 0;
|
virtual bool is_tdma(void) = 0;
|
||||||
|
virtual void enable_sync_plot(bool) = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace op25_repeater
|
} // namespace op25_repeater
|
||||||
|
|
|
@ -35,6 +35,7 @@
|
||||||
#include <stdexcept>
|
#include <stdexcept>
|
||||||
#include <cstdio>
|
#include <cstdio>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
#include <sys/time.h>
|
||||||
|
|
||||||
#include "p25_frame.h"
|
#include "p25_frame.h"
|
||||||
#include "p25p2_framer.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 FM_COUNT=500; // number of samples per measurement frame
|
||||||
|
|
||||||
|
static const int N_FILES = 4; // number of filenames to cycle through
|
||||||
|
|
||||||
namespace gr {
|
namespace gr {
|
||||||
namespace op25_repeater {
|
namespace op25_repeater {
|
||||||
|
|
||||||
|
@ -66,6 +69,49 @@ static inline std::complex<float> sgn(std::complex<float>c) {
|
||||||
d_event_type = c; \
|
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 gardner_costas_cc_impl::slicer(float sym) {
|
||||||
uint8_t dibit = 0;
|
uint8_t dibit = 0;
|
||||||
static const float PI_4 = M_PI / 4.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)) {
|
if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ P25_FRAME_SYNC_MAGIC, 0, 48)) {
|
||||||
// fprintf(stderr, "P25P1 Framing detect\n");
|
// fprintf(stderr, "P25P1 Framing detect\n");
|
||||||
UPDATE_COUNT(' ')
|
UPDATE_COUNT(' ')
|
||||||
|
dump_samples(0);
|
||||||
}
|
}
|
||||||
else if(check_frame_sync((nid_accum & P25P2_FRAME_SYNC_MASK) ^ P25P2_FRAME_SYNC_MAGIC, 0, 40)) {
|
else if(check_frame_sync((nid_accum & P25P2_FRAME_SYNC_MASK) ^ P25P2_FRAME_SYNC_MAGIC, 0, 40)) {
|
||||||
// fprintf(stderr, "P25P2 Framing detect\n");
|
// fprintf(stderr, "P25P2 Framing detect\n");
|
||||||
UPDATE_COUNT(' ')
|
UPDATE_COUNT(' ')
|
||||||
|
dump_samples(0);
|
||||||
}
|
}
|
||||||
if (d_is_tdma) {
|
if (d_is_tdma) {
|
||||||
if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0x000104015155LL, 0, 40)) {
|
if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0x000104015155LL, 0, 40)) {
|
||||||
fprintf(stderr, "TDMA: channel %d tuning error -1200\n", -1);
|
fprintf(stderr, "TDMA: channel %d tuning error -1200\n", -1);
|
||||||
UPDATE_COUNT('-')
|
UPDATE_COUNT('-')
|
||||||
|
dump_samples(-1200);
|
||||||
}
|
}
|
||||||
else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xfefbfeaeaaLL, 0, 40)) {
|
else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xfefbfeaeaaLL, 0, 40)) {
|
||||||
fprintf(stderr, "TDMA: channel %d tuning error +1200\n", -1);
|
fprintf(stderr, "TDMA: channel %d tuning error +1200\n", -1);
|
||||||
UPDATE_COUNT('+')
|
UPDATE_COUNT('+')
|
||||||
|
dump_samples(1200);
|
||||||
}
|
}
|
||||||
else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xa8a2a80800LL, 0, 40)) {
|
else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xa8a2a80800LL, 0, 40)) {
|
||||||
fprintf(stderr, "TDMA: channel %d tuning error +/- 2400\n", -1);
|
fprintf(stderr, "TDMA: channel %d tuning error +/- 2400\n", -1);
|
||||||
UPDATE_COUNT('|')
|
UPDATE_COUNT('|')
|
||||||
|
dump_samples(2400);
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0x001050551155LL, 0, 48)) {
|
if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0x001050551155LL, 0, 48)) {
|
||||||
// fprintf(stderr, "tuning error -1200\n");
|
// fprintf(stderr, "tuning error -1200\n");
|
||||||
UPDATE_COUNT('-')
|
UPDATE_COUNT('-')
|
||||||
|
dump_samples(-1200);
|
||||||
}
|
}
|
||||||
else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xFFEFAFAAEEAALL, 0, 48)) {
|
else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xFFEFAFAAEEAALL, 0, 48)) {
|
||||||
// fprintf(stderr, "tuning error +1200\n");
|
// fprintf(stderr, "tuning error +1200\n");
|
||||||
UPDATE_COUNT('+')
|
UPDATE_COUNT('+')
|
||||||
|
dump_samples(1200);
|
||||||
}
|
}
|
||||||
else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xAA8A0A008800LL, 0, 48)) {
|
else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xAA8A0A008800LL, 0, 48)) {
|
||||||
// fprintf(stderr, "tuning error +/- 2400\n");
|
// fprintf(stderr, "tuning error +/- 2400\n");
|
||||||
UPDATE_COUNT('|')
|
UPDATE_COUNT('|')
|
||||||
|
dump_samples(2400);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (d_event_type == ' ' || d_event_count < 5) {
|
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_event_count(0), d_event_type(' '),
|
||||||
d_symbol_seq(samples_per_symbol * 4800),
|
d_symbol_seq(samples_per_symbol * 4800),
|
||||||
d_update_request(0),
|
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_omega(samples_per_symbol);
|
||||||
set_relative_rate (1.0 / d_omega);
|
set_relative_rate (1.0 / d_omega);
|
||||||
set_history(d_twice_sps); // ensure extra input is available
|
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()
|
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_dl;
|
||||||
delete d_interp;
|
delete d_interp;
|
||||||
}
|
}
|
||||||
|
@ -245,21 +307,20 @@ gardner_costas_cc_impl::phase_error_tracking(gr_complex sample)
|
||||||
phase_error = phase_error_detector_qpsk(sample);
|
phase_error = phase_error_detector_qpsk(sample);
|
||||||
|
|
||||||
d_freq += d_beta*phase_error*abs(sample); // adjust frequency based on error
|
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
|
// Make sure we stay within +-pi
|
||||||
while(d_phase > M_TWOPI)
|
while(d_phase > M_PI)
|
||||||
d_phase -= M_TWOPI;
|
d_phase -= M_TWOPI;
|
||||||
while(d_phase < -M_TWOPI)
|
while(d_phase < -M_PI)
|
||||||
d_phase += M_TWOPI;
|
d_phase += M_TWOPI;
|
||||||
|
|
||||||
// Limit the frequency range
|
// Limit the frequency range
|
||||||
d_freq = gr::branchless_clip(d_freq, d_max_freq);
|
d_freq = gr::branchless_clip(d_freq, d_max_freq);
|
||||||
|
|
||||||
#if VERBOSE_COSTAS
|
#if VERBOSE_COSTAS
|
||||||
printf("cl: phase_error: %f phase: %f freq: %f sample: %f+j%f constellation: %f+j%f\n",
|
fprintf(stderr, "COSTAS\t%f\t%f\t%f\t%f+j%f\n",
|
||||||
phase_error, d_phase, d_freq, sample.real(), sample.imag(),
|
phase_error, d_phase, d_freq, sample.real(), sample.imag());
|
||||||
d_constellation[d_current_const_point].real(), d_constellation[d_current_const_point].imag());
|
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -277,21 +338,33 @@ gardner_costas_cc_impl::general_work (int noutput_items,
|
||||||
gr_complex interp_samp, interp_samp_mid, diffdec;
|
gr_complex interp_samp, interp_samp_mid, diffdec;
|
||||||
float error_real, error_imag, symbol_error;
|
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((o < noutput_items) && (i < ninput_items[0])) {
|
||||||
while((d_mu > 1.0) && (i < ninput_items[0])) {
|
while((d_mu > 1.0) && (i < ninput_items[0])) {
|
||||||
d_mu --;
|
d_mu --;
|
||||||
|
|
||||||
d_phase += d_freq;
|
d_phase += d_freq;
|
||||||
// Keep phase clamped and not walk to infinity
|
// Keep phase clamped and not walk to infinity
|
||||||
while(d_phase > M_TWOPI)
|
while(d_phase > M_PI)
|
||||||
d_phase -= M_TWOPI;
|
d_phase -= M_TWOPI;
|
||||||
while(d_phase < -M_TWOPI)
|
while(d_phase < -M_PI)
|
||||||
d_phase += M_TWOPI;
|
d_phase += M_TWOPI;
|
||||||
|
|
||||||
nco = gr_expj(d_phase+d_theta); // get the NCO value for derotating the curr
|
nco = gr_expj(d_phase+d_theta); // get the NCO value for derotating the curr
|
||||||
symbol = in[i];
|
symbol = in[i];
|
||||||
sample = nco*symbol; // get the downconverted symbol
|
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] = sample;
|
||||||
d_dl[d_dl_index + d_twice_sps] = sample;
|
d_dl[d_dl_index + d_twice_sps] = sample;
|
||||||
d_dl_index ++;
|
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 + 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
|
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
|
#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
|
#endif
|
||||||
} else {
|
} else {
|
||||||
symbol_error = 0;
|
symbol_error = 0;
|
||||||
|
|
|
@ -78,6 +78,7 @@ namespace gr {
|
||||||
inline bool is_muted(void) { return d_muted; }
|
inline bool is_muted(void) { return d_muted; }
|
||||||
inline void set_tdma(bool v) { d_is_tdma = v; }
|
inline void set_tdma(bool v) { d_is_tdma = v; }
|
||||||
inline bool is_tdma(void) { return d_is_tdma; }
|
inline bool is_tdma(void) { return d_is_tdma; }
|
||||||
|
inline void enable_sync_plot(bool v) { d_enable_sync_plot = v; }
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
bool input_sample0(gr_complex, gr_complex& outp);
|
bool input_sample0(gr_complex, gr_complex& outp);
|
||||||
|
@ -121,9 +122,18 @@ protected:
|
||||||
int d_fm_count;
|
int d_fm_count;
|
||||||
bool d_muted;
|
bool d_muted;
|
||||||
bool d_is_tdma;
|
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);
|
float phase_error_detector_qpsk(gr_complex sample);
|
||||||
void phase_error_tracking(gr_complex sample);
|
void phase_error_tracking(gr_complex sample);
|
||||||
|
void dump_samples(int);
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace op25_repeater
|
} // namespace op25_repeater
|
||||||
|
|
|
@ -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)
|
// Copyright 2017 Graham J. Norbury (modularization rewrite)
|
||||||
//
|
//
|
||||||
// This file is part of OP25
|
// This file is part of OP25
|
||||||
|
@ -38,6 +38,7 @@
|
||||||
|
|
||||||
static const int BURST_SIZE = 180;
|
static const int BURST_SIZE = 180;
|
||||||
static const int SUPERFRAME_SIZE = (12*BURST_SIZE);
|
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) {
|
static uint16_t crc12(const uint8_t bits[], unsigned int len) {
|
||||||
uint16_t crc=0;
|
uint16_t crc=0;
|
||||||
|
@ -70,24 +71,6 @@ static bool crc12_ok(const uint8_t bits[], unsigned int len) {
|
||||||
return (crc == crc12(bits,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<int16_t> &qptr, bool do_audio_output, int msgq_id) : // constructor
|
p25p2_tdma::p25p2_tdma(const op25_audio& udp, int slotid, int debug, bool do_msgq, gr::msg_queue::sptr queue, std::deque<int16_t> &qptr, bool do_audio_output, int msgq_id) : // constructor
|
||||||
op25audio(udp),
|
op25audio(udp),
|
||||||
write_bufp(0),
|
write_bufp(0),
|
||||||
|
@ -122,6 +105,7 @@ bool p25p2_tdma::rx_sym(uint8_t sym)
|
||||||
|
|
||||||
void p25p2_tdma::set_slotid(int slotid)
|
void p25p2_tdma::set_slotid(int slotid)
|
||||||
{
|
{
|
||||||
|
memset(tdma_xormask, 0, SUPERFRAME_SIZE);
|
||||||
assert (slotid == 0 || slotid == 1);
|
assert (slotid == 0 || slotid == 1);
|
||||||
d_slotid = slotid;
|
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 opcode = (byte_buf[0] >> 5) & 0x7;
|
||||||
unsigned int offset = (byte_buf[0] >> 2) & 0x7;
|
unsigned int offset = (byte_buf[0] >> 2) & 0x7;
|
||||||
|
bool my_slot = (which_slot[sync.tdma_slotid()] == d_slotid);
|
||||||
|
|
||||||
if (d_debug >= 10) {
|
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)
|
switch (opcode)
|
||||||
{
|
{
|
||||||
case 0: // MAC_SIGNAL
|
case 0: // MAC_SIGNAL
|
||||||
handle_mac_signal(byte_buf, len);
|
handle_mac_signal(byte_buf, len);
|
||||||
|
return -1;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 1: // MAC_PTT
|
case 1: // MAC_PTT
|
||||||
|
@ -161,20 +163,22 @@ int p25p2_tdma::process_mac_pdu(const uint8_t byte_buf[], const unsigned int len
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 3: // MAC_IDLE
|
case 3: // MAC_IDLE
|
||||||
handle_mac_idle(byte_buf, len);
|
op25audio.send_audio_flag(op25_audio::DRAIN);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 4: // MAC_ACTIVE
|
case 4: // MAC_ACTIVE
|
||||||
handle_mac_active(byte_buf, len);
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 6: // MAC_HANGTIME
|
case 6: // MAC_HANGTIME
|
||||||
handle_mac_hangtime(byte_buf, len);
|
|
||||||
op25audio.send_audio_flag(op25_audio::DRAIN);
|
op25audio.send_audio_flag(op25_audio::DRAIN);
|
||||||
break;
|
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
|
// 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];
|
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);
|
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 p25p2_tdma::handle_acch_frame(const uint8_t dibits[], bool fast, bool is_lcch)
|
||||||
{
|
{
|
||||||
int i, j, rc;
|
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);
|
bool crc_ok = (is_lcch) ? (crc16(bits, len) == 0) : crc12_ok(bits, len);
|
||||||
int olen = (is_lcch) ? 23 : len/8;
|
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;
|
rc = -1;
|
||||||
if (crc_ok) { // TODO: rewrite crc12 so we don't have to do so much bit manipulation
|
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<olen; i++) {
|
for (int i=0; i<olen; i++) {
|
||||||
byte_buf[i] = (bits[i*8 + 0] << 7) + (bits[i*8 + 1] << 6) + (bits[i*8 + 2] << 5) + (bits[i*8 + 3] << 4) + (bits[i*8 + 4] << 3) + (bits[i*8 + 5] << 2) + (bits[i*8 + 6] << 1) + (bits[i*8 + 7] << 0);
|
byte_buf[i] = (bits[i*8 + 0] << 7) + (bits[i*8 + 1] << 6) + (bits[i*8 + 2] << 5) + (bits[i*8 + 3] << 4) + (bits[i*8 + 4] << 3) + (bits[i*8 + 5] << 2) + (bits[i*8 + 6] << 1) + (bits[i*8 + 7] << 0);
|
||||||
}
|
}
|
||||||
rc = process_mac_pdu(byte_buf, olen);
|
rc = process_mac_pdu(byte_buf, olen);
|
||||||
} else {
|
|
||||||
// fprintf(stderr, "crc12 failed\n");
|
|
||||||
}
|
}
|
||||||
return rc;
|
return rc;
|
||||||
}
|
}
|
||||||
|
@ -537,16 +384,14 @@ int p25p2_tdma::handle_frame(void)
|
||||||
int p25p2_tdma::handle_packet(const uint8_t dibits[])
|
int p25p2_tdma::handle_packet(const uint8_t dibits[])
|
||||||
{
|
{
|
||||||
int rc = -1;
|
int rc = -1;
|
||||||
static const int which_slot[] = {0,1,0,1,0,1,0,1,0,1,1,0};
|
|
||||||
packets++;
|
packets++;
|
||||||
sync.check_confidence(dibits);
|
sync.check_confidence(dibits);
|
||||||
if (!sync.in_sync())
|
if (!sync.in_sync())
|
||||||
return -1;
|
return -1;
|
||||||
const uint8_t* burstp = &dibits[10];
|
const uint8_t* burstp = &dibits[10];
|
||||||
uint8_t xored_burst[BURST_SIZE - 10];
|
uint8_t xored_burst[BURST_SIZE - 10];
|
||||||
|
bool my_slot = (which_slot[sync.tdma_slotid()] == d_slotid);
|
||||||
int burst_type = duid.duid_lookup(duid.extract_duid(burstp));
|
int burst_type = duid.duid_lookup(duid.extract_duid(burstp));
|
||||||
if (which_slot[sync.tdma_slotid()] != d_slotid && burst_type != 13) // ignore if on oppo. slot and not CC
|
|
||||||
return -1;
|
|
||||||
for (int i=0; i<BURST_SIZE - 10; i++) {
|
for (int i=0; i<BURST_SIZE - 10; i++) {
|
||||||
xored_burst[i] = burstp[i] ^ tdma_xormask[sync.tdma_slotid() * BURST_SIZE + i];
|
xored_burst[i] = burstp[i] ^ tdma_xormask[sync.tdma_slotid() * BURST_SIZE + i];
|
||||||
}
|
}
|
||||||
|
@ -554,6 +399,8 @@ int p25p2_tdma::handle_packet(const uint8_t dibits[])
|
||||||
fprintf(stderr, "%s TDMA burst type=%d\n", logts.get(), burst_type);
|
fprintf(stderr, "%s TDMA burst type=%d\n", logts.get(), burst_type);
|
||||||
}
|
}
|
||||||
if (burst_type == 0 || burst_type == 6) { // 4V or 2V burst
|
if (burst_type == 0 || burst_type == 6) { // 4V or 2V burst
|
||||||
|
if (!my_slot) // ignore if on oppo. slot
|
||||||
|
return -1;
|
||||||
track_vb(burst_type);
|
track_vb(burst_type);
|
||||||
handle_4V2V_ess(&xored_burst[84]);
|
handle_4V2V_ess(&xored_burst[84]);
|
||||||
if ( !encrypted() ) {
|
if ( !encrypted() ) {
|
||||||
|
|
|
@ -96,7 +96,6 @@ private:
|
||||||
void handle_mac_idle(const uint8_t byte_buf[], const unsigned int len) ;
|
void handle_mac_idle(const uint8_t byte_buf[], const unsigned int len) ;
|
||||||
void handle_mac_active(const uint8_t byte_buf[], const unsigned int len) ;
|
void handle_mac_active(const uint8_t byte_buf[], const unsigned int len) ;
|
||||||
void handle_mac_hangtime(const uint8_t byte_buf[], const unsigned int len) ;
|
void handle_mac_hangtime(const uint8_t byte_buf[], const unsigned int len) ;
|
||||||
void decode_mac_msg(const uint8_t byte_buf[], const unsigned int len) ;
|
|
||||||
void handle_4V2V_ess(const uint8_t dibits[]);
|
void handle_4V2V_ess(const uint8_t dibits[]);
|
||||||
inline bool encrypted() { return (ess_algid != 0x80); }
|
inline bool encrypted() { return (ess_algid != 0x80); }
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue