diff options
Diffstat (limited to 'src/main/kotlin/space/autistic/radio/fmsim/FmFullDemodulator.kt')
| -rw-r--r-- | src/main/kotlin/space/autistic/radio/fmsim/FmFullDemodulator.kt | 158 |
1 files changed, 0 insertions, 158 deletions
diff --git a/src/main/kotlin/space/autistic/radio/fmsim/FmFullDemodulator.kt b/src/main/kotlin/space/autistic/radio/fmsim/FmFullDemodulator.kt deleted file mode 100644 index de44e69..0000000 --- a/src/main/kotlin/space/autistic/radio/fmsim/FmFullDemodulator.kt +++ /dev/null @@ -1,158 +0,0 @@ -package space.autistic.radio.fmsim - -import org.joml.Vector2f -import org.jtransforms.fft.FloatFFT_1D -import space.autistic.radio.complex.I -import space.autistic.radio.complex.cmul -import space.autistic.radio.complex.conjugate -import space.autistic.radio.dsp.Biquad1stOrder -import java.nio.FloatBuffer -import java.util.function.BiConsumer - -class FmFullDemodulator { - private val inputBuffer = FloatBuffer.allocate(FmFullConstants.FFT_SIZE_48K_300K) - private val fft300kBuf = FloatBuffer.allocate(FmFullConstants.FFT_SIZE_48K_300K) - private val fft48kBuf = FloatBuffer.allocate(FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1) - private val outputBuffer = FloatBuffer.allocate(2 * FmFullConstants.FFT_DATA_BLOCK_SIZE_LPF_48K_15K_3K1) - - init { - inputBuffer.position(2 * FmFullConstants.FFT_OVERLAP_48K_300K) - } - - // yep. - private val boxcarI = Biquad1stOrder(1f, 1f, 0f) - private val boxcarQ = Biquad1stOrder(1f, 1f, 0f) - private val delayI = Biquad1stOrder(0f, 1f, 0f) - private val delayQ = Biquad1stOrder(0f, 1f, 0f) - - private val deemphasisLeft = Biquad1stOrder( - FmFullConstants.FM_DEEMPAHSIS_B0_48K, - FmFullConstants.FM_DEEMPHASIS_B1_48K, - FmFullConstants.FM_DEEMPHASIS_A1_48K - ) - private val deemphasisRight = Biquad1stOrder( - FmFullConstants.FM_DEEMPAHSIS_B0_48K, - FmFullConstants.FM_DEEMPHASIS_B1_48K, - FmFullConstants.FM_DEEMPHASIS_A1_48K - ) - - private val lastStereoPilot = Vector2f() - private val lastStereoPilotPolarDiscriminator = Vector2f() - - /** - * Takes in samples at 300kHz, in I/Q format, and processes them for output. - * - * Calls consumer with processed samples at 48kHz, stereo. - */ - fun process(input: FloatBuffer, stereo: Boolean, consumer: BiConsumer<Boolean, FloatBuffer>) { - while (input.remaining() >= 2) { - val z = Vector2f() - val w = Vector2f() - while (input.remaining() >= 2 && inputBuffer.hasRemaining()) { - z.x = boxcarI.process(input.get()) - z.y = boxcarQ.process(input.get()) - // quadrature demodulation = FM demodulation - // see https://wiki.gnuradio.org/index.php/Quadrature_Demod and such - w.x = delayI.process(z.x) - w.y = -delayQ.process(z.y) - z.cmul(w) - inputBuffer.put(org.joml.Math.atan2(z.y, z.x) * FmFullConstants.INVERSE_CORRECTION_FACTOR) - } - if (!inputBuffer.hasRemaining()) { - var stereoPilot = false - fft300kBuf.put(0, inputBuffer.array()) - fft300k.realForward(fft300kBuf.array()) - for (i in 0 until fft48kBuf.capacity()) { - fft48kBuf.put(i, 0f) - } - for (i in 2 until (FmFullConstants.FREQUENCY_MIXING_BINS_38K - 2 and 1.inv()) step 2) { - z.x = fft300kBuf.get(i) - z.y = fft300kBuf.get(i + 1) - w.x = fir48kLpf.get(i) - w.y = fir48kLpf.get(i + 1) - z.cmul(w) - fft48kBuf.put(i, z.x) - fft48kBuf.put(i + 1, z.y) - } - fft48kBuf.put(0, fft300kBuf.get(0) * fir48kLpf.get(0)) - fft48k.realInverse(fft48kBuf.array(), false) - outputBuffer.clear() - fft48kBuf.position(FmFullConstants.FFT_OVERLAP_LPF_48K_15K_3K1) - for (i in 0 until FmFullConstants.FFT_DATA_BLOCK_SIZE_LPF_48K_15K_3K1) { - val sample = fft48kBuf.get() * (1f / FmFullConstants.FFT_SIZE_48K_300K) - outputBuffer.put(sample) - outputBuffer.put(sample) - } - outputBuffer.clear() - if (stereo) { - z.x = fft300kBuf.get(FmFullConstants.FREQUENCY_MIXING_BINS_38K) - z.y = fft300kBuf.get(FmFullConstants.FREQUENCY_MIXING_BINS_38K + 1) - z.conjugate(w).cmul(lastStereoPilot).conjugate().normalize() - if (lastStereoPilotPolarDiscriminator.distanceSquared(w) < 0.5f && z.lengthSquared() >= FmFullConstants.FFT_SIZE_48K_300K) { - stereoPilot = true - } - lastStereoPilot.set(z) - lastStereoPilotPolarDiscriminator.set(w) - if (stereoPilot) { - // w is our phase offset - // TODO check if this is mathematically sound - z.normalize().cmul(z).cmul(w.conjugate()).conjugate() - // z is our recovered 38kHz carrier, including phase offset - for (i in 0 until fft48kBuf.capacity()) { - fft48kBuf.put(i, 0f) - } - val base = FmFullConstants.FREQUENCY_MIXING_BINS_38K * 2 - val sz = Vector2f() - val sw = Vector2f() - for (i in 2 until (FmFullConstants.FREQUENCY_MIXING_BINS_38K - 2 and 1.inv()) step 2) { - sz.x = fft300kBuf.get(base + i) - sz.y = fft300kBuf.get(base + i + 1) - sw.x = fft300kBuf.get(base - i) - sw.y = fft300kBuf.get(base - i + 1) - sz.cmul(z).add(sw.cmul(z).conjugate()) - sw.x = fir48kLpf.get(i) - sw.y = fir48kLpf.get(i + 1) - sz.cmul(sw) - fft48kBuf.put(i, sz.x) - fft48kBuf.put(i + 1, sz.y) - } - sz.x = fft300kBuf.get(base) - sz.y = fft300kBuf.get(base + 1) - sz.cmul(z) - fft48kBuf.put(0, sz.x * fir48kLpf.get(0)) - fft48k.realInverse(fft48kBuf.array(), false) - outputBuffer.clear() - fft48kBuf.position(FmFullConstants.FFT_OVERLAP_LPF_48K_15K_3K1) - for (i in 0 until FmFullConstants.FFT_DATA_BLOCK_SIZE_LPF_48K_15K_3K1) { - val lmr = fft48kBuf.get() * (1f / FmFullConstants.FFT_SIZE_48K_300K) - val lpr = outputBuffer.get(outputBuffer.position()) - outputBuffer.put((lpr + lmr) * 0.5f) - outputBuffer.put((lpr - lmr) * 0.5f) - } - outputBuffer.clear() - } - } - inputBuffer.position(FmFullConstants.FFT_DATA_BLOCK_SIZE_48K_300K) - inputBuffer.compact() - for (i in 0 until outputBuffer.capacity() step 2) { - outputBuffer.put(i, deemphasisLeft.process(outputBuffer.get(i))) - } - for (i in 1 until outputBuffer.capacity() step 2) { - outputBuffer.put(i, deemphasisRight.process(outputBuffer.get(i))) - } - consumer.accept(stereoPilot, outputBuffer) - } - } - } - - companion object { - private val fft300k = FloatFFT_1D(FmFullConstants.FFT_SIZE_48K_300K.toLong()) - private val fft48k = FloatFFT_1D(FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1.toLong()) - private val fir48kLpf = FloatBuffer.allocate(FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1) - - init { - fir48kLpf.put(0, FmFullConstants.FIR_LPF_48K_15K_3K1) - fft48k.realForward(fir48kLpf.array()) - } - } -} \ No newline at end of file |