add correlation plot

op25/gr-op25_repeater/apps/gr_gnuplot.py

@@ -23,6 +23,7 @@ import sys
 import os
 import time
 import subprocess
+import json
 
 from gnuradio import gr, gru, eng_notation
 from gnuradio import blocks, audio
@@ -54,8 +55,11 @@ def limit(a,lim):
 		return lim
 	return a
 
+PSEQ = 0
+
 class wrap_gp(object):
-	def __init__(self, sps=_def_sps, logfile=None):
+	def __init__(self, sps=_def_sps, logfile=None, title=None):
+		global PSEQ
 		self.sps = sps
 		self.center_freq = 0.0
 		self.relative_freq = 0.0
@@ -65,7 +69,7 @@ class wrap_gp(object):
 		self.freqs = ()
 		self.avg_pwr = np.zeros(FFT_BINS)
 		self.avg_sum_pwr = 0.0
-		self.buf = []
+		self.buf = np.array([])
 		self.plot_count = 0
 		self.last_plot = 0
 		self.plot_interval = None
@@ -73,6 +77,12 @@ class wrap_gp(object):
 		self.output_dir = None
 		self.filename = None
 		self.logfile = logfile
+		self.title = title
+		self.sequence_id = PSEQ
+		PSEQ += 1
+		x = self.sequence_id % 3
+		y = self.sequence_id // 3
+		self.position = (x, y)
 
 		self.attach_gp()
 
@@ -110,77 +120,70 @@ class wrap_gp(object):
 		BUFSZ = bufsz
 		consumed = min(len(buf), BUFSZ-len(self.buf))
 		if len(self.buf) < BUFSZ:
-			self.buf.extend(buf[:consumed])
+			self.buf = np.concatenate((self.buf, buf[:int(consumed)]))
 		if len(self.buf) < BUFSZ:
 			return consumed
 
 		self.plot_count += 1
 		if mode == 'eye' and self.plot_count % 20 != 0:
-			self.buf = []
+			self.buf = np.array([])
 			return consumed
 
 		plots = []
 		s = ''
 		plot_size = (320,240)
-		while(len(self.buf)):
+		if mode == 'eye':
-			if mode == 'eye':
+			nplots = len(self.buf) // self.sps - 2
-				if len(self.buf) < self.sps:
+			for i in range(nplots):
-					break
+				s += '\n'.join(['%f' % self.buf[i*self.sps+j] for j in range(self.sps+1)])
-				for i in range(self.sps):
+				s += '\ne\n'
-					s += '%f\n' % self.buf[i]
-				s += 'e\n'
-				self.buf=self.buf[self.sps:]
 				plots.append('"-" with lines')
