Add WIP CFR implementation to OfdmGenerator

1 files changed, 127 insertions, 7 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();
+}