Add RDS demodulation stuff to pyfm.py.
This commit is contained in:
parent
ca411c57e1
commit
640c75038b
456
pyfm.py
456
pyfm.py
|
|
@ -1,8 +1,18 @@
|
|||
"""
|
||||
Test lab for FM decoding algorithms.
|
||||
|
||||
Use as follows:
|
||||
|
||||
>>> graw = pyfm.lazyRawSamples('rtlsdr.dat', 1000000)
|
||||
>>> gtune = pyfm.freqShiftIQ(graw, 0.25)
|
||||
>>> bfir = scipy.signal.firwin(20, 0.2, window='nuttall')
|
||||
>>> gfilt = pyfm.firFilter(gtune, bfir)
|
||||
>>> gbase = pyfm.quadratureDetector(gfilt, fs=1.0e6)
|
||||
>>> fs,qs = pyfm.spectrum(gbase, fs=1.0e6)
|
||||
"""
|
||||
|
||||
import sys
|
||||
import datetime
|
||||
import types
|
||||
import numpy
|
||||
import numpy.fft
|
||||
|
|
@ -11,6 +21,20 @@ import numpy.random
|
|||
import scipy.signal
|
||||
|
||||
|
||||
def sincw(n):
|
||||
"""Return Sinc or Lanczos window of length n."""
|
||||
|
||||
w = numpy.zeros(n)
|
||||
for i in xrange(n):
|
||||
if 2 * i == n + 1:
|
||||
w[i] = 1.0
|
||||
else:
|
||||
t = 2 * i / float(n+1) - 1
|
||||
w[i] = numpy.sin(numpy.pi * t) / (numpy.pi * t)
|
||||
|
||||
return w
|
||||
|
||||
|
||||
def readRawSamples(fname):
|
||||
"""Read raw sample file from rtl_sdr."""
|
||||
|
||||
|
|
@ -364,3 +388,435 @@ def modulateAndReconstruct(sigfreq, sigampl, nsampl, fs, noisebw=None, ifbw=None
|
|||
|
||||
return ampl1, phase1, noise1
|
||||
|
||||
|
||||
def rdsDemodulate(d, fs):
|
||||
"""Demodulate RDS bit stream.
|
||||
|
||||
d :: block of baseband samples or lazy baseband sample stream
|
||||
fs :: sample frequency in Hz
|
||||
|
||||
Return (bits, levels)
|
||||
where bits is a list of RDS data bits
|
||||
levels is a list of squared RDS carrier amplitudes
|
||||
"""
|
||||
|
||||
# RDS carrier in Hz
|
||||
carrier = 57000.0
|
||||
|
||||
# RDS bit rate in bit/s
|
||||
bitrate = 1187.5
|
||||
|
||||
# Approximate nr of samples per bit.
|
||||
bitsteps = round(fs / bitrate)
|
||||
|
||||
# Prepare FIR coefficients for matched filter.
|
||||
#
|
||||
# The filter is a root-raised-cosine with hard cutoff at f = 2/bitrate.
|
||||
# H(f) = cos(pi * f / (4*bitrate)) if f < 2*bitrate
|
||||
# H(f) = 0 if f >= 2*bitrate
|
||||
#
|
||||
# Impulse response:
|
||||
# h(t) = ampl * cos(pi*4*bitrate*t) / (1 - 4 * (4*bitrate*t)**2)
|
||||
#
|
||||
wlen = int(1.5 * fs / bitrate)
|
||||
w = numpy.zeros(wlen)
|
||||
for i in xrange(wlen):
|
||||
t = (i - 0.5 * (wlen - 1)) * 4.0 * bitrate / fs
|
||||
if abs(abs(t) - 0.5) < 1.0e-4:
|
||||
# lim {t->0.5} {cos(pi*t) / (1 - 4*t**2)} = 0.25 * pi
|
||||
w[i] = 0.25 * numpy.pi - 0.25 * numpy.pi * (abs(t) - 0.5)
|
||||
else:
|
||||
w[i] = numpy.cos(numpy.pi * t) / (1 - 4.0 * t * t)
|
||||
|
||||
# Use Sinc window to reduce leakage.
|
||||
w *= sincw(wlen)
|
||||
|
||||
# Scale filter such that peak output of filter equals original amplitude.
|
||||
w /= numpy.sum(w**2)
|
||||
|
||||
demod_phase = 0.0
|
||||
prev_a1 = 0.0
|
||||
|
||||
prevb = numpy.array([])
|
||||
pos = 0
|
||||
|
||||
bits = [ ]
|
||||
levels = [ ]
|
||||
|
||||
if not isinstance(d, types.GeneratorType):
|
||||
d = [ d ]
|
||||
|
||||
for b in d:
|
||||
|
||||
n = len(b)
|
||||
|
||||
# I/Q demodulate with fixed 57 kHz phasor
|
||||
ps = numpy.arange(n) * (carrier / float(fs)) + demod_phase
|
||||
dem = b * numpy.exp(-2j * numpy.pi * ps)
|
||||
demod_phase = (demod_phase + n * carrier / float(fs)) % 1.0
|
||||
|
||||
# Merge with remaining data from previous block
|
||||
prevb = numpy.concatenate((prevb[pos:], dem))
|
||||
pos = 0
|
||||
|
||||
# Detect bits.
|
||||
while pos + bitsteps + wlen < len(prevb):
|
||||
|
||||
# Measure average phase of first impulse of symbol.
|
||||
a1 = numpy.sum(prevb[pos:pos+wlen] * w)
|
||||
|
||||
# Measure average phase of second impulse of symbol.
|
||||
a2 = numpy.sum(prevb[pos+bitsteps//2:pos+wlen+bitsteps//2] * w)
|
||||
|
||||
# Measure average phase in middle of symbol.
|
||||
a3 = numpy.sum(prevb[pos+bitsteps//4:pos+wlen+bitsteps//4] * w)
|
||||
|
||||
# Calculate inner product of first impulse and previous symbol.
|
||||
sym = a1.real * prev_a1.real + a1.imag * prev_a1.imag
|
||||
prev_a1 = a1
|
||||
|
||||
if sym < 0:
|
||||
# Consecutive symbols have opposite phase; this is a 1 bit.
|
||||
bits.append(1)
|
||||
else:
|
||||
# Consecutive symbols are in phase; this is a 0 bit.
|
||||
bits.append(0)
|
||||
|
||||
# Calculate inner product of first and second impulse.
|
||||
a1a2 = a1.real * a2.real + a1.imag * a2.imag
|
||||
|
||||
# Calculate inner product of first impulse and middle phasor.
|
||||
a1a3 = a1.real * a3.real + a1.imag * a3.imag
|
||||
|
||||
levels.append(-a1a2)
|
||||
|
||||
if a1a2 >= 0:
|
||||
# First and second impulse are in phase;
|
||||
# we must be woefully misaligned.
|
||||
pos += 5 * bitsteps // 8
|
||||
elif a1a3 > -0.02 * a1a2:
|
||||
# Middle phasor is in phase with first impulse;
|
||||
# we are sampling slightly too early.
|
||||
pos += (102 * bitsteps) // 100
|
||||
elif a1a3 > -0.01 * a1a2:
|
||||
pos += (101 * bitsteps) // 100
|
||||
elif a1a3 < 0.02 * a1a2:
|
||||
# Middle phasor is opposite to first impulse;
|
||||
# we are sampling slightly too late.
|
||||
pos += (98 * bitsteps) // 100
|
||||
elif a1a3 < 0.01 * a1a2:
|
||||
pos += (99 * bitsteps) // 100
|
||||
else:
|
||||
# Middle phasor is zero; we are sampling just right.
|
||||
pos += bitsteps
|
||||
|
||||
return (bits, levels)
|
||||
|
||||
|
||||
def rdsDecodeBlock(bits, typ):
|
||||
"""Decode one RDS data block.
|
||||
|
||||
bits :: list of 26 bits
|
||||
typ :: expected block type, "A" or "B" or "C" or "C'" or "D" or "E"
|
||||
|
||||
Return (block, ber)
|
||||
where block is a 16-bit unsigned integer if the block is correctly decoded,
|
||||
block is None if decoding failed,
|
||||
ber is 0 if the block is error-free,
|
||||
ber is 1 if a single-bit error has been corrected,
|
||||
ber is 2 if decoding failed.
|
||||
"""
|
||||
|
||||
# TODO : there are still problems with bit alignment on weak stations
|
||||
# TODO : try to pin down the problem
|
||||
|
||||
# Offset word for each type of block.
|
||||
rdsOffsetTable = { "A": 0x0fc, "B": 0x198,
|
||||
"C": 0x168, "C'": 0x350,
|
||||
"D": 0x1B4, "E": 0 }
|
||||
|
||||
# RDS checkword generator polynomial.
|
||||
gpoly = 0x5B9
|
||||
|
||||
# Convert bits to word.
|
||||
assert len(bits) == 26
|
||||
w = 0
|
||||
for b in bits:
|
||||
w = 2 * w + b
|
||||
|
||||
# Remove block offset.
|
||||
w ^= rdsOffsetTable[typ]
|
||||
|
||||
# Calculate error syndrome.
|
||||
syn = w
|
||||
for i in xrange(16):
|
||||
if syn & (1 << (25 - i)):
|
||||
syn ^= gpoly << (15 - i)
|
||||
|
||||
# Check block.
|
||||
if syn == 0:
|
||||
return (w >> 10, 0)
|
||||
|
||||
# Error detected; try all single-bit errors.
|
||||
p = 1
|
||||
for k in xrange(26):
|
||||
if p == syn:
|
||||
# Detected single-bit error in bit k.
|
||||
w ^= (1 << k)
|
||||
return (w >> 10, 1)
|
||||
p <<= 1
|
||||
if p & 0x400:
|
||||
p ^= gpoly
|
||||
|
||||
# No single-bit error can explain this syndrome.
|
||||
return (None, 2)
|
||||
|
||||
|
||||
class RdsData(object):
|
||||
"""Stucture to hold common RDS data fields."""
|
||||
|
||||
pi = None
|
||||
pty = None
|
||||
tp = None
|
||||
ta = None
|
||||
ms = None
|
||||
af = None
|
||||
di = None
|
||||
pin = None
|
||||
pserv = None
|
||||
ptyn = None
|
||||
ptynab = None
|
||||
rtext = None
|
||||
rtextab = None
|
||||
time = None
|
||||
|
||||
tmp_afs = None
|
||||
tmp_aflen = 0
|
||||
tmp_afmode = 0
|
||||
|
||||
ptyTable = [
|
||||
'None', 'News',
|
||||
'Current Affairs', 'Information',
|
||||
'Sport', 'Education',
|
||||
'Drama', 'Cultures',
|
||||
'Science', 'Varied Speech',
|
||||
'Pop Music', 'Rock Music',
|
||||
'Easy Listening', 'Light Classics M',
|
||||
'Serious Classics', 'Other Music',
|
||||
'Weather & Metr', 'Finance',
|
||||
"Children's Progs", 'Social Affairs',
|
||||
'Religion', 'Phone In',
|
||||
'Travel & Touring', 'Leisure & Hobby',
|
||||
'Jazz Music', 'Country Music',
|
||||
'National Music', 'Oldies Music',
|
||||
'Folk Music', 'Documentary',
|
||||
'Alarm Test', 'Alarm - Alarm !' ]
|
||||
|
||||
def __str__(self):
|
||||
|
||||
if self.pi is None:
|
||||
return str(None)
|
||||
|
||||
s = 'RDS PI=%-5d' % self.pi
|
||||
|
||||
s += ' TP=%d' % self.tp
|
||||
|
||||
if self.ta is not None:
|
||||
s += ' TA=%d' % self.ta
|
||||
else:
|
||||
s += ' '
|
||||
|
||||
if self.ms is not None:
|
||||
s += ' MS=%d' % self.ms
|
||||
else:
|
||||
s += ' '
|
||||
|
||||
s += ' PTY=%-2d %-20s' % (self.pty, '(' + self.ptyTable[self.pty] + ')')
|
||||
|
||||
if self.ptyn is not None:
|
||||
s += ' PTYN=%r' + str(self.ptyn).strip('\x00')
|
||||
|
||||
if self.di is not None or self.pserv is not None:
|
||||
s += '\n '
|
||||
if self.di is not None:
|
||||
distr = '('
|
||||
distr += 'stereo' if self.di & 1 else 'mono'
|
||||
if self.di & 2:
|
||||
distr += ',artificial'
|
||||
if self.di & 4:
|
||||
distr += ',compressed'
|
||||
if self.di & 8:
|
||||
distr += ',dynpty'
|
||||
distr += ')'
|
||||
s += ' DI=%-2d %-37s' % (self.di, distr)
|
||||
else:
|
||||
s += 45 * ' '
|
||||
if self.pserv is not None:
|
||||
s += ' SERV=%r' % str(self.pserv).strip('\x00')
|
||||
|
||||
if self.time is not None or self.pin is not None:
|
||||
s += '\n '
|
||||
if self.time is not None:
|
||||
(day, hour, mt, off) = self.time
|
||||
dt = datetime.date.fromordinal(day + datetime.date(1858, 11, 17).toordinal())
|
||||
s += ' TIME=%04d-%02d-%02d %02d:%02d UTC ' % (dt.year, dt.month, dt.day, hour, mt)
|
||||
else:
|
||||
s += 27 * ' '
|
||||
if self.pin is not None:
|
||||
(day, hour, mt) = self.pin
|
||||
s += ' PIN=d%02d %02d:%02d' % (day, hour, mt)
|
||||
else:
|
||||
s += 14 * ' '
|
||||
|
||||
if self.af is not None:
|
||||
s += '\n AF='
|
||||
for f in self.af:
|
||||
if f > 1.0e6:
|
||||
s += '%.1fMHz ' % (f * 1.0e-6)
|
||||
else:
|
||||
s += '%.0fkHz ' % (f * 1.0e-3)
|
||||
|
||||
if self.rtext is not None:
|
||||
s += '\n RT=%r' % str(self.rtext).strip('\x00')
|
||||
|
||||
return s
|
||||
|
||||
|
||||
def rdsDecode(bits, rdsdata=None):
|
||||
"""Decode RDS data stream.
|
||||
|
||||
bits :: list of RDS data bits
|
||||
rdsdata :: optional RdsData object to store RDS information
|
||||
|
||||
Return (rdsdata, ngroups, errsoft, errhard)
|
||||
where rdsdata is the updated RdsData object
|
||||
ngroup is the number of correctly decoded RDS groups
|
||||
errsoft is the number of correctable bit errors
|
||||
errhard is the number of uncorrectable bit errors
|
||||
"""
|
||||
|
||||
if rdsdata is None:
|
||||
rdsdata = RdsData()
|
||||
|
||||
ngroup = 0
|
||||
errsoft = 0
|
||||
errhard = 0
|
||||
|
||||
p = 0
|
||||
n = len(bits)
|
||||
while p + 4 * 26 <= n:
|
||||
|
||||
(wa, ea) = rdsDecodeBlock(bits[p:p+26], "A")
|
||||
if wa is None:
|
||||
errhard += 1
|
||||
p += 1
|
||||
continue
|
||||
|
||||
(wb, eb) = rdsDecodeBlock(bits[p+26:p+2*26], "B")
|
||||
if wb is None:
|
||||
errhard += 1
|
||||
p += 1
|
||||
continue
|
||||
|
||||
if (wb >> 11) & 1:
|
||||
(wc, ec) = rdsDecodeBlock(bits[p+2*26:p+3*26], "C'")
|
||||
else:
|
||||
(wc, ec) = rdsDecodeBlock(bits[p+2*26:p+3*26], "C")
|
||||
if wc is None:
|
||||
errhard += 1
|
||||
p += 1
|
||||
continue
|
||||
|
||||
(wd, ed) = rdsDecodeBlock(bits[p+3*26:p+4*26], "D")
|
||||
if wd is None:
|
||||
errhard += 1
|
||||
p += 1
|
||||
continue
|
||||
|
||||
errsoft += ea + eb + ec + ed
|
||||
ngroup += 1
|
||||
|
||||
# Found an RDS group; decode it.
|
||||
typ = (wb >> 12)
|
||||
typb = (wb >> 11) & 1
|
||||
|
||||
# PI, TP, PTY are present in all groups
|
||||
rdsdata.pi = wa
|
||||
rdsdata.tp = (wb >> 10) & 1
|
||||
rdsdata.pty = (wb >> 5) & 0x1f
|
||||
|
||||
if typ == 0:
|
||||
# group type 0: TA, MS, DI, program service name
|
||||
rdsdata.ta = (wb >> 4) & 1
|
||||
rdsdata.ms = (wb >> 3) & 1
|
||||
dseg = wb & 3
|
||||
if rdsdata.di is None:
|
||||
rdsdata.di = 0
|
||||
rdsdata.di &= ~(1 << dseg)
|
||||
rdsdata.di |= (((wb >> 2) & 1) << dseg)
|
||||
if rdsdata.pserv is None:
|
||||
rdsdata.pserv = bytearray(8)
|
||||
rdsdata.pserv[2*dseg] = wd >> 8
|
||||
rdsdata.pserv[2*dseg+1] = wd & 0xff
|
||||
|
||||
if typ == 0 and not typb:
|
||||
# group type 0A: alternate frequencies
|
||||
for f in ((wc >> 8), wc & 0xff):
|
||||
if f >= 224 and f <= 249:
|
||||
rdsdata.tmp_aflen = f - 224
|
||||
rdsdata.tmp_aflfmode = 0
|
||||
rdsdata.tmp_afs = [ ]
|
||||
elif f == 250 and rdsdata.tmp_aflen > 0 and len(rdsdata.tmp_afs) < rdsdata.tmp_aflen:
|
||||
rdsdata.tmp_aflfmode = 1
|
||||
elif f >= 1 and f <= 204 and rdsdata.tmp_aflen > 0 and len(rdsdata.tmp_afs) < rdsdata.tmp_aflen:
|
||||
if rdsdata.tmp_aflfmode:
|
||||
rdsdata.tmp_afs.append(144.0e3 + f * 9.0e3)
|
||||
else:
|
||||
rdsdata.tmp_afs.append(87.5e6 + f * 0.1e6)
|
||||
if len(rdsdata.tmp_afs) == rdsdata.tmp_aflen:
|
||||
rdsdata.af = rdsdata.tmp_afs
|
||||
rdsdata.tmp_aflen = 0
|
||||
rdsdata.tmp_afs = [ ]
|
||||
rdsdata.tmp_aflfmode = 0
|
||||
|
||||
if typ == 1:
|
||||
# group type 1: program item number
|
||||
rdsdata.pin = (wd >> 11, (wd >> 6) & 0x1f, wd & 0x3f)
|
||||
|
||||
if typ == 2:
|
||||
# group type 2: radio text
|
||||
dseg = wb & 0xf
|
||||
if rdsdata.rtext is None or ((wb >> 4) & 1) != rdsdata.rtextab:
|
||||
rdsdata.rtext = bytearray(64)
|
||||
rdsdata.rtextab = (wb >> 4) & 1
|
||||
if typb:
|
||||
rdsdata.rtext[2*dseg] = (wd >> 8)
|
||||
rdsdata.rtext[2*dseg+1] = wd & 0xff
|
||||
else:
|
||||
rdsdata.rtext[4*dseg] = (wc >> 8)
|
||||
rdsdata.rtext[4*dseg+1] = wc & 0xff
|
||||
rdsdata.rtext[4*dseg+2] = (wd >> 8)
|
||||
rdsdata.rtext[4*dseg+3] = wd & 0xff
|
||||
|
||||
if typ == 4 and not typb:
|
||||
# group type 4A: clock-time and date
|
||||
rdsdata.time = (((wb & 3) << 15) | (wc >> 1),
|
||||
((wc & 1) << 4) | (wd >> 12),
|
||||
(wd >> 6) & 0x3f, (wd & 0x1f) - (wd & 0x20))
|
||||
|
||||
if typ == 10 and not typb:
|
||||
# group type 10A: program type name
|
||||
dseg = wb & 1
|
||||
if rdsdata.ptyn is None or ((wb >> 4) & 1) != rdsdata.ptynab:
|
||||
rdsdata.ptyn = bytearray(8)
|
||||
rdsdata.ptynab = (wb >> 4) & 1
|
||||
rdsdata.ptyn[4*dseg] = (wc >> 8)
|
||||
rdsdata.ptyn[4*dsseg+1] = wc & 0xff
|
||||
rdsdata.ptyn[4*dseg+2] = (wd >> 8)
|
||||
rdsdata.ptyn[4*dsseg+3] = wd & 0xff
|
||||
|
||||
# Go to next group.
|
||||
p += 4 * 26
|
||||
|
||||
return (rdsdata, ngroup, errsoft, errhard)
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue