Fix some CFR things, still WIP

5 files changed, 76 insertions, 31 deletions

diff --git a/src/ConfigParser.cpp b/src/ConfigParser.cpp
index 9ac1280..1cc94c0 100644
--- a/src/ConfigParser.cpp
+++ b/src/ConfigParser.cpp
@@ -177,6 +177,13 @@ static void parse_configfile(
             pt.get<int>("poly.num_threads", 0);
     }
 
+    // Crest factor reduction
+    if (pt.get("cfr.enabled", 0) == 1) {
+        mod_settings.enableCfr = true;
+        mod_settings.cfrClip = pt.get<float>("cfr.clip");
+        mod_settings.cfrErrorClip = pt.get<float>("cfr.error_clip");
+    }
+
     // Output options
     std::string output_selected;
     try {
diff --git a/src/ConfigParser.h b/src/ConfigParser.h
index 89f0fb7..a8d7837 100644
--- a/src/ConfigParser.h
+++ b/src/ConfigParser.h
@@ -77,6 +77,12 @@ struct mod_settings_t {
     std::string polyCoefFilename = "";
     unsigned polyNumThreads = 0;
 
+    // Settings for crest factor reduction
+    bool enableCfr = false;
+    float cfrClip = 1.0f;
+    float cfrErrorClip = 1.0f;
+
+
 #if defined(HAVE_OUTPUT_UHD)
     OutputUHDConfig outputuhd_conf;
 #endif
diff --git a/src/DabModulator.cpp b/src/DabModulator.cpp
index cc2642a..0914469 100644
--- a/src/DabModulator.cpp
+++ b/src/DabModulator.cpp
@@ -183,7 +183,14 @@ int DabModulator::process(Buffer* dataOut)
         }
 
         auto cifOfdm = make_shared<OfdmGenerator>(
-                (1 + myNbSymbols), myNbCarriers, mySpacing);
+                (1 + myNbSymbols),
+                myNbCarriers,
+                mySpacing,
+                m_settings.enableCfr,
+                m_settings.cfrClip,
+                m_settings.cfrErrorClip);
+
+        rcs.enrol(cifOfdm.get());
 
         auto cifGain = make_shared<GainControl>(
                 mySpacing,
diff --git a/src/OfdmGenerator.cpp b/src/OfdmGenerator.cpp
index 3d3dfd7..41c1c85 100644
--- a/src/OfdmGenerator.cpp
+++ b/src/OfdmGenerator.cpp
@@ -26,6 +26,8 @@
 
 #include "OfdmGenerator.h"
 #include "PcDebug.h"
+
+#include <complex>
 #include "fftw3.h"
 #define FFT_TYPE fftwf_complex
 
@@ -33,23 +35,25 @@
 #include <string.h>
 #include <stdexcept>
 #include <assert.h>
-#include <complex>
 #include <string>
 
 
 OfdmGenerator::OfdmGenerator(size_t nbSymbols,
-        size_t nbCarriers,
-        size_t spacing,
-        bool inverse) :
+                             size_t nbCarriers,
+                             size_t spacing,
+                             bool enableCfr,
+                             float cfrClip,
+                             float cfrErrorClip,
+                             bool inverse) :
     ModCodec(), RemoteControllable("ofdm"),
     myFftPlan(nullptr),
     myFftIn(nullptr), myFftOut(nullptr),
     myNbSymbols(nbSymbols),
     myNbCarriers(nbCarriers),
     mySpacing(spacing),
