even more stuff

2026-04-19 06:42:43 +00:00 · 2022-06-15 16:08:54 +02:00
parent 343ec6ca1c
commit d1318d3a0f
156 changed files with 24826 additions and 0 deletions
--- a/core/src/dsp/demod/broadcast_fm.h
+++ b/core/src/dsp/demod/broadcast_fm.h
@@ -0,0 +1,156 @@
+#pragma once
+#include "fm.h"
+#include "../taps/band_pass.h"
+#include "../filter/fir.h"
+#include "../loop/pll.h"
+#include "../convert/l_r_to_stereo.h"
+#include "../convert/real_to_complex.h"
+#include "../convert/complex_to_real.h"
+#include "../math/conjugate.h"
+#include "../math/delay.h"
+#include "../math/multiply.h"
+#include "../math/add.h"
+#include "../math/subtract.h"
+
+namespace dsp::demod {
+    class BroadcastFM : public Processor<complex_t, stereo_t> {
+        using base_type = Processor<complex_t, stereo_t>;
+    public:
+        BroadcastFM() {}
+
+        BroadcastFM(stream<complex_t>* in, double deviation, double samplerate, bool stereo = true) { init(in, deviation, samplerate, stereo); }
+
+        ~BroadcastFM() {
+            if (!base_type::_block_init) { return; }
+            base_type::stop();
+            buffer::free(lmr);
+            buffer::free(l);
+            buffer::free(r);
+        }
+
+        virtual void init(stream<complex_t>* in, double deviation, double samplerate, bool stereo = true) {
+            _deviation = deviation;
+            _samplerate = samplerate;
+            _stereo = stereo;
+            
+            demod.init(NULL, _deviation, _samplerate);
+            pilotFirTaps = taps::bandPass<complex_t>(18750.0, 19250.0, 3000.0, _samplerate);
+            pilotFir.init(NULL, pilotFirTaps);
+            rtoc.init(NULL);
+            pilotPLL.init(NULL, 0.1/*TODO: adapt to samplerate*/, 0.0, math::freqToOmega(19000.0, _samplerate), math::freqToOmega(18750.0, _samplerate), math::freqToOmega(19250.0, _samplerate));
+            delay.init(NULL, pilotFirTaps.size / 2.0);
+
+            lmr = buffer::alloc<float>(STREAM_BUFFER_SIZE);
+            l = buffer::alloc<float>(STREAM_BUFFER_SIZE);
+            r = buffer::alloc<float>(STREAM_BUFFER_SIZE);
+
+            base_type::init(in);
+        }
+
+        void setDeviation(double deviation) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            _deviation = deviation;
+            demod.setDeviation(_deviation, _samplerate);
+        }
+
+        void setSamplerate(double samplerate) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            base_type::tempStop();
+            _samplerate = samplerate;
+
+            demod.setDeviation(_deviation, _samplerate);
+            taps::free(pilotFirTaps);
+            pilotFirTaps = taps::bandPass<complex_t>(18750.0, 19250.0, 3000.0, samplerate);
+            pilotFir.setTaps(pilotFirTaps);
+            pilotPLL.setFrequencyLimits(math::freqToOmega(18750.0, _samplerate), math::freqToOmega(19250.0, _samplerate));
+            pilotPLL.setInitialFreq(math::freqToOmega(19000.0, _samplerate));
+            delay.setDelay(pilotFirTaps.size / 2);
+
+            reset();
+            base_type::tempStart();
+        }
+
+        void setStereo(bool stereo) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            base_type::tempStop();
+            _stereo = stereo;
+            reset();
+            base_type::tempStart();
+        }
+
+        void reset() {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            base_type::tempStop();
+            demod.reset();
+            pilotFir.reset();
+            pilotPLL.reset();
+            delay.reset();
+            base_type::tempStart();
+        }
+
+        inline int process(int count, complex_t* in, stereo_t* out) {
+            // Demodulate
+            demod.process(count, in, demod.out.writeBuf);
+            if (_stereo) {
+                // Convert to complex
+                rtoc.process(count, demod.out.writeBuf, rtoc.out.writeBuf);
+
+                // Filter out pilot and run through PLL
+                pilotFir.process(count, rtoc.out.writeBuf, pilotFir.out.writeBuf);
+                pilotPLL.process(count, pilotFir.out.writeBuf, pilotPLL.out.writeBuf);
+
+                // Conjugate PLL output to down convert the L-R signal
+                math::Conjugate::process(count, pilotPLL.out.writeBuf, pilotPLL.out.writeBuf);
+                math::Multiply<dsp::complex_t>::process(count, rtoc.out.writeBuf, pilotPLL.out.writeBuf, rtoc.out.writeBuf);
+
+                // Convert output back to real for further processing
+                convert::ComplexToReal::process(count, rtoc.out.writeBuf, lmr);
+
+                // Do L = (L+R) + (L-R), R = (L+R) - (L-R)
+                math::Add<float>::process(count, demod.out.writeBuf, lmr, l);
+                math::Subtract<float>::process(count, demod.out.writeBuf, lmr, r);
+
+                // Interleave into stereo
+                convert::LRToStereo::process(count, l, r, out);
+            }
+            else {
+                // Interleave raw MPX to stereo
+                convert::LRToStereo::process(count, demod.out.writeBuf, demod.out.writeBuf, out);
+            }
+
+            return count;
+        }
+
+        int run() {
+            int count = base_type::_in->read();
+            if (count < 0) { return -1; }
+
+            process(count, base_type::_in->readBuf, base_type::out.writeBuf);
+
+            base_type::_in->flush();
+            if (!base_type::out.swap(count)) { return -1; }
+            return count;
+        }
+
+    protected:
+        double _deviation;
+        double _samplerate;
+        bool _stereo;
+
+        FM demod;
+        tap<complex_t> pilotFirTaps;
+        filter::FIR<complex_t, complex_t> pilotFir;
+        convert::RealToComplex rtoc;
+        loop::PLL pilotPLL;
+        math::Delay<float> delay;
+
+        float* lmr;
+        float* l;
+        float* r;
+        
+    };
+}