FIR Polyphase filterbank channelizer prototype implementation

- Can now handle several 200khz FM streams with SDRPlay at 8Mhz+ on my
old 2010 Macbook Pro :)
- Demod bandwidth max now limited to 400khz, temporary until alternate
path for high-bandwidth is available

src/CubicSDRDefs.h

@@ -34,3 +34,5 @@ const char filePathSeparator =
 #define DEFAULT_DEMOD_BW 200000
 
 #define DEFAULT_WATERFALL_LPS 30
+
+#define CHANNELIZER_RATE_MAX 400000

src/demod/DemodulatorPreThread.cpp

@@ -155,18 +155,20 @@ void DemodulatorPreThread::run() {
         }
 
         if (!initialized) {
+            inp->decRefCount();
             continue;
         }
 
         // Requested frequency is not center, shift it into the center!
         if ((params.frequency - inp->frequency) != shiftFrequency || rateChanged) {
             shiftFrequency = params.frequency - inp->frequency;
-            if (abs(shiftFrequency) <= (int) ((double) (wxGetApp().getSampleRate() / 2) * 1.5)) {
-                nco_crcf_set_frequency(freqShifter, (2.0 * M_PI) * (((double) abs(shiftFrequency)) / ((double) wxGetApp().getSampleRate())));
+            if (abs(shiftFrequency) <= (int) ((double) (inp->sampleRate / 2) * 1.5)) {
+                nco_crcf_set_frequency(freqShifter, (2.0 * M_PI) * (((double) abs(shiftFrequency)) / ((double) inp->sampleRate)));
             }
         }
 
-        if (abs(shiftFrequency) > (int) ((double) (wxGetApp().getSampleRate() / 2) * 1.5)) {
+        if (abs(shiftFrequency) > (int) ((double) (inp->sampleRate / 2) * 1.5)) {
+            inp->decRefCount();
             continue;
         }
 

src/sdr/SDREnumerator.cpp

@@ -124,7 +124,7 @@ std::vector<SDRDeviceInfo *> *SDREnumerator::enumerate_devices(std::string remot
         
         if (isRemote) {
             wxGetApp().sdrEnumThreadNotify(SDREnumerator::SDR_ENUM_MESSAGE, "Querying remote " + remoteAddr + " device #" + std::to_string(i));
-            deviceArgs["remote"] = remoteAddr;
+//            deviceArgs["remote"] = remoteAddr;
             if (deviceArgs.count("rtl") != 0) {
                 streamArgs["remote:mtu"] = "8192";
                 streamArgs["remote:format"] = "CS8";

src/sdr/SDRPostThread.cpp

@@ -7,24 +7,10 @@
 
 SDRPostThread::SDRPostThread() : IOThread(),
         iqDataInQueue(NULL), iqDataOutQueue(NULL), iqVisualQueue(NULL), dcFilter(NULL){
-	
 	swapIQ.store(false);
-
-    // create a lookup table
-    for (unsigned int i = 0; i <= 0xffff; i++) {
-        liquid_float_complex tmp,tmp_swap;
-# if (__BYTE_ORDER == __LITTLE_ENDIAN)
-        tmp_swap.imag = tmp.real = (float(i & 0xff) - 127.4f) * (1.0f/128.0f);
-        tmp_swap.real = tmp.imag = (float(i >> 8) - 127.4f) * (1.0f/128.0f);
-        _lut.push_back(tmp);
-        _lut_swap.push_back(tmp_swap);
-#else // BIG_ENDIAN
-        tmp_swap.imag = tmp.real = (float(i >> 8) - 127.4f) * (1.0f/128.0f);
-        tmp_swap.real = tmp.imag = (float(i & 0xff) - 127.4f) * (1.0f/128.0f);
-        _lut.push_back(tmp);
-        _lut_swap.push_back(tmp_swap);
-#endif
-    }
+    numChannels = 0;
+    channelizer = NULL;
+    sampleRate = 0;
 }
 
 SDRPostThread::~SDRPostThread() {
@@ -61,7 +47,7 @@ bool SDRPostThread::getSwapIQ() {
 void SDRPostThread::run() {
 #ifdef __APPLE__
     pthread_t tID = pthread_self();  // ID of this thread
-    int priority = sched_get_priority_max( SCHED_FIFO) - 1;
+    int priority = sched_get_priority_max( SCHED_FIFO);
     sched_param prio = {priority}; // scheduling priority of thread
     pthread_setschedparam(tID, SCHED_FIFO, &prio);
 #endif
@@ -79,6 +65,14 @@ void SDRPostThread::run() {
     std::vector<liquid_float_complex> dataOut;
     
     iqDataInQueue->set_max_num_items(0);
+
+    std::vector<long long> chanCenters;
+    long long chanBw;
+    
+    int nRunDemods = 0;
+    std::vector<DemodulatorInstance *> runDemods;
+    std::vector<int> demodChannel;
+    std::vector<int> demodChannelActive;
     
     while (!terminated) {
         SDRThreadIQData *data_in;
@@ -86,17 +80,36 @@ void SDRPostThread::run() {
         iqDataInQueue->pop(data_in);
         //        std::lock_guard < std::mutex > lock(data_in->m_mutex);
         
-        if (data_in && data_in->data.size()) {
-            int dataSize = data_in->data.size()/2;
-
-            if (dataSize > dataOut.capacity()) {
-                dataOut.reserve(dataSize);
+        if (data_in && data_in->data.size() && data_in->numChannels) {
+            if (numChannels != data_in->numChannels || sampleRate != data_in->sampleRate) {
+                numChannels = data_in->numChannels;
+                sampleRate = data_in->sampleRate;
+                std::cout << "Initializing post-process FIR polyphase filterbank channelizer with " << numChannels << " channels." << std::endl;
+                if (channelizer) {
+                    firpfbch2_crcf_destroy(channelizer);
+                }
+                channelizer = firpfbch2_crcf_create_kaiser(LIQUID_ANALYZER, numChannels, 1, 60);
+                
+                chanBw = (data_in->sampleRate / numChannels) * 2;
+                
+                chanCenters.resize(numChannels);
+                demodChannelActive.resize(numChannels);
+                
+                // firpfbch2 returns 2x sample rate per channel
+                // so, max demodulation without gaps is 1/2 chanBw ..?
+                std::cout << "Channel bandwidth spacing: " << (chanBw/2) << " actual bandwidth: " << chanBw << std::endl;
             }
-            if (dataSize != dataOut.size()) {
-                dataOut.resize(dataSize);
-            }
-
             
+            int dataSize = data_in->data.size();
+            int outSize = data_in->data.size()*2;
+
+            if (outSize > dataOut.capacity()) {
+                dataOut.reserve(outSize);
+            }
+            if (outSize != dataOut.size()) {
+                dataOut.resize(outSize);
+            }
+
             //            if (swapIQ) {
             //                for (int i = 0; i < dataSize; i++) {
             //                    fpData[i] = _lut_swap[*((uint16_t*)&data_in->data[2*i])];
@@ -107,80 +120,57 @@ void SDRPostThread::run() {
             //                }
             //            }
 
+            if (dataSize > fpData.capacity()) {
+                fpData.reserve(dataSize);
+            }
+            if (dataSize != fpData.size()) {
+                fpData.resize(dataSize);
+            }
+
             if (data_in->dcCorrected) {
-                for (int i = 0; i < dataSize; i++) {
-                    dataOut[i].real = data_in->data[i*2];
-                    dataOut[i].imag = data_in->data[i*2+1];
-                }
+                fpData.assign(data_in->data.begin(), data_in->data.end());
             } else {
-                if (dataSize > fpData.capacity()) {
-                    fpData.reserve(dataSize);
-                }
-                if (dataSize != fpData.size()) {
-                    fpData.resize(dataSize);
-                }
-
-                for (int i = 0; i < dataSize; i++) {
-                    fpData[i].real = data_in->data[i*2];
-                    fpData[i].imag = data_in->data[i*2+1];
-                }
-            
-                iirfilt_crcf_execute_block(dcFilter, &fpData[0], dataSize, &dataOut[0]);
+                iirfilt_crcf_execute_block(dcFilter, &data_in->data[0], dataSize, &fpData[0]);
             }
             
-            if (iqVisualQueue != NULL && !iqVisualQueue->full()) {
-                DemodulatorThreadIQData *visualDataOut = visualDataBuffers.getBuffer();
-                visualDataOut->setRefCount(1);
-                
-                int num_vis_samples = dataOut.size();
+            if (iqVisualQueue != NULL || iqDataOutQueue != NULL) {
+                int num_vis_samples = fpData.size();
 
-//                if (visualDataOut->data.size() < num_vis_samples) {
-//                    if (visualDataOut->data.capacity() < num_vis_samples) {
-//                        visualDataOut->data.reserve(num_vis_samples);
-//                    }
-//                    visualDataOut->data.resize(num_vis_samples);
-//                }
-//
-                visualDataOut->frequency = data_in->frequency;
-                visualDataOut->sampleRate = data_in->sampleRate;
-                visualDataOut->data.assign(dataOut.begin(), dataOut.begin() + num_vis_samples);
+                bool doIQVis = iqVisualQueue && !iqVisualQueue->full();
+                bool doIQOut = iqDataOutQueue != NULL;
                 
-                iqVisualQueue->push(visualDataOut);
+                DemodulatorThreadIQData *iqDataOut = visualDataBuffers.getBuffer();
+                iqDataOut->setRefCount((doIQVis?1:0) + (doIQOut?1:0));
+                
+                iqDataOut->frequency = data_in->frequency;
+                iqDataOut->sampleRate = data_in->sampleRate;
+                iqDataOut->data.assign(fpData.begin(), fpData.begin() + num_vis_samples);
+
+                if (doIQVis) {
+                    iqVisualQueue->push(iqDataOut);
+                }
+                
+                if (doIQOut) {
+                    iqDataOutQueue->push(iqDataOut);
+                }
             }
-            
+
             busy_demod.lock();
             
-            int activeDemods = 0;
-            bool pushedData = false;
-            
-            if (demodulators.size() || iqDataOutQueue != NULL) {
+            // Find active demodulators
+            if (demodulators.size()) {
+                // In range?
                 std::vector<DemodulatorInstance *>::iterator demod_i;
-                for (demod_i = demodulators.begin(); demod_i != demodulators.end(); demod_i++) {
-                    DemodulatorInstance *demod = *demod_i;
-                    if (demod->getFrequency() != data_in->frequency
-                        && abs(data_in->frequency - demod->getFrequency()) > (wxGetApp().getSampleRate() / 2)) {
-                        continue;
-                    }
-                    activeDemods++;
-                }
-                
-                if (iqDataOutQueue != NULL) {
-                    activeDemods++;
-                }
-                
-                DemodulatorThreadIQData *demodDataOut = buffers.getBuffer();
-                
-                //                    std::lock_guard < std::mutex > lock(demodDataOut->m_mutex);
-                demodDataOut->frequency = data_in->frequency;
-                demodDataOut->sampleRate = data_in->sampleRate;
-                demodDataOut->setRefCount(activeDemods);
-                demodDataOut->data.assign(dataOut.begin(), dataOut.end());
+
+                nRunDemods = 0;
                 
                 for (demod_i = demodulators.begin(); demod_i != demodulators.end(); demod_i++) {
                     DemodulatorInstance *demod = *demod_i;
                     DemodulatorThreadInputQueue *demodQueue = demod->getIQInputDataPipe();
-                    
-                    if (abs(data_in->frequency - demod->getFrequency()) > (wxGetApp().getSampleRate() / 2)) {
+                
+                    // not in range?
+                    if (abs(data_in->frequency - demod->getFrequency()) > (data_in->sampleRate / 2)) {
+                        // deactivate if active
                         if (demod->isActive() && !demod->isFollow() && !demod->isTracking()) {
                             demod->setActive(false);
                             DemodulatorThreadIQData *dummyDataOut = new DemodulatorThreadIQData;
@@ -189,10 +179,12 @@ void SDRPostThread::run() {
                             demodQueue->push(dummyDataOut);
                         }
                         
+                        // follow if follow mode
                         if (demod->isFollow() && wxGetApp().getFrequency() != demod->getFrequency()) {
                             wxGetApp().setFrequency(demod->getFrequency());
+                            demod->setFollow(false);
                         }
-                    } else if (!demod->isActive()) {
+                    } else if (!demod->isActive()) { // in range, activate if not activated
                         demod->setActive(true);
                         if (wxGetApp().getDemodMgr().getLastActiveDemodulator() == NULL) {
                             wxGetApp().getDemodMgr().setActiveDemodulator(demod);
@@ -202,26 +194,93 @@ void SDRPostThread::run() {
                     if (!demod->isActive()) {
                         continue;
                     }
-                    if (demod->isFollow()) {
-                        demod->setFollow(false);
+                    
+                    // Add to the current run
+                    if (nRunDemods == runDemods.size()) {
+                        runDemods.push_back(demod);
+                        demodChannel.push_back(-1);
+                    } else {
+                        runDemods[nRunDemods] = demod;
+                        demodChannel[nRunDemods] = -1;
+                    }
+                    nRunDemods++;
+                }
+                
+                // calculate channel center frequencies, todo: cache
+                for (int i = 0; i < numChannels/2; i++) {
+                    int ofs = ((chanBw/2) * i);
+                    chanCenters[i] = data_in->frequency + ofs;
+                    chanCenters[i+(numChannels/2)] = data_in->frequency - (data_in->sampleRate/2) + ofs;
+                }
+                
+                // channelize data
+                // firpfbch2 output rate is 2 x ( input rate / channels )
+                for (int i = 0, iMax = dataSize; i < iMax; i+=numChannels/2) {
+                    firpfbch2_crcf_execute(channelizer, &fpData[i], &dataOut[i * 2]);
+                }
+
+                for (int i = 0, iMax = numChannels; i < iMax; i++) {
+                    demodChannelActive[i] = 0;
+                }
+                
+                // Find nearest channel for each demodulator
+                for (int i = 0; i < nRunDemods; i++) {
+                    DemodulatorInstance *demod = runDemods[i];
+                    long long minDelta = data_in->sampleRate;
+                    for (int j = 0, jMax = numChannels; j < jMax; j++) {
+                        // Distance from channel center to demod center
+                        long long fdelta = abs(demod->getFrequency() - chanCenters[j]);
+                        if (fdelta < minDelta) {
+                            minDelta = fdelta;
+                            demodChannel[i] = j;
+                        }
+                    }
+                }
+
+                for (int i = 0; i < nRunDemods; i++) {
+                    // cache channel usage refcounts
+                    if (demodChannel[i] >= 0) {
+                        demodChannelActive[demodChannel[i]]++;
+                    }
+                }
+
+                
+                // Run channels
+                for (int i = 0; i < numChannels; i++) {
+                    if (demodChannelActive[i] == 0) {
+                        // Nothing using this channel, skip
+                        continue;
+                    }
+
+                    DemodulatorThreadIQData *demodDataOut = buffers.getBuffer();
+                    demodDataOut->setRefCount(demodChannelActive[i]);
+                    demodDataOut->frequency = chanCenters[i];
+                    demodDataOut->sampleRate = chanBw;
+                    
+                    // Calculate channel buffer size
+                    int chanDataSize = (outSize/numChannels);
+
+                    if (demodDataOut->data.size() != chanDataSize) {
+                        if (demodDataOut->data.capacity() < chanDataSize) {
+                            demodDataOut->data.reserve(chanDataSize);
+                        }
+                        demodDataOut->data.resize(chanDataSize);
+                    }
+
+                    // prepare channel data buffer
+                    for (int j = 0, idx = i; j < chanDataSize; j++) {
+                        idx += numChannels;
+                        demodDataOut->data[j] = dataOut[idx];
                     }
                     
-                    demodQueue->push(demodDataOut);
-                    pushedData = true;
-                }
-                
-                if (iqDataOutQueue != NULL) {
-                    if (!iqDataOutQueue->full()) {
-                        iqDataOutQueue->push(demodDataOut);
-                        pushedData = true;
-                    } else {
-                        demodDataOut->decRefCount();
+                    for (int j = 0; j < nRunDemods; j++) {
+                        if (demodChannel[j] == i) {
+                            DemodulatorInstance *demod = runDemods[j];
+                            demod->getIQInputDataPipe()->push(demodDataOut);
+//                            std::cout << "Demodulator " << j << " in channel #" << i << " ctr: " << chanCenters[i] << " dataSize: " << chanDataSize << std::endl;
+                        }
                     }
                 }
-                
-                if (!pushedData && iqDataOutQueue == NULL) {
-                    demodDataOut->setRefCount(0);
-                }
             }
             
             busy_demod.unlock();

src/sdr/SDRPostThread.h

@@ -30,10 +30,8 @@ protected:
     std::vector<DemodulatorInstance *> demodulators;
     iirfilt_crcf dcFilter;
     std::atomic_bool swapIQ;
-    ReBuffer<DemodulatorThreadIQData> visualDataBuffers;
 
-    
-private:
-    std::vector<liquid_float_complex> _lut;
-    std::vector<liquid_float_complex> _lut_swap;
+    ReBuffer<DemodulatorThreadIQData> visualDataBuffers;
+    int numChannels, sampleRate;
+    firpfbch2_crcf channelizer;
 };

src/sdr/SoapySDRThread.cpp

@@ -28,6 +28,7 @@ SDRThread::SDRThread() : IOThread() {
 
     hasPPM.store(false);
     hasHardwareDC.store(false);
+    numChannels.store(8);
 }
 
 SDRThread::~SDRThread() {
@@ -85,7 +86,9 @@ void SDRThread::init() {
 
     device->setGainMode(SOAPY_SDR_RX,0,true);
     
-    numElems = getOptimalElementCount(sampleRate.load(), 60);
+    numChannels.store(getOptimalChannelCount(sampleRate.load()));
+    numElems.store(getOptimalElementCount(sampleRate.load(), 30));
+    
     
     buffs[0] = malloc(numElems * 2 * sizeof(float));
 }
@@ -102,15 +105,15 @@ void SDRThread::readStream(SDRThreadIQDataQueue* iqDataOutQueue) {
     long long timeNs;
 
     SDRThreadIQData *dataOut = buffers.getBuffer();
-    if (dataOut->data.size() != numElems * 2) {
-        dataOut->data.resize(numElems * 2);
+    if (dataOut->data.size() != numElems) {
+        dataOut->data.resize(numElems);
     }
 
     int n_read = 0;
-    while (n_read != numElems) {
+    while (n_read != numElems && !terminated) {
         int n_stream_read = device->readStream(stream, buffs, numElems-n_read, flags, timeNs);
         if (n_stream_read > 0) {
-            memcpy(&dataOut->data[n_read * 2], buffs[0], n_stream_read * sizeof(float) * 2);
+            memcpy(&dataOut->data[n_read], buffs[0], n_stream_read * sizeof(float) * 2);
             n_read += n_stream_read;
         } else {
             dataOut->data.resize(n_read);
@@ -120,11 +123,12 @@ void SDRThread::readStream(SDRThreadIQDataQueue* iqDataOutQueue) {
     
     //        std::cout << n_read << std::endl;
     
-    if (n_read > 0) {
+    if (n_read > 0 && !terminated) {
         dataOut->setRefCount(1);
-        dataOut->frequency = frequency;
+        dataOut->frequency = frequency.load();
         dataOut->sampleRate = sampleRate.load();
-        dataOut->dcCorrected = hasHardwareDC;
+        dataOut->dcCorrected = hasHardwareDC.load();
+        dataOut->numChannels = numChannels.load();
         
         iqDataOutQueue->push(dataOut);
     }
@@ -148,6 +152,7 @@ void SDRThread::readLoop() {
         if (rate_changed.load()) {
             device->setSampleRate(SOAPY_SDR_RX,0,sampleRate.load());
             sampleRate.store(device->getSampleRate(SOAPY_SDR_RX,0));
+            numChannels.store(getOptimalChannelCount(sampleRate.load()));
             numElems.store(getOptimalElementCount(sampleRate.load(), 60));
             free(buffs[0]);
             buffs[0] = malloc(numElems.load() * 2 * sizeof(float));
@@ -174,7 +179,7 @@ void SDRThread::readLoop() {
 void SDRThread::run() {
 //#ifdef __APPLE__
 //    pthread_t tID = pthread_self();  // ID of this thread
-//    int priority = sched_get_priority_max( SCHED_FIFO) - 1;
+//    int priority = sched_get_priority_max( SCHED_FIFO);
 //    sched_param prio = { priority }; // scheduling priority of thread
 //    pthread_setschedparam(tID, SCHED_FIFO, &prio);
 //#endif
@@ -214,11 +219,31 @@ void SDRThread::setDevice(SDRDeviceInfo *dev) {
 
 int SDRThread::getOptimalElementCount(long long sampleRate, int fps) {
     int elemCount = (int)floor((double)sampleRate/(double)fps);
-    elemCount = int(ceil((double)elemCount/512.0)*512.0);
-    std::cout << "Calculated optimal element count of " << elemCount << std::endl;
+    int nch = numChannels.load();
+    elemCount = int(ceil((double)elemCount/(double)nch))*nch;
+    std::cout << "Calculated optimal " << numChannels.load() << " channel element count of " << elemCount << std::endl;
     return elemCount;
 }
 
+int SDRThread::getOptimalChannelCount(long long sampleRate) {
+    int optimal_rate = CHANNELIZER_RATE_MAX;
+    int optimal_count = int(ceil(double(sampleRate)/double(optimal_rate)));
+    
+    if (optimal_count % 2 == 1) {
+        optimal_count--;
+    }
+    
+    if (optimal_count < 4) {
+        optimal_count = 4;
+    }
+    
+    if (optimal_count > 16) {
+        optimal_count = 16;
+    }
+    return optimal_count;
+}
+
+
 void SDRThread::setFrequency(long long freq) {
     if (freq < sampleRate.load() / 2) {
         freq = sampleRate.load() / 2;

src/sdr/SoapySDRThread.h

@@ -18,10 +18,11 @@ public:
     long long frequency;
     long long sampleRate;
     bool dcCorrected;
-    std::vector<float> data;
+    int numChannels;
+    std::vector<liquid_float_complex> data;
 
     SDRThreadIQData() :
-            frequency(0), sampleRate(DEFAULT_SAMPLE_RATE), dcCorrected(true) {
+            frequency(0), sampleRate(DEFAULT_SAMPLE_RATE), dcCorrected(true), numChannels(0) {
 
     }
 
@@ -54,6 +55,7 @@ public:
     SDRDeviceInfo *getDevice();
     void setDevice(SDRDeviceInfo *dev);
     int getOptimalElementCount(long long sampleRate, int fps);
+    int getOptimalChannelCount(long long sampleRate);
     
     void setFrequency(long long freq);
     long long getFrequency();
@@ -81,7 +83,7 @@ protected:
 
     std::atomic<uint32_t> sampleRate;
     std::atomic_llong frequency, offset;
-    std::atomic_int ppm, direct_sampling_mode, numElems;
+    std::atomic_int ppm, direct_sampling_mode, numElems, numChannels;
     std::atomic_bool hasPPM, hasHardwareDC;
 
     std::atomic_bool rate_changed, freq_changed, offset_changed,

src/visual/TuningCanvas.cpp

@@ -190,8 +190,8 @@ void TuningCanvas::StepTuner(ActiveState state, int exponent, bool up) {
             bw += amount;
         }
 
-        if (bw > wxGetApp().getSampleRate()) {
-            bw = wxGetApp().getSampleRate();
+        if (bw > CHANNELIZER_RATE_MAX) {
+            bw = CHANNELIZER_RATE_MAX;
         }
 
         wxGetApp().getDemodMgr().setLastBandwidth(bw);

src/visual/WaterfallCanvas.cpp

@@ -435,8 +435,8 @@ void WaterfallCanvas::OnMouseMoved(wxMouseEvent& event) {
             int currentBW = demod->getBandwidth();
 
             currentBW = currentBW + bwDiff;
-            if (currentBW > wxGetApp().getSampleRate()) {
-                currentBW = wxGetApp().getSampleRate();
+            if (currentBW > CHANNELIZER_RATE_MAX) {
+                currentBW = CHANNELIZER_RATE_MAX;
             }
             if (currentBW < MIN_BANDWIDTH) {
                 currentBW = MIN_BANDWIDTH;