Modified the FM demodulator to work with all input stream formats
This commit is contained in:
parent
fcb86176c3
commit
6b0c221283
225
fm1s.py
225
fm1s.py
@ -1,225 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
# FM demodulator based on I/Q (quadrature) samples
|
||||
|
||||
#
|
||||
# This code belongs to https://github.com/elvis-epx/sdr
|
||||
# No licence information is provided.
|
||||
#
|
||||
|
||||
import struct, math, random, sys, numpy, filters, time
|
||||
|
||||
optimized = "-o" in sys.argv
|
||||
debug_mode = "-d" in sys.argv
|
||||
disable_pll = "--disable-pll" in sys.argv
|
||||
if disable_pll:
|
||||
optimized = False
|
||||
|
||||
if optimized:
|
||||
import fastfm # Cython
|
||||
|
||||
MAX_DEVIATION = 200000.0 # Hz
|
||||
INPUT_RATE = 1000000
|
||||
OUTPUT_RATE = 50000
|
||||
|
||||
if debug_mode:
|
||||
OUTPUT_RATE=256000
|
||||
|
||||
DECIMATION = INPUT_RATE / OUTPUT_RATE
|
||||
assert DECIMATION == math.floor(DECIMATION)
|
||||
|
||||
FM_BANDWIDTH = 15000 # Hz
|
||||
STEREO_CARRIER = 38000 # Hz
|
||||
DEVIATION_X_SIGNAL = 0.999 / (math.pi * MAX_DEVIATION / (INPUT_RATE / 2))
|
||||
|
||||
pll = math.pi - random.random() * 2 * math.pi
|
||||
last_pilot = 0.0
|
||||
deviation_avg = math.pi - random.random() * 2 * math.pi
|
||||
last_deviation_avg = deviation_avg
|
||||
tau = 2 * math.pi
|
||||
|
||||
# Downsample mono audio
|
||||
decimate1 = filters.decimator(DECIMATION)
|
||||
|
||||
# Deemph + Low-pass filter for mono (L+R) audio
|
||||
lo = filters.deemphasis(INPUT_RATE, 75, FM_BANDWIDTH, 120)
|
||||
|
||||
# Downsample jstereo audio
|
||||
decimate2 = filters.decimator(DECIMATION)
|
||||
|
||||
# Deemph + Low-pass filter for joint-stereo demodulated audio (L-R)
|
||||
lo_r = filters.deemphasis(INPUT_RATE, 75, FM_BANDWIDTH, 120)
|
||||
|
||||
# Band-pass filter for stereo (L-R) modulated audio
|
||||
hi = filters.band_pass(INPUT_RATE,
|
||||
STEREO_CARRIER - FM_BANDWIDTH, STEREO_CARRIER + FM_BANDWIDTH, 120)
|
||||
|
||||
# Filter to extract pilot signal
|
||||
pilot = filters.band_pass(INPUT_RATE,
|
||||
STEREO_CARRIER / 2 - 100, STEREO_CARRIER / 2 + 100, 120)
|
||||
|
||||
last_angle = 0.0
|
||||
remaining_data = b''
|
||||
|
||||
while True:
|
||||
# Ingest 0.1s worth of data
|
||||
data = sys.stdin.buffer.read((INPUT_RATE * 2) // 10)
|
||||
if not data:
|
||||
break
|
||||
#data = remaining_data + data
|
||||
|
||||
if len(data) < 4:
|
||||
remaining_data = data
|
||||
continue
|
||||
|
||||
# Save one sample to next batch, and the odd byte if exists
|
||||
if len(data) % 2 == 1:
|
||||
print("Odd byte, that's odd", file=sys.stderr)
|
||||
remaining_data = data[-3:]
|
||||
data = data[:-1]
|
||||
else:
|
||||
remaining_data = data[-2:]
|
||||
|
||||
samples = len(data) // 2
|
||||
|
||||
# Find angle (phase) of I/Q pairs
|
||||
iqdata = numpy.frombuffer(data, dtype=numpy.uint8)
|
||||
print(iqdata.dtype, iqdata.shape, iqdata, file=sys.stderr)
|
||||
iqdata = iqdata - 127.5
|
||||
iqdata = iqdata / 128.0
|
||||
print(iqdata.dtype, iqdata.shape, iqdata, file=sys.stderr)
|
||||
iqdata = iqdata.view(complex)
|
||||
print(iqdata.dtype, iqdata.shape, iqdata, file=sys.stderr)
|
||||
|
||||
angles = numpy.angle(iqdata)
|
||||
|
||||
|
||||
# Determine phase rotation between samples
|
||||
# (Output one element less, that's we always save last sample
|
||||
# in remaining_data)
|
||||
rotations = numpy.ediff1d(angles)
|
||||
|
||||
# Wrap rotations >= +/-180º
|
||||
rotations = (rotations + numpy.pi) % (2 * numpy.pi) - numpy.pi
|
||||
|
||||
# Convert rotations to baseband signal
|
||||
output_raw = numpy.multiply(rotations, DEVIATION_X_SIGNAL)
|
||||
output_raw = numpy.clip(output_raw, -0.999, +0.999)
|
||||
|
||||
# At this point, output_raw contains two audio signals:
|
||||
# L+R (mono-compatible) and L-R (joint-stereo) modulated in AM-SC,
|
||||
# carrier 38kHz
|
||||
|
||||
# Downsample and low-pass L+R (mono) signal
|
||||
output_mono = lo.feed(output_raw)
|
||||
output_mono = decimate1.feed(output_mono)
|
||||
|
||||
# Filter pilot tone
|
||||
detected_pilot = pilot.feed(output_raw)
|
||||
|
||||
# Separate ultrasonic L-R signal by high-pass filtering
|
||||
output_jstereo_mod = hi.feed(output_raw)
|
||||
|
||||
# Demodulate L-R, which is AM-SC with 53kHz carrier
|
||||
|
||||
if optimized:
|
||||
output_jstereo, pll, STEREO_CARRIER, \
|
||||
last_pilot, deviation_avg, last_deviation_avg = \
|
||||
fastfm.demod_stereo(output_jstereo_mod,
|
||||
pll,
|
||||
STEREO_CARRIER,
|
||||
INPUT_RATE,
|
||||
detected_pilot,
|
||||
last_pilot,
|
||||
deviation_avg,
|
||||
last_deviation_avg)
|
||||
else:
|
||||
output_jstereo = []
|
||||
|
||||
for n in range(0, len(output_jstereo_mod)):
|
||||
# Advance carrier
|
||||
pll = (pll + tau * STEREO_CARRIER / INPUT_RATE) % tau
|
||||
|
||||
# Standard demodulation
|
||||
output_jstereo.append(math.cos(pll) * output_jstereo_mod[n])
|
||||
|
||||
if disable_pll:
|
||||
continue
|
||||
|
||||
############ Carrier PLL #################
|
||||
|
||||
# Detect pilot zero-crossing
|
||||
cur_pilot = detected_pilot[n]
|
||||
zero_crossed = (cur_pilot * last_pilot) <= 0
|
||||
last_pilot = cur_pilot
|
||||
if not zero_crossed:
|
||||
continue
|
||||
|
||||
# When pilot is at 90º or 270º, carrier should be around 180º
|
||||
# t=0 => cos(t) = 1, cos(2t) = 1
|
||||
# t=π/2 => cos(t) = 0, cos(2t) = -1
|
||||
# t=π => cos(t) = -1, cos(2t) = 1
|
||||
# t=-π/2 => cos(t) = 0, cos(2t) = -1
|
||||
ideal = math.pi
|
||||
deviation = pll - ideal
|
||||
if deviation > math.pi:
|
||||
# 350º => -10º
|
||||
deviation -= tau
|
||||
deviation_avg = 0.99 * deviation_avg + 0.01 * deviation
|
||||
rotation = deviation_avg - last_deviation_avg
|
||||
last_deviation_avg = deviation_avg
|
||||
|
||||
if abs(deviation_avg) > math.pi / 8:
|
||||
# big phase deviation, reset PLL
|
||||
# print("Resetting PLL", file=sys.stderr)
|
||||
pll = ideal
|
||||
pll = (pll + tau * STEREO_CARRIER / INPUT_RATE) % tau
|
||||
deviation_avg = 0.0
|
||||
last_deviation_avg = 0.0
|
||||
|
||||
# Translate rotation to frequency deviation e.g.
|
||||
# cos(tau + 3.6º) = cos(1.01 * tau)
|
||||
# cos(tau - 9º) = cos(tau * 0.975)
|
||||
#
|
||||
# Overcorrect by 5% to (try to) sync phase,
|
||||
# otherwise only the frequency would be synced.
|
||||
|
||||
STEREO_CARRIER /= (1 + (rotation * 1.05) / tau)
|
||||
|
||||
'''
|
||||
print("%d deviationavg=%f rotation=%f freq=%f" %
|
||||
(n,
|
||||
deviation_avg * 180 / math.pi,
|
||||
rotation * 180 / math.pi,
|
||||
STEREO_CARRIER),
|
||||
file=sys.stderr)
|
||||
time.sleep(0.05)
|
||||
'''
|
||||
|
||||
# Downsample, Low-pass/deemphasis demodulated L-R
|
||||
output_jstereo = lo_r.feed(output_jstereo)
|
||||
output_jstereo = decimate2.feed(output_jstereo)
|
||||
|
||||
assert len(output_jstereo) == len(output_mono)
|
||||
|
||||
# Scale to 16-bit and divide by 2 for channel sum
|
||||
output_mono = numpy.multiply(output_mono, 32767 / 2.0)
|
||||
output_jstereo = numpy.multiply(output_jstereo, 32767 / 2.0)
|
||||
|
||||
# Output stereo by adding or subtracting joint-stereo to mono
|
||||
output_left = output_mono + output_jstereo
|
||||
output_right = output_mono - output_jstereo
|
||||
|
||||
if not debug_mode:
|
||||
# Interleave L and R samples using NumPy trickery
|
||||
output = numpy.empty(len(output_mono) * 2, dtype=output_mono.dtype)
|
||||
output[0::2] = output_left
|
||||
output[1::2] = output_right
|
||||
output = output.astype(int)
|
||||
else:
|
||||
output = numpy.empty(len(output_mono) * 3, dtype=output_mono.dtype)
|
||||
output[0::3] = output_mono
|
||||
output[1::3] = output_jstereo
|
||||
output[2::3] = numpy.multiply(detected_pilot, 32767)
|
||||
output = output.astype(int)
|
||||
|
||||
sys.stdout.buffer.write(struct.pack('<%dh' % len(output), *output))
|
||||
225
fm1s_int16.py
225
fm1s_int16.py
@ -1,225 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
# FM demodulator based on I/Q (quadrature) samples
|
||||
|
||||
#
|
||||
# This code belongs to https://github.com/elvis-epx/sdr
|
||||
# No licence information is provided.
|
||||
#
|
||||
|
||||
import struct, math, random, sys, numpy, filters, time
|
||||
|
||||
optimized = "-o" in sys.argv
|
||||
debug_mode = "-d" in sys.argv
|
||||
disable_pll = "--disable-pll" in sys.argv
|
||||
if disable_pll:
|
||||
optimized = False
|
||||
|
||||
if optimized:
|
||||
import fastfm # Cython
|
||||
|
||||
MAX_DEVIATION = 200000.0 # Hz
|
||||
INPUT_RATE = 1000000
|
||||
OUTPUT_RATE = 50000
|
||||
|
||||
if debug_mode:
|
||||
OUTPUT_RATE=256000
|
||||
|
||||
DECIMATION = INPUT_RATE / OUTPUT_RATE
|
||||
assert DECIMATION == math.floor(DECIMATION)
|
||||
|
||||
FM_BANDWIDTH = 15000 # Hz
|
||||
STEREO_CARRIER = 38000 # Hz
|
||||
DEVIATION_X_SIGNAL = 0.999 / (math.pi * MAX_DEVIATION / (INPUT_RATE / 2))
|
||||
|
||||
pll = math.pi - random.random() * 2 * math.pi
|
||||
last_pilot = 0.0
|
||||
deviation_avg = math.pi - random.random() * 2 * math.pi
|
||||
last_deviation_avg = deviation_avg
|
||||
tau = 2 * math.pi
|
||||
|
||||
# Downsample mono audio
|
||||
decimate1 = filters.decimator(DECIMATION)
|
||||
|
||||
# Deemph + Low-pass filter for mono (L+R) audio
|
||||
lo = filters.deemphasis(INPUT_RATE, 75, FM_BANDWIDTH, 120)
|
||||
|
||||
# Downsample jstereo audio
|
||||
decimate2 = filters.decimator(DECIMATION)
|
||||
|
||||
# Deemph + Low-pass filter for joint-stereo demodulated audio (L-R)
|
||||
lo_r = filters.deemphasis(INPUT_RATE, 75, FM_BANDWIDTH, 120)
|
||||
|
||||
# Band-pass filter for stereo (L-R) modulated audio
|
||||
hi = filters.band_pass(INPUT_RATE,
|
||||
STEREO_CARRIER - FM_BANDWIDTH, STEREO_CARRIER + FM_BANDWIDTH, 120)
|
||||
|
||||
# Filter to extract pilot signal
|
||||
pilot = filters.band_pass(INPUT_RATE,
|
||||
STEREO_CARRIER / 2 - 100, STEREO_CARRIER / 2 + 100, 120)
|
||||
|
||||
last_angle = 0.0
|
||||
remaining_data = b''
|
||||
|
||||
while True:
|
||||
# Ingest 0.1s worth of data
|
||||
bytes_per_sample = 1
|
||||
data = sys.stdin.buffer.read((INPUT_RATE * bytes_per_sample * 2) // 10)
|
||||
if not data:
|
||||
break
|
||||
#data = remaining_data + data
|
||||
|
||||
if len(data) < 4:
|
||||
remaining_data = data
|
||||
continue
|
||||
|
||||
# Save one sample to next batch, and the odd byte if exists
|
||||
if len(data) % 2 == 1:
|
||||
print("Odd byte, that's odd", file=sys.stderr)
|
||||
remaining_data = data[-3:]
|
||||
data = data[:-1]
|
||||
else:
|
||||
remaining_data = data[-2:]
|
||||
|
||||
samples = len(data) // (2 * bytes_per_sample)
|
||||
|
||||
# Find angle (phase) of I/Q pairs
|
||||
iqdata = numpy.frombuffer(data, dtype=numpy.int16)
|
||||
print(iqdata.dtype, iqdata.shape, iqdata, file=sys.stderr)
|
||||
iqdata = iqdata / (2**15)
|
||||
print(iqdata.dtype, iqdata.shape, iqdata, file=sys.stderr)
|
||||
iqdata = iqdata.view(complex)
|
||||
print(iqdata.dtype, iqdata.shape, iqdata, file=sys.stderr)
|
||||
|
||||
angles = numpy.angle(iqdata)
|
||||
|
||||
|
||||
# Determine phase rotation between samples
|
||||
# (Output one element less, that's we always save last sample
|
||||
# in remaining_data)
|
||||
rotations = numpy.ediff1d(angles)
|
||||
|
||||
# Wrap rotations >= +/-180º
|
||||
rotations = (rotations + numpy.pi) % (2 * numpy.pi) - numpy.pi
|
||||
|
||||
# Convert rotations to baseband signal
|
||||
output_raw = numpy.multiply(rotations, DEVIATION_X_SIGNAL)
|
||||
output_raw = numpy.clip(output_raw, -0.999, +0.999)
|
||||
|
||||
# At this point, output_raw contains two audio signals:
|
||||
# L+R (mono-compatible) and L-R (joint-stereo) modulated in AM-SC,
|
||||
# carrier 38kHz
|
||||
|
||||
# Downsample and low-pass L+R (mono) signal
|
||||
output_mono = lo.feed(output_raw)
|
||||
output_mono = decimate1.feed(output_mono)
|
||||
|
||||
# Filter pilot tone
|
||||
detected_pilot = pilot.feed(output_raw)
|
||||
|
||||
# Separate ultrasonic L-R signal by high-pass filtering
|
||||
output_jstereo_mod = hi.feed(output_raw)
|
||||
|
||||
# Demodulate L-R, which is AM-SC with 53kHz carrier
|
||||
|
||||
if optimized:
|
||||
output_jstereo, pll, STEREO_CARRIER, \
|
||||
last_pilot, deviation_avg, last_deviation_avg = \
|
||||
fastfm.demod_stereo(output_jstereo_mod,
|
||||
pll,
|
||||
STEREO_CARRIER,
|
||||
INPUT_RATE,
|
||||
detected_pilot,
|
||||
last_pilot,
|
||||
deviation_avg,
|
||||
last_deviation_avg)
|
||||
else:
|
||||
output_jstereo = []
|
||||
|
||||
for n in range(0, len(output_jstereo_mod)):
|
||||
# Advance carrier
|
||||
pll = (pll + tau * STEREO_CARRIER / INPUT_RATE) % tau
|
||||
|
||||
# Standard demodulation
|
||||
output_jstereo.append(math.cos(pll) * output_jstereo_mod[n])
|
||||
|
||||
if disable_pll:
|
||||
continue
|
||||
|
||||
############ Carrier PLL #################
|
||||
|
||||
# Detect pilot zero-crossing
|
||||
cur_pilot = detected_pilot[n]
|
||||
zero_crossed = (cur_pilot * last_pilot) <= 0
|
||||
last_pilot = cur_pilot
|
||||
if not zero_crossed:
|
||||
continue
|
||||
|
||||
# When pilot is at 90º or 270º, carrier should be around 180º
|
||||
# t=0 => cos(t) = 1, cos(2t) = 1
|
||||
# t=π/2 => cos(t) = 0, cos(2t) = -1
|
||||
# t=π => cos(t) = -1, cos(2t) = 1
|
||||
# t=-π/2 => cos(t) = 0, cos(2t) = -1
|
||||
ideal = math.pi
|
||||
deviation = pll - ideal
|
||||
if deviation > math.pi:
|
||||
# 350º => -10º
|
||||
deviation -= tau
|
||||
deviation_avg = 0.99 * deviation_avg + 0.01 * deviation
|
||||
rotation = deviation_avg - last_deviation_avg
|
||||
last_deviation_avg = deviation_avg
|
||||
|
||||
if abs(deviation_avg) > math.pi / 8:
|
||||
# big phase deviation, reset PLL
|
||||
# print("Resetting PLL", file=sys.stderr)
|
||||
pll = ideal
|
||||
pll = (pll + tau * STEREO_CARRIER / INPUT_RATE) % tau
|
||||
deviation_avg = 0.0
|
||||
last_deviation_avg = 0.0
|
||||
|
||||
# Translate rotation to frequency deviation e.g.
|
||||
# cos(tau + 3.6º) = cos(1.01 * tau)
|
||||
# cos(tau - 9º) = cos(tau * 0.975)
|
||||
#
|
||||
# Overcorrect by 5% to (try to) sync phase,
|
||||
# otherwise only the frequency would be synced.
|
||||
|
||||
STEREO_CARRIER /= (1 + (rotation * 1.05) / tau)
|
||||
|
||||
'''
|
||||
print("%d deviationavg=%f rotation=%f freq=%f" %
|
||||
(n,
|
||||
deviation_avg * 180 / math.pi,
|
||||
rotation * 180 / math.pi,
|
||||
STEREO_CARRIER),
|
||||
file=sys.stderr)
|
||||
time.sleep(0.05)
|
||||
'''
|
||||
|
||||
# Downsample, Low-pass/deemphasis demodulated L-R
|
||||
output_jstereo = lo_r.feed(output_jstereo)
|
||||
output_jstereo = decimate2.feed(output_jstereo)
|
||||
|
||||
assert len(output_jstereo) == len(output_mono)
|
||||
|
||||
# Scale to 16-bit and divide by 2 for channel sum
|
||||
output_mono = numpy.multiply(output_mono, 32767 / 2.0)
|
||||
output_jstereo = numpy.multiply(output_jstereo, 32767 / 2.0)
|
||||
|
||||
# Output stereo by adding or subtracting joint-stereo to mono
|
||||
output_left = output_mono + output_jstereo
|
||||
output_right = output_mono - output_jstereo
|
||||
|
||||
if not debug_mode:
|
||||
# Interleave L and R samples using NumPy trickery
|
||||
output = numpy.empty(len(output_mono) * 2, dtype=output_mono.dtype)
|
||||
output[0::2] = output_left
|
||||
output[1::2] = output_right
|
||||
output = output.astype(int)
|
||||
else:
|
||||
output = numpy.empty(len(output_mono) * 3, dtype=output_mono.dtype)
|
||||
output[0::3] = output_mono
|
||||
output[1::3] = output_jstereo
|
||||
output[2::3] = numpy.multiply(detected_pilot, 32767)
|
||||
output = output.astype(int)
|
||||
|
||||
sys.stdout.buffer.write(struct.pack('<%dh' % len(output), *output))
|
||||
204
fm_demod.py
Executable file
204
fm_demod.py
Executable file
@ -0,0 +1,204 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
# FM demodulator based on I/Q (quadrature) samples
|
||||
#
|
||||
# This code is modified from the original demodulator by:
|
||||
# https://github.com/elvis-epx/sdr
|
||||
# No licence information is provided.
|
||||
#
|
||||
|
||||
import struct
|
||||
import math
|
||||
import random
|
||||
import sys
|
||||
import numpy as np
|
||||
import filters
|
||||
import argparse
|
||||
import prefixed
|
||||
|
||||
|
||||
TYPES = {
|
||||
'CU8': np.uint8,
|
||||
'CS16': np.int16,
|
||||
'CF32': np.float32,
|
||||
}
|
||||
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('-v', '--verbose', help='Print additional informational output', action='store_true')
|
||||
parser.add_argument('-f', '--format', choices=list(TYPES.keys()), help='Input sample format', required=True)
|
||||
parser.add_argument('-s', '--sample-rate', metavar='rate', help='Source sample rate (Hz)', required=True)
|
||||
parser.add_argument('-d', '--demod-rate', metavar='rate', help='Output sample rate (Hz)', required=True)
|
||||
# TODO JMT: Output to file
|
||||
#parser.add_argument('-o', metavar='file', help='Specify an output file for demodulated audio. Omit for stdout or use \'-\'.')
|
||||
args = parser.parse_args()
|
||||
|
||||
INPUT_RATE = int(prefixed.Float(args.sample_rate))
|
||||
OUTPUT_RATE = int(prefixed.Float(args.demod_rate))
|
||||
DECIMATION = INPUT_RATE / OUTPUT_RATE
|
||||
|
||||
if DECIMATION != math.floor(DECIMATION):
|
||||
print(f'The output rate must be an integer divisor of the input rate: {INPUT_RATE}/{OUTPUT_RATE} = {DECIMATION}', file=sys.stderr)
|
||||
sys.exit(1)
|
||||
|
||||
FORMAT = TYPES[args.format]
|
||||
DT = np.dtype(FORMAT)
|
||||
BYTES_PER_SAMPLE = 2 * DT.itemsize
|
||||
|
||||
MAX_DEVIATION = 200000.0 # Hz
|
||||
FM_BANDWIDTH = 15000 # Hz
|
||||
STEREO_CARRIER = 38000 # Hz
|
||||
DEVIATION_X_SIGNAL = 0.999 / (math.pi * MAX_DEVIATION / (INPUT_RATE / 2))
|
||||
|
||||
pll = math.pi - random.random() * 2 * math.pi
|
||||
last_pilot = 0.0
|
||||
deviation_avg = math.pi - random.random() * 2 * math.pi
|
||||
last_deviation_avg = deviation_avg
|
||||
tau = 2 * math.pi
|
||||
|
||||
# Downsample mono audio
|
||||
decimate1 = filters.decimator(DECIMATION)
|
||||
|
||||
# Deemph + Low-pass filter for mono (L+R) audio
|
||||
lo = filters.deemphasis(INPUT_RATE, 75, FM_BANDWIDTH, 120)
|
||||
|
||||
# Downsample jstereo audio
|
||||
decimate2 = filters.decimator(DECIMATION)
|
||||
|
||||
# Deemph + Low-pass filter for joint-stereo demodulated audio (L-R)
|
||||
lo_r = filters.deemphasis(INPUT_RATE, 75, FM_BANDWIDTH, 120)
|
||||
|
||||
# Band-pass filter for stereo (L-R) modulated audio
|
||||
hi = filters.band_pass(INPUT_RATE,
|
||||
STEREO_CARRIER - FM_BANDWIDTH, STEREO_CARRIER + FM_BANDWIDTH, 120)
|
||||
|
||||
# Filter to extract pilot signal
|
||||
pilot = filters.band_pass(INPUT_RATE,
|
||||
STEREO_CARRIER / 2 - 100, STEREO_CARRIER / 2 + 100, 120)
|
||||
|
||||
last_angle = 0.0
|
||||
remaining_data = b''
|
||||
|
||||
while True:
|
||||
# Ingest 0.1s worth of data
|
||||
data = sys.stdin.buffer.read((INPUT_RATE * BYTES_PER_SAMPLE) // 10)
|
||||
if not data:
|
||||
break
|
||||
# TODO JMT: Something about this is broken for BYTES_PER_SAMPLE > 1
|
||||
#data = remaining_data + data
|
||||
|
||||
if len(data) < 2 * BYTES_PER_SAMPLE:
|
||||
remaining_data = data
|
||||
continue
|
||||
|
||||
# Save one sample to next batch, and the odd byte if exists
|
||||
if len(data) % 2 == 1:
|
||||
print("Odd byte, that's odd", file=sys.stderr)
|
||||
remaining_data = data[-3:]
|
||||
data = data[:-1]
|
||||
else:
|
||||
remaining_data = data[-2:]
|
||||
|
||||
samples = len(data) // BYTES_PER_SAMPLE
|
||||
|
||||
# Find angle (phase) of I/Q pairs
|
||||
iqdata = np.frombuffer(data, dtype=FORMAT)
|
||||
|
||||
if args.verbose:
|
||||
print(iqdata.dtype, iqdata.shape, iqdata, file=sys.stderr)
|
||||
|
||||
if np.issubdtype(FORMAT, np.integer):
|
||||
iinfo = np.iinfo(FORMAT)
|
||||
if np.issubdtype(FORMAT, np.unsignedinteger):
|
||||
iqdata = iqdata - (iinfo.max / 2.0)
|
||||
iqdata = iqdata / (iinfo.max / 2.0)
|
||||
else:
|
||||
iqdata = iqdata / np.float64(iinfo.max)
|
||||
else:
|
||||
iqdata = iqdata.astype(np.float64)
|
||||
|
||||
if args.verbose:
|
||||
print(iqdata.dtype, iqdata.shape, iqdata, file=sys.stderr)
|
||||
|
||||
iqdata = iqdata.view(complex)
|
||||
angles = np.angle(iqdata)
|
||||
|
||||
# Determine phase rotation between samples
|
||||
rotations = np.ediff1d(angles)
|
||||
|
||||
# Wrap rotations >= +/-180º
|
||||
rotations = (rotations + np.pi) % (2 * np.pi) - np.pi
|
||||
|
||||
# Convert rotations to baseband signal
|
||||
output_raw = np.multiply(rotations, DEVIATION_X_SIGNAL)
|
||||
output_raw = np.clip(output_raw, -0.999, +0.999)
|
||||
|
||||
# At this point, output_raw contains two audio signals:
|
||||
# L+R (mono-compatible) and L-R (joint-stereo) modulated in AM-SC,
|
||||
# carrier 38kHz
|
||||
|
||||
# Downsample and low-pass L+R (mono) signal
|
||||
output_mono = lo.feed(output_raw)
|
||||
output_mono = decimate1.feed(output_mono)
|
||||
|
||||
# Filter pilot tone
|
||||
detected_pilot = pilot.feed(output_raw)
|
||||
|
||||
# Separate ultrasonic L-R signal by high-pass filtering
|
||||
output_jstereo_mod = hi.feed(output_raw)
|
||||
output_jstereo = []
|
||||
|
||||
# Demodulate L-R, which is AM-SC with 53kHz carrier
|
||||
for n in range(0, len(output_jstereo_mod)):
|
||||
# Advance carrier
|
||||
pll = (pll + tau * STEREO_CARRIER / INPUT_RATE) % tau
|
||||
# Standard demodulation
|
||||
output_jstereo.append(math.cos(pll) * output_jstereo_mod[n])
|
||||
|
||||
# Detect pilot zero-crossing
|
||||
cur_pilot = detected_pilot[n]
|
||||
zero_crossed = (cur_pilot * last_pilot) <= 0
|
||||
last_pilot = cur_pilot
|
||||
if not zero_crossed:
|
||||
continue
|
||||
|
||||
# When pilot is at 90º or 270º, carrier should be around 180º
|
||||
ideal = math.pi
|
||||
deviation = pll - ideal
|
||||
if deviation > math.pi:
|
||||
deviation -= tau
|
||||
deviation_avg = 0.99 * deviation_avg + 0.01 * deviation
|
||||
rotation = deviation_avg - last_deviation_avg
|
||||
last_deviation_avg = deviation_avg
|
||||
|
||||
if abs(deviation_avg) > math.pi / 8:
|
||||
# big phase deviation, reset PLL
|
||||
pll = ideal
|
||||
pll = (pll + tau * STEREO_CARRIER / INPUT_RATE) % tau
|
||||
deviation_avg = 0.0
|
||||
last_deviation_avg = 0.0
|
||||
|
||||
# Translate rotation to frequency deviation
|
||||
STEREO_CARRIER /= (1 + (rotation * 1.05) / tau)
|
||||
|
||||
|
||||
# Downsample, Low-pass/deemphasis demodulated L-R
|
||||
output_jstereo = lo_r.feed(output_jstereo)
|
||||
output_jstereo = decimate2.feed(output_jstereo)
|
||||
|
||||
assert len(output_jstereo) == len(output_mono)
|
||||
|
||||
# Scale to 16-bit and divide by 2 for channel sum
|
||||
output_mono = np.multiply(output_mono, 32767 / 2.0)
|
||||
output_jstereo = np.multiply(output_jstereo, 32767 / 2.0)
|
||||
|
||||
# Output stereo by adding or subtracting joint-stereo to mono
|
||||
output_left = output_mono + output_jstereo
|
||||
output_right = output_mono - output_jstereo
|
||||
|
||||
# Interleave L and R samples using np trickery
|
||||
output = np.empty(len(output_mono) * 2, dtype=output_mono.dtype)
|
||||
output[0::2] = output_left
|
||||
output[1::2] = output_right
|
||||
output = output.astype(int)
|
||||
|
||||
sys.stdout.buffer.write(struct.pack('<%dh' % len(output), *output))
|
||||
21
fm_rt_stereo
21
fm_rt_stereo
@ -1,21 +0,0 @@
|
||||
#!/bin/sh
|
||||
|
||||
#
|
||||
# This code belongs to https://github.com/elvis-epx/sdr
|
||||
# No licence information is provided.
|
||||
#
|
||||
|
||||
|
||||
#./example.py | ./fm1s.py | sox -t raw -r 50000 -b 16 -c 2 -L -e signed-integer - -d
|
||||
|
||||
#./example.py > buffer.iq
|
||||
#./demo.py > sdrplay_buffer.iq
|
||||
./fm1s_2.py < sdrplay_buffer.iq
|
||||
|
||||
|
||||
# Run the radio direct to audio. Something about 1M samples and 50000 output rate sounds ass, but it works
|
||||
#rtl_sdr -f 105.5M -s 1M - | ./fm1s.py | sox -t raw -r 50000 -b 16 -c 2 -L -e signed-integer - -d
|
||||
|
||||
# Get data from the nooelec device
|
||||
#rtl_sdr -f 105.5M -s 1M -n 10000 nooelec_buffer.iq
|
||||
./fm1s.py < nooelec_buffer.iq
|
||||
7
nooelec
7
nooelec
@ -1,7 +0,0 @@
|
||||
#! /bin/bash
|
||||
|
||||
if ! [ -f nooelec_buffer.iq ]; then
|
||||
rtl_sdr -f 105.5M -s1M -n 1000000 nooelec_buffer.iq
|
||||
fi
|
||||
./fm1s.py < nooelec_buffer.iq > nooelec_buffer.raw
|
||||
./play.sh < nooelec_buffer.raw
|
||||
7
sdrplay
7
sdrplay
@ -1,7 +0,0 @@
|
||||
#! /bin/bash
|
||||
|
||||
if ! [ -f sdrplay_int16.iq ]; then
|
||||
./demo.py > sdrplay_int16.iq
|
||||
fi
|
||||
./fm1s_int16.py < sdrplay_int16.iq > sdrplay_int16.raw
|
||||
./play.sh < sdrplay_int16.raw
|
||||
@ -71,7 +71,7 @@ parser.add_argument('-s', metavar='rate', help=f'Sample rate (Hz)', default=SAMP
|
||||
parser.add_argument('-g', metavar='gain', help='Signal gain (dB). Omit for automatic gain setting.')
|
||||
parser.add_argument('-n', metavar='count', help='Specify a number of samples to collect, or 0 for continuous.', default=SAMPLE_COUNT)
|
||||
parser.add_argument('-o', metavar='file', help='Specify an output file for received samples. For stdout use \'-\'.')
|
||||
parser.add_argument('--format', metavar='format', help='Specify the binary format of each sample. Defaults to the first reported supported stream format for the device. Use `-v --list-devices` to find supported output formats.')
|
||||
parser.add_argument('--format', choices=FORMATS.keys(), help='Specify the binary format of each sample. Defaults to the first reported supported stream format for the device. Use `-v --list-devices` to find supported output formats.')
|
||||
args = parser.parse_args()
|
||||
|
||||
|
||||
@ -144,7 +144,7 @@ if args.d:
|
||||
sdr.setGain(soapy.SOAPY_SDR_RX, 0, gain)
|
||||
|
||||
#setup a stream
|
||||
samples = 10000
|
||||
samples = 1000
|
||||
buffer = np.array([0] * samples * 2, TYPES[format])
|
||||
stream = sdr.setupStream(soapy.SOAPY_SDR_RX, format)
|
||||
sdr.activateStream(stream)
|
||||
@ -161,7 +161,12 @@ if args.d:
|
||||
break
|
||||
|
||||
result = sdr.readStream(stream, [buffer], collect_samples)
|
||||
print('Rx:', result.ret, result.flags, result.timeNs, file=sys.stderr)
|
||||
|
||||
if args.verbose:
|
||||
print('Rx:', result.ret, result.flags, result.timeNs, file=sys.stderr)
|
||||
if result.ret != samples:
|
||||
continue
|
||||
|
||||
if result.ret < 1:
|
||||
print('Stream read failed, exiting.', file=sys.stderr)
|
||||
keep_going = False
|
||||
|
||||
Loading…
Reference in New Issue
Block a user