diff options
Diffstat (limited to 'src/main/kotlin/space/autistic/radio')
4 files changed, 294 insertions, 107 deletions
diff --git a/src/main/kotlin/space/autistic/radio/cli/OfflineSimulator.kt b/src/main/kotlin/space/autistic/radio/cli/OfflineSimulator.kt index 517957b..bc16814 100644 --- a/src/main/kotlin/space/autistic/radio/cli/OfflineSimulator.kt +++ b/src/main/kotlin/space/autistic/radio/cli/OfflineSimulator.kt @@ -3,6 +3,7 @@ package space.autistic.radio.cli import org.joml.Vector2f import space.autistic.radio.complex.cmul import space.autistic.radio.fmsim.FmFullConstants +import space.autistic.radio.fmsim.FmFullDemodulator import space.autistic.radio.fmsim.FmFullModulator import java.io.FileInputStream import java.io.FileOutputStream @@ -17,20 +18,24 @@ import kotlin.math.min import kotlin.system.exitProcess fun printUsage() { - println("Usage: OfflineSimulator -o OUTFILE.raw {[-p POWER] [-l|-h] file:///FILE.raw}") + println("Usage: OfflineSimulator <-o|-O> OUTFILE.raw {[-p POWER] [-l|-h] [-m] file:///FILE.raw} [-m]") println(" file:///FILE.raw (or ./FILE.raw - the ./ is required)") - println(" The raw input file. 2x48kHz 32-bit float") + println(" The raw input file. two-channel (even with -m), 48kHz 32-bit float.") println(" -o OUTFILE.raw") - println(" The raw RF stream to output, 2x200kHz 32-bit float") + println(" The raw RF stream to output, 2x300kHz 32-bit float.") + println(" -O OUTFILE.raw") + println(" The raw audio stream to output, 2x48kHz 32-bit float.") println(" -p POWER") - println(" The signal amplitude (power level), e.g. 1.0") + println(" The signal amplitude (power level), e.g. 1.0.") println(" -l") println(" Simulate a partial overlap on the lower half of the tuned-into frequency.") println(" -h") println(" Simulate a partial overlap on the upper half of the tuned-into frequency.") + println(" -m") + println(" Downconvert to mono. As the last option, demodulate as mono.") } -class SimFile(val power: Float, val band: Int, val filename: String) { +class SimFile(val power: Float, val band: Int, val filename: String, val stereo: Boolean) { var closed: Boolean = false val buffer: FloatBuffer = FloatBuffer.allocate(8192) val modulator = FmFullModulator() @@ -45,14 +50,16 @@ fun main(args: Array<String>) { var hasOutput = false var inArg = "" var output = "" + var rfOutput = true var power = 1.0f var band = 2 + var stereo = FmFullConstants.STEREO val files: ArrayList<SimFile> = ArrayList() for (arg in args) { if (!hasOutput) { - if (arg == "-o") { + if (arg == "-o" || arg == "-O") { hasOutput = true - inArg = "-o" + inArg = arg } else { printUsage() exitProcess(1) @@ -61,6 +68,13 @@ fun main(args: Array<String>) { when (inArg) { "-o" -> { output = arg + rfOutput = true + inArg = "" + } + + "-O" -> { + output = arg + rfOutput = false inArg = "" } @@ -75,15 +89,17 @@ fun main(args: Array<String>) { "" -> { if (!arg.startsWith("-")) { - files.add(SimFile(power, band, arg)) + files.add(SimFile(power, band, arg, stereo)) inArg = "" band = 2 power = 1.0f + stereo = FmFullConstants.STEREO } else { when (arg) { "-p" -> inArg = "-p" "-l" -> band = 1 "-h" -> band = 3 + "-m" -> stereo = FmFullConstants.MONO else -> { println("Unknown option") printUsage() @@ -119,14 +135,16 @@ fun main(args: Array<String>) { val buffer = ByteBuffer.allocate(2 * 4 * FmFullConstants.FFT_DATA_BLOCK_SIZE_LPF_48K_15K_3K1) val plus100k = FloatBuffer.wrap(FmFullConstants.CBUFFER_100K_300K) val minus100k = FloatBuffer.wrap(FmFullConstants.CBUFFER_100K_300K) + val demodulator = FmFullDemodulator() + var lastStereoPilot = false while (true) { // initialized to maximum buffer size, trimmed down later var minBuffer = 8192 for (inputFile in files) { val stream = inputFile.stream if (stream == null) { - if (inputFile.buffer.remaining() > 2 * FmFullConstants.IFFT_DATA_BLOCK_SIZE_48K_300K) { - inputFile.modulator.flush(inputFile.power) { + if (inputFile.buffer.remaining() > 2 * FmFullConstants.FFT_DATA_BLOCK_SIZE_48K_300K) { + inputFile.modulator.flush(inputFile.power, inputFile.stereo) { inputFile.buffer.put(it) } } @@ -136,18 +154,18 @@ fun main(args: Array<String>) { stream.close() inputFile.stream = null inputFile.closed = true - inputFile.modulator.flush(inputFile.power) { + inputFile.modulator.flush(inputFile.power, inputFile.stereo) { inputFile.buffer.put(it) } } else { val floats = buffer.slice(0, bytes).order(ByteOrder.LITTLE_ENDIAN).asFloatBuffer() var shouldFlush = true - inputFile.modulator.process(floats, inputFile.power) { + inputFile.modulator.process(floats, inputFile.power, inputFile.stereo) { inputFile.buffer.put(it) shouldFlush = false } if (shouldFlush) { - inputFile.modulator.flush(inputFile.power) { + inputFile.modulator.flush(inputFile.power, inputFile.stereo) { inputFile.buffer.put(it) } } @@ -199,7 +217,18 @@ fun main(args: Array<String>) { floatView.put(i, floatView.get(i) + z.x) floatView.put(i, floatView.get(i) + z.y) } - outputStream.write(outputBuffer.array()) + if (rfOutput) { + outputStream.write(outputBuffer.array()) + } else { + demodulator.process(floatView, stereo) { stereoPilot, it -> + if (stereoPilot != lastStereoPilot) { + println(if (stereoPilot) "stereo" else "mono") + } + lastStereoPilot = stereoPilot + buffer.order(ByteOrder.LITTLE_ENDIAN).asFloatBuffer().put(0, it.array()) + outputStream.write(buffer.array()) + } + } if (files.all { it.closed }) { break } diff --git a/src/main/kotlin/space/autistic/radio/fmsim/FmFullConstants.kt b/src/main/kotlin/space/autistic/radio/fmsim/FmFullConstants.kt index 5874166..6b92328 100644 --- a/src/main/kotlin/space/autistic/radio/fmsim/FmFullConstants.kt +++ b/src/main/kotlin/space/autistic/radio/fmsim/FmFullConstants.kt @@ -58,12 +58,12 @@ object FmFullConstants { -0.0010006913216784596f, ) - // chosen such that we can easily do 38kHz mixing in frequency (750*38k/300k = shift of 95 bins, where 750 comes - // from the 4/25 ratio 48k/300k i.e. 120*25/4) + // chosen such that we can easily do 38kHz mixing in frequency (1500*38k/300k = shift of 95 bins, where 1500 comes + // from the 4/25 ratio 48k/300k i.e. 240*25/4) // (the theoretical optimum, as per above, would be around 180) // (we could have fudged the carrier frequency a bit but we chose not to) // NOTE: latency = (data block size / 48000) seconds (84 -> 1.75 ms) - const val FFT_SIZE_LPF_48K_15K_3K1 = 120 + const val FFT_SIZE_LPF_48K_15K_3K1 = 2 * 120 const val FFT_OVERLAP_LPF_48K_15K_3K1 = 36 const val FFT_DATA_BLOCK_SIZE_LPF_48K_15K_3K1 = FFT_SIZE_LPF_48K_15K_3K1 - FFT_OVERLAP_LPF_48K_15K_3K1 @@ -74,9 +74,9 @@ object FmFullConstants { const val DECIMATION_48K_300K = 4 const val INTERPOLATION_48K_300K = 25 - const val IFFT_SIZE_48K_300K = FFT_SIZE_LPF_48K_15K_3K1 * INTERPOLATION_48K_300K / DECIMATION_48K_300K - const val IFFT_OVERLAP_48K_300K = FFT_OVERLAP_LPF_48K_15K_3K1 * INTERPOLATION_48K_300K / DECIMATION_48K_300K - const val IFFT_DATA_BLOCK_SIZE_48K_300K = IFFT_SIZE_48K_300K - IFFT_OVERLAP_48K_300K + const val FFT_SIZE_48K_300K = FFT_SIZE_LPF_48K_15K_3K1 * INTERPOLATION_48K_300K / DECIMATION_48K_300K + const val FFT_OVERLAP_48K_300K = FFT_OVERLAP_LPF_48K_15K_3K1 * INTERPOLATION_48K_300K / DECIMATION_48K_300K + const val FFT_DATA_BLOCK_SIZE_48K_300K = FFT_SIZE_48K_300K - FFT_OVERLAP_48K_300K // how many bins to shift for 38kHz mixing // assuming FFT_SIZE_LPF_48K_15K_3K1 *bins* (complex) @@ -88,12 +88,14 @@ object FmFullConstants { // since that number isn't exact, buffer an entire 19 cycles. const val BUFFER_SIZE_19K_300K = 300 + // using cosine is nicer val BUFFER_19K_300K = FloatArray(BUFFER_SIZE_19K_300K) { - 0.1f * sin(2 * PI * 19000.0 * it.toDouble() / 300000.0).toFloat() + 0.1f * cos(2 * PI * 19000.0 * it.toDouble() / 300000.0).toFloat() } // we want a carrier deviation of +-75kHz, at a sampling rate of 300kHz const val CORRECTION_FACTOR = (75000.0 / (300000.0 / (2.0 * PI))).toFloat() + const val INVERSE_CORRECTION_FACTOR = 1 / CORRECTION_FACTOR // these are used for "low/high" mixing const val CBUFFER_SIZE_100K_300K = 3 @@ -106,4 +108,7 @@ object FmFullConstants { 1f * cos(2 * PI * 100000.0 * index.toDouble() / 300000.0).toFloat() } } + + const val STEREO = true + const val MONO = false } \ No newline at end of file diff --git a/src/main/kotlin/space/autistic/radio/fmsim/FmFullDemodulator.kt b/src/main/kotlin/space/autistic/radio/fmsim/FmFullDemodulator.kt new file mode 100644 index 0000000..ce32c77 --- /dev/null +++ b/src/main/kotlin/space/autistic/radio/fmsim/FmFullDemodulator.kt @@ -0,0 +1,158 @@ +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 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 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 diff --git a/src/main/kotlin/space/autistic/radio/fmsim/FmFullModulator.kt b/src/main/kotlin/space/autistic/radio/fmsim/FmFullModulator.kt index 96ad186..7334c37 100644 --- a/src/main/kotlin/space/autistic/radio/fmsim/FmFullModulator.kt +++ b/src/main/kotlin/space/autistic/radio/fmsim/FmFullModulator.kt @@ -7,8 +7,10 @@ import space.autistic.radio.dsp.Biquad1stOrder import java.nio.FloatBuffer import java.util.function.Consumer import org.jtransforms.fft.FloatFFT_1D +import space.autistic.radio.complex.I import kotlin.math.max import kotlin.math.min +import kotlin.math.sqrt class FmFullModulator { private val leftPlusRight = FloatBuffer.allocate(FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1) @@ -24,31 +26,25 @@ class FmFullModulator { FmFullConstants.FM_PREEMPHASIS_A1_48K ) private val fft48kBuffer = FloatBuffer.allocate(FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1) - private val fir48kLpf = FloatBuffer.allocate(FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1) - private val mixingBuffer = FloatBuffer.allocate(FmFullConstants.IFFT_SIZE_48K_300K) - private val outputBuffer = FloatBuffer.allocate(2 * FmFullConstants.IFFT_DATA_BLOCK_SIZE_48K_300K) + private val mixingBuffer = FloatBuffer.allocate(FmFullConstants.FFT_SIZE_48K_300K) + private val outputBuffer = FloatBuffer.allocate(2 * FmFullConstants.FFT_DATA_BLOCK_SIZE_48K_300K) private val stereoPilot = FloatBuffer.wrap(FmFullConstants.BUFFER_19K_300K) - private var cycle = -1f + private val cycle19k = Vector2f(0f, 1f) private var lastSum = 0f init { - fir48kLpf.put(0, FmFullConstants.FIR_LPF_48K_15K_3K1) - Companion.fft48k.realForward(fir48kLpf.array()) - // pre-pad the buffers - while (leftPlusRight.position() < FmFullConstants.FFT_OVERLAP_LPF_48K_15K_3K1) { - leftPlusRight.put(0f) - leftMinusRight.put(0f) - } + leftPlusRight.position(FmFullConstants.FFT_OVERLAP_LPF_48K_15K_3K1) + leftMinusRight.position(FmFullConstants.FFT_OVERLAP_LPF_48K_15K_3K1) } /** - * Takes in samples at 48kHz, interleaved stereo and processes them for output. + * Takes in samples at 48kHz, interleaved stereo (even when set to MONO), and processes them for output. * - * Calls consumer with processed samples in I/Q format. + * Calls consumer with processed samples at 300kHz in I/Q format. */ - fun process(input: FloatBuffer, power: Float, consumer: Consumer<FloatBuffer>) { + fun process(input: FloatBuffer, power: Float, stereo: Boolean, consumer: Consumer<FloatBuffer>) { while (input.remaining() >= 2) { while (input.remaining() >= 2 && leftPlusRight.hasRemaining()) { // FIXME AGC (currently clamping/clipping) @@ -63,7 +59,7 @@ class FmFullModulator { mixingBuffer.put(i, 0f) } fft48kBuffer.put(0, leftPlusRight, 0, FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1) - Companion.fft48k.realForward(fft48kBuffer.array()) + fft48k.realForward(fft48kBuffer.array()) fft48kBuffer.array().forEachIndexed { index, fl -> fft48kBuffer.put( index, @@ -72,90 +68,83 @@ class FmFullModulator { } val z = Vector2f() val w = Vector2f() - for (i in 2 until FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1 step 2) { + for (i in 2 until (FmFullConstants.FREQUENCY_MIXING_BINS_38K and 1.inv()) step 2) { z.x = fft48kBuffer.get(i) z.y = fft48kBuffer.get(i + 1) w.x = fir48kLpf.get(i) w.y = fir48kLpf.get(i + 1) z.cmul(w) - fft48kBuffer.put(i, z.x) - fft48kBuffer.put(i + 1, z.y) + mixingBuffer.put(i, z.x) + mixingBuffer.put(i + 1, z.y) } - fft48kBuffer.put(0, fft48kBuffer.get(0) * fir48kLpf.get(0)) - fft48kBuffer.put(1, fft48kBuffer.get(1) * fir48kLpf.get(1)) - // copy only around 19kHz of bandwidth - mixingBuffer.put(0, fft48kBuffer, 0, FmFullConstants.FREQUENCY_MIXING_BINS_38K or 1) - // zero out nyquist frequency bucket - mixingBuffer.put(1, 0f) - fft48kBuffer.put(0, leftMinusRight, 0, FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1) - Companion.fft48k.realForward(fft48kBuffer.array()) - fft48kBuffer.array().forEachIndexed { index, fl -> - fft48kBuffer.put( - index, - 0.2f / FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1 * fl + mixingBuffer.put(0, fft48kBuffer.get(0) * fir48kLpf.get(0)) + if (stereo) { + fft48kBuffer.put(0, leftMinusRight, 0, FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1) + fft48k.realForward(fft48kBuffer.array()) + fft48kBuffer.array().forEachIndexed { index, fl -> + fft48kBuffer.put( + index, + 0.2f / FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1 * fl + ) + } + val base = FmFullConstants.FREQUENCY_MIXING_BINS_38K * 2 + for (i in 2 until (FmFullConstants.FREQUENCY_MIXING_BINS_38K and 1.inv()) step 2) { + z.x = fft48kBuffer.get(i) + z.y = fft48kBuffer.get(i + 1) + w.x = fir48kLpf.get(i) + w.y = fir48kLpf.get(i + 1) + z.cmul(w) + mixingBuffer.put(base + i, z.x) + mixingBuffer.put(base + i + 1, z.y) + } + mixingBuffer.put(base, fft48kBuffer.get(0) * fir48kLpf.get(0)) + // cycle (phase offset) is frequency-doubled the 19k carrier + // but we need to add a 90deg rotation because ??? + // TODO check if this is mathematically sound + val cycle = cycle19k.cmul(cycle19k, Vector2f()).cmul(I) + // bandwidth we care about is about half of 38k, so just, well, half it + for (i in 2 until (FmFullConstants.FREQUENCY_MIXING_BINS_38K and 1.inv()) step 2) { + z.x = mixingBuffer.get(base + i) + z.y = mixingBuffer.get(base + i + 1) + // we also need the conjugate + z.conjugate(w) + z.cmul(cycle) + w.cmul(cycle) + mixingBuffer.put(base + i, z.x) + mixingBuffer.put(base + i + 1, z.y) + mixingBuffer.put(base - i, mixingBuffer.get(base - i) + w.x) + mixingBuffer.put(base - i + 1, mixingBuffer.get(base - i + 1) + w.y) + } + // handle 38kHz itself + z.x = mixingBuffer.get(base) + z.y = mixingBuffer.get(base + 1) + z.cmul(cycle) + mixingBuffer.put(base, z.x) + mixingBuffer.put(base + 1, z.y) + // add pilot + mixingBuffer.put( + FmFullConstants.FREQUENCY_MIXING_BINS_38K, + 75f / FmFullConstants.FFT_SIZE_48K_300K * cycle19k.x ) + mixingBuffer.put( + FmFullConstants.FREQUENCY_MIXING_BINS_38K + 1, + 75f / FmFullConstants.FFT_SIZE_48K_300K * cycle19k.y + ) + // phase correction factors (due to dropping 225 bins) + cycle19k.cmul(I.conjugate()) } - for (i in 2 until FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1 step 2) { - z.x = fft48kBuffer.get(i) - z.y = fft48kBuffer.get(i + 1) - w.x = fir48kLpf.get(i) - w.y = fir48kLpf.get(i + 1) - z.cmul(w) - fft48kBuffer.put(i, z.x) - fft48kBuffer.put(i + 1, z.y) - } - fft48kBuffer.put(0, fft48kBuffer.get(0) * fir48kLpf.get(0)) - // (unnecessary) - //fft48kBuffer.put(1, fft48kBuffer.get(1) * fir48kLpf.get(1)) - mixingBuffer.put( - FmFullConstants.FREQUENCY_MIXING_BINS_38K * 2 + 2, - fft48kBuffer, - 2, - // number of floats to copy - // bins are complex, so this halves the bins (~19kHz bandwidth) - // length should be even (for an exact number of complex bins) - FmFullConstants.FREQUENCY_MIXING_BINS_38K and 1.inv() - ) - // the actual 38k bin is at this offset, account for jt convention (buf[0 until 3] = R0,Rn,R1) - mixingBuffer.put(FmFullConstants.FREQUENCY_MIXING_BINS_38K * 2, fft48kBuffer.get(0)) - val base = FmFullConstants.FREQUENCY_MIXING_BINS_38K * 2 - // phase correction factor (due to dropping 150 bins) - // TODO figure out if phase is correct - cycle = -cycle - // bandwidth we care about is about half of 38k, so just, well, half it - for (i in 2 until FmFullConstants.FREQUENCY_MIXING_BINS_38K step 2) { - z.x = mixingBuffer.get(base + i) - z.y = mixingBuffer.get(base + i + 1) - // we also need the conjugate - z.conjugate(w) - mixingBuffer.put(base + i, z.y * -cycle) - mixingBuffer.put(base + i + 1, z.x * cycle) - mixingBuffer.put(base - i, mixingBuffer.get(base - i - 2) - w.y * cycle) - mixingBuffer.put(base - i + 1, mixingBuffer.get(base - i - 1) + w.x * cycle) - } - // handle 38kHz itself - z.x = mixingBuffer.get(base) - z.y = mixingBuffer.get(base + 1) - mixingBuffer.put(base, z.y * -cycle) - mixingBuffer.put(base + 1, z.x * cycle) - // (don't need to handle nyquist) // mark data block as processed leftPlusRight.position(FmFullConstants.FFT_DATA_BLOCK_SIZE_LPF_48K_15K_3K1) leftMinusRight.position(FmFullConstants.FFT_DATA_BLOCK_SIZE_LPF_48K_15K_3K1) leftPlusRight.compact() leftMinusRight.compact() - Companion.fft300k.realInverse(mixingBuffer.array(), false) + fft300k.realInverse(mixingBuffer.array(), false) outputBuffer.clear() var sum = lastSum - for (i in FmFullConstants.IFFT_OVERLAP_48K_300K until FmFullConstants.IFFT_SIZE_48K_300K) { - if (!stereoPilot.hasRemaining()) { - stereoPilot.clear() - } - val result = mixingBuffer.get(i) + stereoPilot.get() - sum += result * FmFullConstants.CORRECTION_FACTOR - val sin = org.joml.Math.sin(sum) - outputBuffer.put(sin * power) + for (i in FmFullConstants.FFT_OVERLAP_48K_300K until FmFullConstants.FFT_SIZE_48K_300K) { + sum += mixingBuffer.get(i) * FmFullConstants.CORRECTION_FACTOR outputBuffer.put(org.joml.Math.cos(sum) * power) + outputBuffer.put(org.joml.Math.sin(sum) * power) } lastSum = sum % (2 * Math.PI).toFloat() outputBuffer.clear() @@ -165,12 +154,18 @@ class FmFullModulator { input.compact() } - fun flush(power: Float, consumer: Consumer<FloatBuffer>) { - process(FloatBuffer.allocate(2 * leftPlusRight.remaining()), power, consumer) + fun flush(power: Float, stereo: Boolean, consumer: Consumer<FloatBuffer>) { + process(FloatBuffer.allocate(2 * leftPlusRight.remaining()), power, stereo, consumer) } companion object { private val fft48k = FloatFFT_1D(FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1.toLong()) - private val fft300k = FloatFFT_1D(FmFullConstants.IFFT_SIZE_48K_300K.toLong()) + private val fft300k = FloatFFT_1D(FmFullConstants.FFT_SIZE_48K_300K.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 |