-			elif mode == 'constellation':
+		elif mode == 'constellation':
-				plot_size = (240,240)
+			plot_size = (240,240)
-				self.buf = self.buf[:100]
+			self.buf = self.buf[:100]
-				for b in self.buf:
+			for b in self.buf:
-					s += '%f\t%f\n' % (degrees(np.angle(b)), limit(np.abs(b),1.0))
+				s += '%f\t%f\n' % (degrees(np.angle(b)), limit(np.abs(b),1.0))
-				s += 'e\n'
+			s += 'e\n'
-				plots.append('"-" with points')
+			plots.append('"-" with points')
-				for b in self.buf:
+			for b in self.buf:
-					#s += '%f\t%f\n' % (b.real, b.imag)
+				#s += '%f\t%f\n' % (b.real, b.imag)
-					s += '%f\t%f\n' % (degrees(np.angle(b)), limit(np.abs(b),1.0))
+				s += '%f\t%f\n' % (degrees(np.angle(b)), limit(np.abs(b),1.0))
-				s += 'e\n'
+			s += 'e\n'
-				self.buf = []
+			plots.append('"-" with lines')
-				plots.append('"-" with lines')
+		elif mode == 'symbol':
-			elif mode == 'symbol':
+			for b in self.buf:
-				for b in self.buf:
+				s += '%f\n' % (b)
-					s += '%f\n' % (b)
+			s += 'e\n'
-				s += 'e\n'
+			plots.append('"-" with points')
-				self.buf = []
+		elif mode == 'fft' or mode == 'mixer':
-				plots.append('"-" with points')
+			sum_pwr = 0.0
-			elif mode == 'fft' or mode == 'mixer':
+			self.ffts = np.fft.fft(self.buf * np.blackman(BUFSZ)) / (0.42 * BUFSZ)
-				sum_pwr = 0.0
+			self.ffts = np.fft.fftshift(self.ffts)
-				self.ffts = np.fft.fft(self.buf * np.blackman(BUFSZ)) / (0.42 * BUFSZ)
+			self.freqs = np.fft.fftfreq(len(self.ffts))
-				self.ffts = np.fft.fftshift(self.ffts)
+			self.freqs = np.fft.fftshift(self.freqs)
-				self.freqs = np.fft.fftfreq(len(self.ffts))
+			tune_freq = (self.center_freq - self.relative_freq) / 1e6
-				self.freqs = np.fft.fftshift(self.freqs)
+			if self.center_freq and self.width:
-				tune_freq = (self.center_freq - self.relative_freq) / 1e6
+                               	self.freqs = ((self.freqs * self.width) + self.center_freq + self.offset_freq) / 1e6
-				if self.center_freq and self.width:
+			for i in range(len(self.ffts)):
-                                	self.freqs = ((self.freqs * self.width) + self.center_freq + self.offset_freq) / 1e6
+				if mode == 'fft':
-				for i in range(len(self.ffts)):
+					self.avg_pwr[i] = ((1.0 - FFT_AVG) * self.avg_pwr[i]) + (FFT_AVG * np.abs(self.ffts[i]))
-					if mode == 'fft':
+				else:
-						self.avg_pwr[i] = ((1.0 - FFT_AVG) * self.avg_pwr[i]) + (FFT_AVG * np.abs(self.ffts[i]))
+					self.avg_pwr[i] = ((1.0 - MIX_AVG) * self.avg_pwr[i]) + (MIX_AVG * np.abs(self.ffts[i]))
-					else:
+				s += '%f\t%f\n' % (self.freqs[i], 20 * np.log10(self.avg_pwr[i]))
-						self.avg_pwr[i] = ((1.0 - MIX_AVG) * self.avg_pwr[i]) + (MIX_AVG * np.abs(self.ffts[i]))
+				if (mode == 'mixer') and (self.avg_pwr[i] > 1e-5):
-					s += '%f\t%f\n' % (self.freqs[i], 20 * np.log10(self.avg_pwr[i]))
+					if (self.freqs[i] - self.center_freq) < 0:
-					if (mode == 'mixer') and (self.avg_pwr[i] > 1e-5):
+						sum_pwr -= self.avg_pwr[i]
-						if (self.freqs[i] - self.center_freq) < 0:
+					elif (self.freqs[i] - self.center_freq) > 0:
-							sum_pwr -= self.avg_pwr[i]
+						sum_pwr += self.avg_pwr[i]
-						elif (self.freqs[i] - self.center_freq) > 0:
+					self.avg_sum_pwr = ((1.0 - BAL_AVG) * self.avg_sum_pwr) + (BAL_AVG * sum_pwr)
-							sum_pwr += self.avg_pwr[i]
+			s += 'e\n'
-						self.avg_sum_pwr = ((1.0 - BAL_AVG) * self.avg_sum_pwr) + (BAL_AVG * sum_pwr)
+			plots.append('"-" with lines')
-				s += 'e\n'
+		elif mode == 'float' or mode == 'correlation':
-				self.buf = []
+			for b in self.buf:
-				plots.append('"-" with lines')
+				s += '%f\n' % (b)
-			elif mode == 'float':
+			s += 'e\n'
-				for b in self.buf:
+			plots.append('"-" with lines')
-					s += '%f\n' % (b)
+		self.buf = np.array([])
-				s += 'e\n'
-				self.buf = []
-				plots.append('"-" with lines')
-		self.buf = []
 
 		# FFT processing needs to be completed to maintain the weighted average buckets
 		# regardless of whether we actually produce a new plot or not.
@@ -191,15 +194,22 @@ class wrap_gp(object):
 		filename = None
 		if self.output_dir:
 			if self.sequence >= 2:
-				delete_pathname = '%s/plot-%s-%d.png' % (self.output_dir, mode, self.sequence-2)
+				delete_pathname = '%s/plot-%s%d-%d.png' % (self.output_dir, mode, self.sequence_id, self.sequence-2)
 				if os.access(delete_pathname, os.W_OK):
 					os.remove(delete_pathname)
 			h0= 'set terminal png size %d, %d\n' % (plot_size)
-			filename = 'plot-%s-%d.png' % (mode, self.sequence)
+			filename = 'plot-%s%d-%d.png' % (mode, self.sequence_id, self.sequence)
 			h0 += 'set output "%s/%s"\n' % (self.output_dir, filename)
 			self.sequence += 1
 		else:
