Add WIP CFR implementation to OfdmGenerator

2 files changed, 178 insertions, 35 deletions

diff --git a/src/OfdmGenerator.cpp b/src/OfdmGenerator.cpp
index 20ba2b7..3d3dfd7 100644
--- a/src/OfdmGenerator.cpp
+++ b/src/OfdmGenerator.cpp
@@ -2,7 +2,7 @@
    Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011 Her Majesty
    the Queen in Right of Canada (Communications Research Center Canada)
 
-   Copyright (C) 2016
+   Copyright (C) 2017
    Matthias P. Braendli, matthias.braendli@mpb.li
 
     http://opendigitalradio.org
@@ -34,19 +34,23 @@
 #include <stdexcept>
 #include <assert.h>
 #include <complex>
-typedef std::complex<float> complexf;
+#include <string>
 
 
 OfdmGenerator::OfdmGenerator(size_t nbSymbols,
         size_t nbCarriers,
         size_t spacing,
         bool inverse) :
-    ModCodec(),
-    myFftPlan(NULL),
-    myFftIn(NULL), myFftOut(NULL),
+    ModCodec(), RemoteControllable("ofdm"),
+    myFftPlan(nullptr),
+    myFftIn(nullptr), myFftOut(nullptr),
     myNbSymbols(nbSymbols),
     myNbCarriers(nbCarriers),
-    mySpacing(spacing)
+    mySpacing(spacing),
+    myCfr(false),
+    myCfrClip(1.0f),
+    myCfrErrorClip(1.0f),
+    myCfrFft(nullptr)
 {
     PDEBUG("OfdmGenerator::OfdmGenerator(%zu, %zu, %zu, %s) @ %p\n",
             nbSymbols, nbCarriers, spacing, inverse ? "true" : "false", this);
@@ -56,6 +60,11 @@ OfdmGenerator::OfdmGenerator(size_t nbSymbols,
                 "OfdmGenerator::OfdmGenerator nbCarriers > spacing!");
     }
 
