diff options
Diffstat (limited to 'src/main/kotlin/space/autistic/radio/fmsim')
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 |