-    myCfr(false),
-    myCfrClip(1.0f),
-    myCfrErrorClip(1.0f),
+    myCfr(enableCfr),
+    myCfrClip(cfrClip),
+    myCfrErrorClip(cfrErrorClip),
     myCfrFft(nullptr)
 {
     PDEBUG("OfdmGenerator::OfdmGenerator(%zu, %zu, %zu, %s) @ %p\n",
@@ -62,8 +66,8 @@ OfdmGenerator::OfdmGenerator(size_t nbSymbols,
 
     /* register the parameters that can be remote controlled */
     RC_ADD_PARAMETER(cfr, "Enable crest factor reduction");
-    RC_ADD_PARAMETER(cfrclip, "CFR: Clip to amplitude");
-    RC_ADD_PARAMETER(cfrerrorclip, "CFR: Limit error");
+    RC_ADD_PARAMETER(clip, "CFR: Clip to amplitude");
+    RC_ADD_PARAMETER(errorclip, "CFR: Limit error");
 
     if (inverse) {
         myPosDst = (nbCarriers & 1 ? 0 : 1);
@@ -100,11 +104,10 @@ OfdmGenerator::OfdmGenerator(size_t nbSymbols,
             myFftIn, myFftOut,
             FFTW_BACKWARD, FFTW_MEASURE);
 
-    myCfrPostClip.resize(N);
-    myCfrPostFft.resize(N);
+    myCfrPostClip = (FFT_TYPE*)fftwf_malloc(sizeof(FFT_TYPE) * N);
+    myCfrPostFft = (FFT_TYPE*)fftwf_malloc(sizeof(FFT_TYPE) * N);
     myCfrFft = fftwf_plan_dft_1d(N,
-            reinterpret_cast<FFT_TYPE*>(myCfrPostClip.data()),
-            reinterpret_cast<FFT_TYPE*>(myCfrPostFft.data()),
+            myCfrPostClip, myCfrPostFft,
             FFTW_FORWARD, FFTW_MEASURE);
 
     if (sizeof(complexf) != sizeof(FFT_TYPE)) {
@@ -167,6 +170,9 @@ int OfdmGenerator::process(Buffer* const dataIn, Buffer* dataOut)
                 "OfdmGenerator::process output size not valid!");
     }
 
+    myNumClip = 0;
+    myNumErrorClip = 0;
+
     for (size_t i = 0; i < myNbSymbols; ++i) {
         myFftIn[0][0] = 0;
         myFftIn[0][1] = 0;
@@ -177,10 +183,17 @@ int OfdmGenerator::process(Buffer* const dataIn, Buffer* dataOut)
         memcpy(&myFftIn[myNegDst], &in[myNegSrc],
                 myNegSize * sizeof(FFT_TYPE));
 
+        std::vector<complexf> reference;
+        if (myCfr) {
+            reference.resize(mySpacing);
+            memcpy(reference.data(), myFftIn, mySpacing * sizeof(FFT_TYPE));
+        }
+
         fftwf_execute(myFftPlan); // IFFT from myFftIn to myFftOut
 
         if (myCfr) {
-            cfr_one_iteration(dataOut, i);
+            complexf *symbol = reinterpret_cast<complexf*>(myFftOut);
+            cfr_one_iteration(symbol, reference.data());
         }
 
         memcpy(out, myFftOut, mySpacing * sizeof(FFT_TYPE));
@@ -188,33 +201,36 @@ int OfdmGenerator::process(Buffer* const dataIn, Buffer* dataOut)
         in += myNbCarriers;
         out += mySpacing;
     }
+
+    if (myCfr) {
+        etiLog.level(debug) << "CFR: " << myNumClip << " clipped, " <<
+            myNumErrorClip << " err clipped";
+    }
+
     return sizeOut;
 }
 
-void OfdmGenerator::cfr_one_iteration(Buffer *symbols, size_t symbol_ix)
+void OfdmGenerator::cfr_one_iteration(complexf *symbol, const complexf *reference)
 {
-    complexf* out = reinterpret_cast<complexf*>(symbols->getData());
-
-    out += (symbol_ix * mySpacing);
-
     // use std::norm instead of std::abs to avoid calculating the
     // square roots
     const float clip_squared = myCfrClip * myCfrClip;
 
     // Clip
     for (size_t i = 0; i < mySpacing; i++) {
-        const float mag_squared = std::norm(out[i]);
+        const float mag_squared = std::norm(symbol[i]);
         if (mag_squared > clip_squared) {
             // normalise absolute value to myCfrClip:
             // x_clipped = x * clip / |x|
             //           = x * sqrt(clip_squared) / sqrt(mag_squared)
             //           = x * sqrt(clip_squared / mag_squared)
-            out[i] *= std::sqrt(clip_squared / mag_squared);
+            symbol[i] *= std::sqrt(clip_squared / mag_squared);
+            myNumClip++;
         }
     }
 
     // Take FFT of our clipped signal
-    std::copy(out, out + mySpacing, myCfrPostClip.begin());
+    memcpy(myCfrPostClip, symbol, mySpacing * sizeof(FFT_TYPE));
     fftwf_execute(myCfrFft); // FFT from myCfrPostClip to myCfrPostFft
 
     // Calculate the error in frequency domain by subtracting our reference
@@ -223,23 +239,26 @@ void OfdmGenerator::cfr_one_iteration(Buffer *symbols, size_t symbol_ix)
     // extent.
     const float err_clip_squared = myCfrErrorClip * myCfrErrorClip;
 
-    complexf *reference = reinterpret_cast<complexf*>(myFftIn);
     for (size_t i = 0; i < mySpacing; i++) {
-        complexf error = myCfrPostFft[i] - reference[i];
+        const complexf constellation_point =
+            reinterpret_cast<complexf*>(myCfrPostFft)[i];
+
+        complexf error = reference[i] - constellation_point;
 
         const float mag_squared = std::norm(error);
         if (mag_squared > err_clip_squared) {
             error *= std::sqrt(err_clip_squared / mag_squared);
+            myNumErrorClip++;
         }
 
-        // Update the reference in-place to avoid another copy for the
+        // Update the input to the FFT directl to avoid another copy for the
         // subsequence IFFT
-        reference[i] += error;
+        complexf *fft_in = reinterpret_cast<complexf*>(myFftIn);
+        fft_in[i] = constellation_point + error;
     }
 
     // Run our error-compensated symbol through the IFFT again
     fftwf_execute(myFftPlan); // IFFT from myFftIn to myFftOut
-
 }
 
 
diff --git a/src/OfdmGenerator.h b/src/OfdmGenerator.h
index 9a6fa01..a41d583 100644
--- a/src/OfdmGenerator.h
+++ b/src/OfdmGenerator.h
@@ -46,6 +46,9 @@ class OfdmGenerator : public ModCodec, public RemoteControllable
         OfdmGenerator(size_t nbSymbols,
                       size_t nbCarriers,
                       size_t spacing,
+                      bool enableCfr,
+                      float cfrClip,
+                      float cfrErrorClip,
                       bool inverse = true);
         virtual ~OfdmGenerator();
         OfdmGenerator(const OfdmGenerator&) = delete;
@@ -65,7 +68,7 @@ class OfdmGenerator : public ModCodec, public RemoteControllable
                 const std::string& parameter) const override;
 
     protected:
-        void cfr_one_iteration(Buffer *symbols, size_t symbol_ix);
+        void cfr_one_iteration(complexf *symbol, const complexf *reference);
 
         fftwf_plan myFftPlan;
         fftwf_complex *myFftIn, *myFftOut;
@@ -85,8 +88,11 @@ class OfdmGenerator : public ModCodec, public RemoteControllable
         float myCfrClip;
         float myCfrErrorClip;
         fftwf_plan myCfrFft;
-        std::vector<complexf> myCfrPostClip;
-        std::vector<complexf> myCfrPostFft;
+        fftwf_complex *myCfrPostClip;
+        fftwf_complex *myCfrPostFft;
+
+        size_t myNumClip;
+        size_t myNumErrorClip;
 };