First public commit

3 files changed, 289 insertions, 0 deletions

diff --git a/src/main/kotlin/space/autistic/radio/fmsim/FmFullConstants.kt b/src/main/kotlin/space/autistic/radio/fmsim/FmFullConstants.kt
new file mode 100644
index 0000000..5874166
--- /dev/null
+++ b/src/main/kotlin/space/autistic/radio/fmsim/FmFullConstants.kt
@@ -0,0 +1,109 @@
+package space.autistic.radio.fmsim
+
+import kotlin.math.PI
+import kotlin.math.cos
+import kotlin.math.sin
+
+object FmFullConstants {
+    // tau = 75us, fh = 20396.25Hz
+    const val FM_PREEMPAHSIS_B0_48K = 6.7639647f
+    const val FM_PREEMPHASIS_B1_48K = -4.975628f
+
+    /* const val FM_PREEMPHASIS_A0_48K = 1f */
+    const val FM_PREEMPHASIS_A1_48K = 0.78833646f
+
+    const val FM_DEEMPAHSIS_B0_48K = 1f / FM_PREEMPAHSIS_B0_48K
+    const val FM_DEEMPHASIS_B1_48K = FM_PREEMPHASIS_A1_48K / FM_PREEMPAHSIS_B0_48K
+
+    /* const val FM_DEEMPHASIS_A0_48K = 1f */
+    const val FM_DEEMPHASIS_A1_48K = FM_PREEMPHASIS_B1_48K / FM_PREEMPAHSIS_B0_48K
+
+    val FIR_LPF_48K_15K_3K1 = floatArrayOf(
+        -0.0010006913216784596f,
+        0.001505308784544468f,
+        -2.625857350794219e-18f,
+        -0.002777613466605544f,
+        0.0030173989944159985f,
+        0.002290070755407214f,
+        -0.008225799538195133f,
+        0.004239063244313002f,
+        0.010359899140894413f,
+        -0.017650796100497246f,
+        1.510757873119297e-17f,
+        0.029305754229426384f,
+        -0.02889496460556984f,
+        -0.020366130396723747f,
+        0.07103750854730606f,
+        -0.03811456635594368f,
+        -0.10945471376180649f,
+        0.29212409257888794f,
+        0.6252123713493347f,
+        0.29212409257888794f,
+        -0.10945471376180649f,
+        -0.03811456635594368f,
+        0.07103750854730606f,
+        -0.020366130396723747f,
+        -0.02889496460556984f,
+        0.029305754229426384f,
+        1.510757873119297e-17f,
+        -0.017650796100497246f,
+        0.010359899140894413f,
+        0.004239063244313002f,
+        -0.008225799538195133f,
+        0.002290070755407214f,
+        0.0030173989944159985f,
+        -0.002777613466605544f,
+        -2.625857350794219e-18f,
+        0.001505308784544468f,
+        -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)
+    // (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_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
+
+    init {
+        assert(FFT_OVERLAP_LPF_48K_15K_3K1 >= FIR_LPF_48K_15K_3K1.size - 1)
+    }
+
+    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
+
+    // how many bins to shift for 38kHz mixing
+    // assuming FFT_SIZE_LPF_48K_15K_3K1 *bins* (complex)
+    // 19 / 150 is the ratio between 38k/300k
+    const val FREQUENCY_MIXING_BINS_38K =
+        FFT_SIZE_LPF_48K_15K_3K1 * INTERPOLATION_48K_300K / DECIMATION_48K_300K * 19 / 150
+
+    // a single cycle of a 19kHz signal takes (1/19k)/(1/300k) or 300k/19k samples.
+    // since that number isn't exact, buffer an entire 19 cycles.
+    const val BUFFER_SIZE_19K_300K = 300
+
+    val BUFFER_19K_300K = FloatArray(BUFFER_SIZE_19K_300K) {
+        0.1f * sin(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()
+
+    // these are used for "low/high" mixing
+    const val CBUFFER_SIZE_100K_300K = 3
+
+    val CBUFFER_100K_300K = FloatArray(2 * CBUFFER_SIZE_100K_300K) {
+        val index = it / 2
+        if (it and 1 == 0) {
+            1f * sin(2 * PI * 100000.0 * index.toDouble() / 300000.0).toFloat()
+        } else {
+            1f * cos(2 * PI * 100000.0 * index.toDouble() / 300000.0).toFloat()
+        }
+    }
+}
\ No newline at end of file
diff --git a/src/main/kotlin/space/autistic/radio/fmsim/FmFullMixer.kt b/src/main/kotlin/space/autistic/radio/fmsim/FmFullMixer.kt
new file mode 100644
index 0000000..654d50f
--- /dev/null
+++ b/src/main/kotlin/space/autistic/radio/fmsim/FmFullMixer.kt
@@ -0,0 +1,4 @@
+package space.autistic.radio.fmsim
+
+class FmFullMixer {
+}
\ 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
new file mode 100644
index 0000000..96ad186
--- /dev/null
+++ b/src/main/kotlin/space/autistic/radio/fmsim/FmFullModulator.kt
@@ -0,0 +1,176 @@
+package space.autistic.radio.fmsim
+
+import org.joml.Vector2f
+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.Consumer
+import org.jtransforms.fft.FloatFFT_1D
+import kotlin.math.max
+import kotlin.math.min
+
+class FmFullModulator {
+    private val leftPlusRight = FloatBuffer.allocate(FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1)
+    private val leftMinusRight = FloatBuffer.allocate(FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1)
+    private val biquadLeft = Biquad1stOrder(
+        FmFullConstants.FM_PREEMPAHSIS_B0_48K,
+        FmFullConstants.FM_PREEMPHASIS_B1_48K,
+        FmFullConstants.FM_PREEMPHASIS_A1_48K
+    )
+    private val biquadRight = Biquad1stOrder(
+        FmFullConstants.FM_PREEMPAHSIS_B0_48K,
+        FmFullConstants.FM_PREEMPHASIS_B1_48K,
+        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 stereoPilot = FloatBuffer.wrap(FmFullConstants.BUFFER_19K_300K)
+
+    private var cycle = -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)
+        }
+    }
+
+    /**
+     * Takes in samples at 48kHz, interleaved stereo and processes them for output.
+     *
+     * Calls consumer with processed samples in I/Q format.
+     */
+    fun process(input: FloatBuffer, power: Float, consumer: Consumer<FloatBuffer>) {
+        while (input.remaining() >= 2) {
+            while (input.remaining() >= 2 && leftPlusRight.hasRemaining()) {
+                // FIXME AGC (currently clamping/clipping)
+                val left = min(max(biquadLeft.process(input.get()), -1f), 1f)
+                val right = min(max(biquadRight.process(input.get()), -1f), 1f)
+                leftPlusRight.put(left + right)
+                leftMinusRight.put(left - right)
+            }
+            if (!leftPlusRight.hasRemaining()) {
+                // zero the mixing buffer
+                for (i in 0 until mixingBuffer.capacity()) {
+                    mixingBuffer.put(i, 0f)
+                }
+                fft48kBuffer.put(0, leftPlusRight, 0, FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1)
+                Companion.fft48k.realForward(fft48kBuffer.array())
+                fft48kBuffer.array().forEachIndexed { index, fl ->
+                    fft48kBuffer.put(
+                        index,
+                        0.4f / FmFullConstants.FFT_SIZE_LPF_48K_15K_3K1 * fl
+                    )
+                }
+                val z = Vector2f()
+                val w = Vector2f()
+                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))
+                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
+                    )
+                }
+                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)
+                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)
+                    outputBuffer.put(org.joml.Math.cos(sum) * power)
+                }
+                lastSum = sum % (2 * Math.PI).toFloat()
+                outputBuffer.clear()
+                consumer.accept(outputBuffer)
+            }
+        }
+        input.compact()
+    }
+
+    fun flush(power: Float, consumer: Consumer<FloatBuffer>) {
+        process(FloatBuffer.allocate(2 * leftPlusRight.remaining()), power, 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())
+    }
+}
\ No newline at end of file