From 28e262604d2bfd5ea48b883fca438acabd3b4a4d Mon Sep 17 00:00:00 2001 From: "Ralph J. Steinhagen" Date: Thu, 26 Dec 2024 18:24:53 +0100 Subject: [PATCH] added Rotator block: fixed frequency shifter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Introduce Rotator block that increments the phase before rotating each sample. - handles angle wrapping to keep accumulated_phase in [0, 2π). Signed-off-by: Ralph J. Steinhagen --- .../include/gnuradio-4.0/math/Rotator.hpp | 66 +++++++++ blocks/math/test/CMakeLists.txt | 1 + blocks/math/test/qa_Rotator.cpp | 126 ++++++++++++++++++ 3 files changed, 193 insertions(+) create mode 100644 blocks/math/include/gnuradio-4.0/math/Rotator.hpp create mode 100644 blocks/math/test/qa_Rotator.cpp diff --git a/blocks/math/include/gnuradio-4.0/math/Rotator.hpp b/blocks/math/include/gnuradio-4.0/math/Rotator.hpp new file mode 100644 index 00000000..c8478486 --- /dev/null +++ b/blocks/math/include/gnuradio-4.0/math/Rotator.hpp @@ -0,0 +1,66 @@ +#ifndef GNURADIO_ROTATOR_HPP +#define GNURADIO_ROTATOR_HPP + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gr::blocks::math { + +template +struct Rotator : gr::Block> { + using value_type = typename T::value_type; + using Description = Doc; + + PortIn in; + PortOut out; + + Annotated, Unit<"Hz">> sample_rate = 1.f; + Annotated, Unit<"Hz">> frequency_shift = 0.0f; + Annotated, Doc<"how many radians to add per sample">> phase_increment{0}; + Annotated, Doc<"starting offset for each new chunk">> initial_phase{0}; + + value_type _accumulated_phase{0}; + + GR_MAKE_REFLECTABLE(Rotator, in, out, sample_rate, frequency_shift, initial_phase, phase_increment); + + void settingsChanged(const property_map& /*oldSettings*/, const property_map& newSettings) { + if (newSettings.contains("frequency_shift") && !newSettings.contains("phase_increment")) { + phase_increment = value_type(2) * static_cast(std::numbers::pi_v * frequency_shift / sample_rate); + } else if (!newSettings.contains("frequency_shift") && newSettings.contains("phase_increment")) { + frequency_shift = static_cast(phase_increment / (value_type(2) * std::numbers::pi_v)) * sample_rate; + } else if (newSettings.contains("frequency_shift") && newSettings.contains("phase_increment")) { + throw gr::exception(fmt::format("cannot set both 'frequency_shift' and 'phase_increment' in new setting (XOR): {}'", newSettings)); + } + _accumulated_phase = initial_phase; + } + + [[nodiscard]] constexpr T processOne(const T& inSample) noexcept { + _accumulated_phase += phase_increment; + // optional: wrap angle if too large + if (_accumulated_phase > value_type(2) * std::numbers::pi_v) { + _accumulated_phase -= value_type(2) * std::numbers::pi_v; + } else if (_accumulated_phase < value_type(0)) { + _accumulated_phase += value_type(2) * std::numbers::pi_v; + } + + return inSample * std::complex(std::cos(_accumulated_phase), std::sin(_accumulated_phase)); + } +}; + +} // namespace gr::blocks::math + +inline static auto registerRotator = gr::registerBlock, std::complex>(gr::globalBlockRegistry()); + +#endif // GNURADIO_ROTATOR_HPP diff --git a/blocks/math/test/CMakeLists.txt b/blocks/math/test/CMakeLists.txt index fdb63354..323d90a8 100644 --- a/blocks/math/test/CMakeLists.txt +++ b/blocks/math/test/CMakeLists.txt @@ -1,2 +1,3 @@ add_ut_test(qa_Math) add_ut_test(qa_ExpressionBlocks) +add_ut_test(qa_Rotator) diff --git a/blocks/math/test/qa_Rotator.cpp b/blocks/math/test/qa_Rotator.cpp new file mode 100644 index 00000000..b3a24452 --- /dev/null +++ b/blocks/math/test/qa_Rotator.cpp @@ -0,0 +1,126 @@ +#include +#include +#include +#include + +#include + +#include + +namespace { + +template +std::vector> execRotator(const std::vector>& input, const gr::property_map& initSettings) { + gr::blocks::math::Rotator> rot(initSettings); + rot.settings().init(); + std::ignore = rot.settings().applyStagedParameters(); // needed for unit-test only when executed outside a Scheduler/Graph + + std::vector> output(input.size()); + for (std::size_t i = 0; i < input.size(); i++) { + output[i] = rot.processOne(input[i]); + } + return output; +} + +template +void plotTimeDomain(const std::vector>& dataIn, const std::vector>& dataOut, float fs, const std::string& label) { + std::vector time(dataOut.size()); + for (std::size_t i = 0UZ; i < dataOut.size(); i++) { + time[i] = static_cast(i) / fs; + } + + std::vector inRe(dataOut.size()); + std::vector inIm(dataOut.size()); + std::vector outRe(dataOut.size()); + std::vector outIm(dataOut.size()); + for (std::size_t i = 0UZ; i < dataOut.size(); i++) { + inRe[i] = static_cast(dataIn[i].real()); + inIm[i] = static_cast(dataIn[i].imag()); + outRe[i] = static_cast(dataOut[i].real()); + outIm[i] = static_cast(dataOut[i].imag()); + } + + // quick chart + gr::graphs::ImChart<100, 15> chart({{0.0f, time.back()}, {-1.5f, +1.5f}}); + chart.axis_name_x = "Time [s]"; + chart.axis_name_y = "Amplitude [a.u.]"; + + chart.draw(time, inRe, "Re(in)"); + chart.draw(time, inIm, "Im(in)"); + chart.draw(time, outRe, fmt::format("out: Re({})", label)); + chart.draw(time, outIm, fmt::format("out: Im({})", label)); + chart.draw(); +} + +} // end anonymous namespace + +const boost::ut::suite<"basic math tests"> basicMath = [] { + using namespace boost::ut; + using namespace gr::blocks::math; + + constexpr auto kArithmeticTypes = std::tuple, std::complex>{}; + + if (std::getenv("DISABLE_SENSITIVE_TESTS") == nullptr) { + // conditionally enable visual tests outside the CI + boost::ext::ut::cfg = {.tag = {"visual", "benchmarks"}}; + } + + "Rotator - basic test"_test = [] { + using value_t = typename T::value_type; + value_t phase_shift = std::numbers::pi_v / value_t(2); + Rotator rot({{"phase_increment", phase_shift}, {"initial_phase", value_t(0)}, {"sample_rate", 1.f}}); + rot.settings().init(); + std::ignore = rot.settings().applyStagedParameters(); // needed for unit-test only when executed outside a Scheduler/Graph + + expect(approx(rot.frequency_shift, 0.25f, 1e-3f)); + expect(approx(rot.initial_phase, value_t(0), value_t(1e-3f))); + + std::vector output(8UZ); + for (std::size_t i = 0; i < 8; i++) { + output[i] = rot.processOne(std::complex(1, 0)); + } + + for (std::size_t i = 0; i < 8; i++) { + value_t wantAngle = value_t(i + 1) * phase_shift; + value_t wantCos = std::cos(wantAngle); + value_t wantSin = std::sin(wantAngle); + + expect(approx(output[i].real(), wantCos, value_t(1e-5))) << "rotator real mismatch i=" << i; + expect(approx(output[i].imag(), wantSin, value_t(1e-5))) << "rotator imag mismatch i=" << i; + } + } | kArithmeticTypes; + + constexpr static float fs = 100.0; // sampling rate + constexpr static float tMax = 2.0; // seconds + constexpr static auto nSamp = static_cast(fs * tMax); + + tag("visual") / "RotatorTest - DC->2 Hz shift"_test = [] { + std::vector> input(nSamp, std::complex(std::sqrt(2.0) / 2.0, std::sqrt(2.0) / 2.0)); + auto output = execRotator(input, {{"frequency_shift", +2.f}, {"sample_rate", fs}}); + plotTimeDomain(input, output, fs, "DC->+2 Hz"); + }; + + tag("visual") / "RotatorTest - 0.5 Hz => shift +1.5 => 2 Hz"_test = [] { + std::vector> input(nSamp); + for (std::size_t i = 0; i < nSamp; i++) { // 0.5 Hz complex sinusoid + double t = static_cast(i) / static_cast(fs); + double angle = 2.0 * std::numbers::pi * 0.5 * t; // 0.5 Hz + input[i] = {std::cos(angle), std::sin(angle)}; + } + auto output = execRotator(input, {{"frequency_shift", +1.5f}, {"sample_rate", fs}}); + plotTimeDomain(input, output, fs, ".5->2 Hz"); + }; + + tag("visual") / "RotatorTest - 2 Hz => shift -1.5 => 0.5 Hz"_test = [] { + std::vector> input(nSamp); + for (std::size_t i = 0; i < nSamp; i++) { // 2 Hz complex sinusoid + double t = static_cast(i) / static_cast(fs); + double angle = 2.0 * std::numbers::pi * 2.0 * t; // 2 Hz + input[i] = {std::cos(angle), std::sin(angle)}; + } + auto output = execRotator(input, {{"frequency_shift", -1.5f}, {"sample_rate", fs}}); + plotTimeDomain(input, output, fs, "2->.5 Hz"); + }; +}; + +int main() { /* not needed for UT */ }