-			h0= 'set terminal x11 noraise\n'
+			pos = ''
+			if self.position is not None:
+				x = self.position[0] * plot_size[0]
+				y = self.position[1] * plot_size[1]
+				x += 50
+				y += 75
+				pos = ' position %d, %d' % (x, y)
+			h0= 'set terminal x11 noraise size %d, %d%s title "%s"\n' % (plot_size[0], plot_size[1], pos, self.title)
 		background = ''
 		h = 'set key off\n'
 		if mode == 'constellation':
@@ -250,12 +260,18 @@ class wrap_gp(object):
 		elif mode == 'float':
 			h+= 'set yrange [-2:2]\n'
 			h+= 'set title "Oscilloscope"\n'
+		elif mode == 'correlation':
+			title = 'Correlation'
+			if self.title:
+				title = self.title
+			h+= 'set yrange [-1.1:1.1]\n'
+			h+= 'set title "%s"\n' % (title)
 		dat = '%s%splot %s\n%s' % (h0, h, ','.join(plots), s)
-		if sys.version[0] != '2':
-			dat = bytes(dat, 'utf8')
 		if self.logfile is not None:
 			with open(self.logfile, 'a') as fd:
 				fd.write(dat)
+		if sys.version[0] != '2':
+			dat = bytes(dat, 'utf8')
 		self.gp.poll()
 		if self.gp.returncode is None:	# make sure gnuplot is still running 
 			try:
@@ -281,6 +297,9 @@ class wrap_gp(object):
 	def set_logfile(self, logfile=None):
 		self.logfile = logfile
 
+	def set_title(self, title):
+		self.title = title
+
 class eye_sink_f(gr.sync_block):
     """
     """
@@ -295,7 +314,7 @@ class eye_sink_f(gr.sync_block):
 
     def work(self, input_items, output_items):
         in0 = input_items[0]
-        consumed = self.gnuplot.plot(in0, 100 * self.sps, mode='eye')
+        consumed = self.gnuplot.plot(in0, 100*self.sps, mode='eye')
         return consumed ### len(input_items[0])
 
     def kill(self):
@@ -416,3 +435,77 @@ class float_sink_f(gr.sync_block):
 
     def kill(self):
         self.gnuplot.kill()
+
+class correlation_sink_f(gr.sync_block):
+    """
+    """
+    def __init__(self, sps=_def_sps, debug = _def_debug):
+        gr.sync_block.__init__(self,
+            name="plot_sink_f",
+            in_sig=[np.float32],
+            out_sig=None)
+        self.debug = debug
+        self.sps = sps
+        self.gnuplot = wrap_gp()
+        self.fs = []
+        self.cbuf = np.array([])
+        self.ignore = 0
+        self.pktlen = 1024
+
+    def set_length(self, l):
+        self.pktlen = l
+
+    def set_title(self, title):
+        self.gnuplot.set_title(title)
+
+    def set_signature(self, fs):
+        self.fs = []
+        for s in fs:
+            for i in range(self.sps):
+                self.fs.append(s)
+        self.fs.reverse()	# reverse order for np.convolve
+        self.fs = np.array(self.fs)
+
+    def work(self, input_items, output_items):
+        if len(self.cbuf) == 0 and self.ignore > 0:
+            self.ignore -= len(input_items[0])
+            if self.ignore < 0:
+                self.ignore = 0
+            return len(input_items[0])
+        if len(self.fs) == 0:
+            return len(input_items[0])
+        in0 = input_items[0]
+        self.cbuf = np.append(self.cbuf, in0)
+        if len(self.cbuf) < self.pktlen:
+            return len(input_items[0])
+        result = np.convolve(self.cbuf[:self.pktlen], self.fs)
+        hi = np.max(np.abs(result))
+        if hi != 0:
+            result = result / hi
+        self.cbuf = []
+        self.ignore = 3000 * self.sps
+        self.gnuplot.plot(result, len(result), mode='correlation')
+        return len(input_items[0])
+
+    def kill(self):
+        self.gnuplot.kill()
+
+def setup_correlation(sps, title, connect_bb):
+    CFG_FILE = 'correlation.json'
+    if not os.access(CFG_FILE, os.R_OK):
+        sys.stderr.write('correlation plot ignored, missing config file %s\n' % CFG_FILE)
+        return []
+    ccfg = json.loads(open(CFG_FILE).read())
+    sinks = []
+    for cfg in ccfg:
+        sink = correlation_sink_f(sps=sps)
+        sink.set_title('%s %s' % (title, cfg['name']))
+        l = cfg['length'] * sps * 4
+        LENGTH_LIMIT = 10000
+        if l > LENGTH_LIMIT:
+            l = LENGTH_LIMIT
+        sink.set_length(l)
+        sink.set_signature(cfg['fs'])
+        connect_bb('baseband_amp', sink)
+        sinks.append(sink)
+    return sinks