+    /* 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");
+
     if (inverse) {
         myPosDst = (nbCarriers & 1 ? 0 : 1);
         myPosSrc = 0;
@@ -91,6 +100,13 @@ OfdmGenerator::OfdmGenerator(size_t nbSymbols,
             myFftIn, myFftOut,
             FFTW_BACKWARD, FFTW_MEASURE);
 
+    myCfrPostClip.resize(N);
+    myCfrPostFft.resize(N);
+    myCfrFft = fftwf_plan_dft_1d(N,
+            reinterpret_cast<FFT_TYPE*>(myCfrPostClip.data()),
+            reinterpret_cast<FFT_TYPE*>(myCfrPostFft.data()),
+            FFTW_FORWARD, FFTW_MEASURE);
+
     if (sizeof(complexf) != sizeof(FFT_TYPE)) {
         printf("sizeof(complexf) %zu\n", sizeof(complexf));
         printf("sizeof(FFT_TYPE) %zu\n", sizeof(FFT_TYPE));
@@ -115,6 +131,10 @@ OfdmGenerator::~OfdmGenerator()
     if (myFftPlan) {
         fftwf_destroy_plan(myFftPlan);
     }
+
+    if (myCfrFft) {
+        fftwf_destroy_plan(myCfrFft);
+    }
 }
 
 int OfdmGenerator::process(Buffer* const dataIn, Buffer* dataOut)
@@ -157,7 +177,11 @@ int OfdmGenerator::process(Buffer* const dataIn, Buffer* dataOut)
         memcpy(&myFftIn[myNegDst], &in[myNegSrc],
                 myNegSize * sizeof(FFT_TYPE));
 
-        fftwf_execute(myFftPlan);
+        fftwf_execute(myFftPlan); // IFFT from myFftIn to myFftOut
+
+        if (myCfr) {
+            cfr_one_iteration(dataOut, i);
+        }
 
         memcpy(out, myFftOut, mySpacing * sizeof(FFT_TYPE));
 
@@ -167,3 +191,99 @@ int OfdmGenerator::process(Buffer* const dataIn, Buffer* dataOut)
     return sizeOut;
 }
 
+void OfdmGenerator::cfr_one_iteration(Buffer *symbols, size_t symbol_ix)
+{
+    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]);
+        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);
+        }
+    }
+
+    // Take FFT of our clipped signal
+    std::copy(out, out + mySpacing, myCfrPostClip.begin());
+    fftwf_execute(myCfrFft); // FFT from myCfrPostClip to myCfrPostFft
+
+    // Calculate the error in frequency domain by subtracting our reference
+    // and clip it to myCfrErrorClip. By adding this clipped error signal
+    // to our FFT output, we compensate the introduced error to some
+    // 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 float mag_squared = std::norm(error);
+        if (mag_squared > err_clip_squared) {
+            error *= std::sqrt(err_clip_squared / mag_squared);
+        }
+
+        // Update the reference in-place to avoid another copy for the
+        // subsequence IFFT
+        reference[i] += error;
+    }
+
+    // Run our error-compensated symbol through the IFFT again
+    fftwf_execute(myFftPlan); // IFFT from myFftIn to myFftOut
+
+}
+
+
+void OfdmGenerator::set_parameter(const std::string& parameter,
+                                  const std::string& value)
+{
+    using namespace std;
+    stringstream ss(value);
+    ss.exceptions ( stringstream::failbit | stringstream::badbit );
+
+    if (parameter == "cfr") {
+        ss >> myCfr;
+    }
+    else if (parameter == "clip") {
+        ss >> myCfrClip;
+    }
+    else if (parameter == "errorclip") {
+        ss >> myCfrErrorClip;
+    }
+    else {
+        stringstream ss;
+        ss << "Parameter '" << parameter
+            << "' is not exported by controllable " << get_rc_name();
+        throw ParameterError(ss.str());
+    }
+}
+
+const std::string OfdmGenerator::get_parameter(const std::string& parameter) const
+{
+    using namespace std;
+    stringstream ss;
+    if (parameter == "cfr") {
+        ss << myCfr;
+    }
+    else if (parameter == "clip") {
+        ss << std::fixed << myCfrClip;
+    }
+    else if (parameter == "errorclip") {
+        ss << std::fixed << myCfrErrorClip;
+    }
+    else {
+        ss << "Parameter '" << parameter <<
+            "' is not exported by controllable " << get_rc_name();
+        throw ParameterError(ss.str());
+    }
+    return ss.str();
+}
diff --git a/src/OfdmGenerator.h b/src/OfdmGenerator.h
index a8c3c19..9a6fa01 100644
--- a/src/OfdmGenerator.h
+++ b/src/OfdmGenerator.h
@@ -2,7 +2,7 @@
    Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011 Her Majesty
    the Queen in Right of Canada (Communications Research Center Canada)
 
-   Copyright (C) 2016
+   Copyright (C) 2017
    Matthias P. Braendli, matthias.braendli@mpb.li
 
     http://opendigitalradio.org
@@ -32,38 +32,61 @@
 
 #include "porting.h"
 #include "ModPlugin.h"
-
+#include "RemoteControl.h"
 #include "fftw3.h"
-
 #include <sys/types.h>
+#include <vector>
+#include <complex>
 
+typedef std::complex<float> complexf;
 
-class OfdmGenerator : public ModCodec
+class OfdmGenerator : public ModCodec, public RemoteControllable
 {
-public:
-    OfdmGenerator(size_t nbSymbols, size_t nbCarriers, size_t spacing, bool inverse = true);
-    virtual ~OfdmGenerator();
-    OfdmGenerator(const OfdmGenerator&);
-    OfdmGenerator& operator=(const OfdmGenerator&);
-
-
-    int process(Buffer* const dataIn, Buffer* dataOut);
-    const char* name() { return "OfdmGenerator"; }
-
-protected:
-    fftwf_plan myFftPlan;
-    fftwf_complex *myFftIn, *myFftOut;
-    size_t myNbSymbols;
-    size_t myNbCarriers;
-    size_t mySpacing;
-    unsigned myPosSrc;
-    unsigned myPosDst;
-    unsigned myPosSize;
-    unsigned myNegSrc;
-    unsigned myNegDst;
-    unsigned myNegSize;
-    unsigned myZeroDst;
-    unsigned myZeroSize;
+    public:
+        OfdmGenerator(size_t nbSymbols,
+                      size_t nbCarriers,
+                      size_t spacing,
+                      bool inverse = true);
+        virtual ~OfdmGenerator();
+        OfdmGenerator(const OfdmGenerator&) = delete;
+        OfdmGenerator& operator=(const OfdmGenerator&) = delete;
+
+        int process(Buffer* const dataIn, Buffer* dataOut) override;
+        const char* name() override { return "OfdmGenerator"; }
+
+        /* Functions for the remote control */
+        /* Base function to set parameters. */
+        virtual void set_parameter(
+                const std::string& parameter,
+                const std::string& value) override;
+
+        /* Getting a parameter always returns a string. */
+        virtual const std::string get_parameter(
+                const std::string& parameter) const override;
+
+    protected:
+        void cfr_one_iteration(Buffer *symbols, size_t symbol_ix);
+
+        fftwf_plan myFftPlan;
+        fftwf_complex *myFftIn, *myFftOut;
+        size_t myNbSymbols;
+        size_t myNbCarriers;
+        size_t mySpacing;
+        unsigned myPosSrc;
+        unsigned myPosDst;
+        unsigned myPosSize;
+        unsigned myNegSrc;
+        unsigned myNegDst;
+        unsigned myNegSize;
+        unsigned myZeroDst;
+        unsigned myZeroSize;
+
+        bool myCfr; // Whether to enable crest factor reduction
+        float myCfrClip;
+        float myCfrErrorClip;
+        fftwf_plan myCfrFft;
+        std::vector<complexf> myCfrPostClip;
+        std::vector<complexf> myCfrPostFft;
 };