op25/gr-op25_repeater/apps/multi_rx.py

@@ -42,6 +42,7 @@ from gr_gnuplot import fft_sink_c
 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 nxdn_trunking import cac_message
 
@@ -193,6 +194,13 @@ class channel(object):
                 self.sinks.append(constellation_sink_c())
                 self.demod.connect_complex('diffdec', self.sinks[i])
                 self.kill_sink.append(self.sinks[i])
+            elif plot == 'correlation':
+                assert config['demod_type'] == 'fsk4'   ## correlation plot requires fsk4 demod type
+                assert config['symbol_rate'] == 4800	## 4800 required for correlation plot
+                sps=config['if_rate'] // self.symbol_rate
+                sinks = setup_correlation(sps, self.name, self.demod.connect_bb)
+                self.kill_sink += sinks
+                self.sinks += sinks
             else:
                 sys.stderr.write('unrecognized plot type %s\n' % plot)
                 return

op25/gr-op25_repeater/apps/p25_demodulator.py

@@ -88,7 +88,7 @@ class p25_demod_base(gr.hier_block2):
 	"""
         self.if_rate = if_rate
         self.symbol_rate = symbol_rate
-        self.bb_sink = None
+        self.bb_sinks = []
 
         self.baseband_amp = blocks.multiply_const_ff(_def_bb_gain)
         coeffs = op25_c4fm_mod.c4fm_taps(sample_rate=self.if_rate, span=9, generator=op25_c4fm_mod.transfer_function_rx).generate()
@@ -145,20 +145,20 @@ class p25_demod_base(gr.hier_block2):
 
     def disconnect_bb(self):
         # assumes lock held or init
-        if not self.bb_sink:
+        if not len(self.bb_sinks):
             return
-        self.disconnect(self.bb_sink[0], self.bb_sink[1])
+        for t in self.bb_sinks:
-        self.bb_sink = None
+            self.disconnect(t[0], t[1])
+        self.bb_sinks = []
 
     def connect_bb(self, src, sink):
         # assumes lock held or init
-        self.disconnect_bb()
         if src == 'symbol_filter':
-            self.connect(self.symbol_filter, sink)
+            b = self.symbol_filter
-            self.bb_sink = [self.symbol_filter, sink]
         elif src == 'baseband_amp':
-            self.connect(self.baseband_amp, sink)
+            b = self.baseband_amp
-            self.bb_sink = [self.baseband_amp, sink]
+        self.connect(b, sink)
+        self.bb_sinks.append([b, sink])
 
 class p25_demod_fb(p25_demod_base):
 

op25/gr-op25_repeater/apps/rx.py

@@ -65,6 +65,7 @@ from gr_gnuplot import fft_sink_c
 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 terminal import op25_terminal
 from sockaudio  import socket_audio
@@ -267,26 +268,34 @@ class p25_rx_block (gr.top_block):
             if plot_mode == 'constellation':
                 assert self.options.demod_type == 'cqpsk'  ## constellation requires cqpsk demod-type
                 sink = constellation_sink_c()
+                self.plot_sinks.append(sink)
                 self.demod.connect_complex('diffdec', sink)
             elif plot_mode == 'symbol':
                 sink = symbol_sink_f()
+                self.plot_sinks.append(sink)
                 self.demod.connect_float(sink)
             elif plot_mode == 'fft':
                 sink = fft_sink_c()
+                self.plot_sinks.append(sink)
                 self.spectrum_decim = filter.rational_resampler_ccf(1, self.options.decim_amt)
                 self.connect(self.spectrum_decim, sink)
                 self.demod.connect_complex('src', self.spectrum_decim)
             elif plot_mode == 'mixer':
                 sink = mixer_sink_c()
+                self.plot_sinks.append(sink)
                 self.demod.connect_complex('mixer', sink)
             elif plot_mode == 'datascope':
                 assert self.options.demod_type == 'fsk4'  ## datascope requires fsk4 demod-type
                 sink = eye_sink_f(sps=sps)
+                self.plot_sinks.append(sink)
                 self.demod.connect_bb('symbol_filter', sink)
+            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)
             else:
                 raise ValueError('unsupported plot type: %s' % plot_mode)
-            self.plot_sinks.append(sink)
+        if self.is_http_term():
-            if self.is_http_term():
+            for sink in self.plot_sinks:
                 sink.gnuplot.set_interval(_def_interval)
                 sink.gnuplot.set_output_dir(_def_file_dir)