From f319c574c346b5b85a5343e837eadbb2624821fd Mon Sep 17 00:00:00 2001 From: rstein Date: Wed, 27 Nov 2024 09:24:56 +0100 Subject: [PATCH] added Trigger Block implements a SchmittTrigger block with edge detection and optional interpolation Signed-off-by: rstein Signed-off-by: Ralph J. Steinhagen --- .../include/gnuradio-4.0/basic/Trigger.hpp | 161 ++++++++++++++++++ blocks/basic/test/CMakeLists.txt | 16 +- blocks/basic/test/qa_TriggerBlocks.cpp | 137 +++++++++++++++ 3 files changed, 307 insertions(+), 7 deletions(-) create mode 100644 blocks/basic/include/gnuradio-4.0/basic/Trigger.hpp create mode 100644 blocks/basic/test/qa_TriggerBlocks.cpp diff --git a/blocks/basic/include/gnuradio-4.0/basic/Trigger.hpp b/blocks/basic/include/gnuradio-4.0/basic/Trigger.hpp new file mode 100644 index 00000000..f4022c10 --- /dev/null +++ b/blocks/basic/include/gnuradio-4.0/basic/Trigger.hpp @@ -0,0 +1,161 @@ +#ifndef TRIGGER_HPP +#define TRIGGER_HPP + +#include +#include +#include +#include + +namespace gr::blocks::basic { + +template +requires(std::is_arithmetic_v or (UncertainValueLike && std::is_arithmetic_v>)) +struct SchmittTrigger : public gr::Block, NoDefaultTagForwarding> { + using Description = Doc and SVD implementation) + +The block generates the following optional trigger that are controlled by: + * trigger_name_rising_edge -> trigger name that is used when a rising edge is detected + * trigger_name_falling_edge -> trigger name that is used when a falling edge is detected + +The trigger time and offset is calculated through sample counting from the last synchronising trigger. +The information is stored (info only) in `trigger_name`, `trigger_time`, `trigger_offset`. +)"">; + using enum gr::trigger::EdgeDetection; + using ClockSourceType = std::chrono::system_clock; + using value_t = meta::fundamental_base_value_type_t; + + template + using A = gr::Annotated; + + constexpr static std::size_t N_HISTORY = 16UZ; + + PortIn in; + PortOut out; + + A, Visible> offset{value_t(0)}; + A, Visible> threshold{value_t(1)}; + A, Visible> trigger_name_rising_edge{magic_enum::enum_name(RISING)}; + A, Visible> trigger_name_falling_edge{magic_enum::enum_name(FALLING)}; + A sample_rate = 1.f; + + A> forward_tag{true}; + A> trigger_name = ""; + A, Unit<"ns">> trigger_time{0U}; + A, Unit<"s">> trigger_offset{0.0f}; + std::string context = ""; + + GR_MAKE_REFLECTABLE(SchmittTrigger, in, out, offset, threshold, trigger_name_rising_edge, trigger_name_falling_edge, sample_rate, forward_tag, trigger_name, trigger_time, trigger_offset, context); + + gr::trigger::SchmittTrigger _trigger{0, 1}; + std::uint64_t _period{1U}; + std::uint64_t _now{0U}; + + void settingsChanged(const gr::property_map& /*oldSettings*/, const gr::property_map& newSettings) { + if (newSettings.contains("sample_rate")) { + _period = static_cast(1e6f / sample_rate); + } + if (newSettings.contains("trigger_time")) { + _now = trigger_time + static_cast(1e6f * trigger_offset); + } + + if (newSettings.contains("offset") || newSettings.contains("threshold")) { + _trigger.setOffset(offset); + _trigger.setThreshold(threshold); + _trigger.reset(); + } + } + + void start() { reset(); } + + void reset() { + _trigger.reset(); + _now = static_cast(std::chrono::duration_cast(ClockSourceType::now().time_since_epoch()).count()); + trigger_time = _now; + } + + gr::work::Status processBulk(InputSpanLike auto& inputSpan, OutputSpanLike auto& outputSpan) { + const std::optional nextEoSTag = samples_to_eos_tag(in); + if (inputSpan.size() < N_HISTORY && nextEoSTag.value_or(0) >= N_HISTORY) { + return gr::work::Status::INSUFFICIENT_INPUT_ITEMS; + } + const std::size_t nProcess = inputSpan.size() - N_HISTORY * nextEoSTag.has_value(); // limit input data to have enough samples to back-date detected trigger if necessary + + auto forwardTags = [&](std::size_t maxRelIndex) { + if (!forward_tag) { + return; + } + for (const auto& tag : inputSpan.rawTags) { + const auto relIndex = static_cast(tag.index) - static_cast(inputSpan.streamIndex); + if (relIndex >= 0 && static_cast(relIndex) <= maxRelIndex) { + outputSpan.publishTag(tag.map, static_cast(relIndex)); + } + } + }; + + auto publishEdge = [&](const std::string& triggerName, std::ptrdiff_t edgePosition) { + const std::size_t edgePosUnsigned = std::max(0UZ, static_cast(edgePosition)); + + forwardTags(edgePosUnsigned); + + // Calculate properties for the edge tag + const auto edgeIdxOffset = _trigger.lastEdgeIdx + _trigger.lastEdgeOffset; + outputSpan.publishTag( + property_map{ + // + {gr::tag::TRIGGER_NAME.shortKey(), triggerName}, // + {gr::tag::TRIGGER_TIME.shortKey(), static_cast(_now - gr::value(edgeIdxOffset) * _period)}, // + {"trigger_time_error", static_cast(gr::uncertainty(edgeIdxOffset) * _period)}, // + {gr::tag::TRIGGER_OFFSET.shortKey(), static_cast(gr::value(edgeIdxOffset) * _period)}, // + {gr::tag::CONTEXT.shortKey(), context} // + }, + edgePosUnsigned); + + // copy samples up to nPublish + std::copy_n(inputSpan.begin(), edgePosUnsigned, outputSpan.begin()); + std::ignore = inputSpan.consume(edgePosUnsigned); + outputSpan.publish(edgePosUnsigned); + return gr::work::Status::OK; + }; + + for (std::size_t i = 0; i < nProcess; ++i) { + const T sample = inputSpan[i]; + _now += _period; + + if (_trigger.processOne(sample) != NONE) { // edge detected + const std::ptrdiff_t edgePosition = static_cast(i) + static_cast(gr::value(_trigger.lastEdgeIdx)); + + if (_trigger.lastEdge == RISING && !trigger_name_rising_edge.value.empty()) { + return publishEdge(trigger_name_rising_edge, edgePosition); + } + + if (_trigger.lastEdge == FALLING && !trigger_name_falling_edge.value.empty()) { + return publishEdge(trigger_name_falling_edge, edgePosition); + } + } + } + + // no trigger found - just copy samples & tags + std::copy_n(inputSpan.begin(), nProcess, outputSpan.begin()); + forwardTags(nProcess - 1UZ); + return gr::work::Status::OK; + } +}; + +} // namespace gr::blocks::basic + +const inline auto registerTrigger = gr::registerBlock>(gr::globalBlockRegistry()) // + + gr::registerBlock>(gr::globalBlockRegistry()) // + + gr::registerBlock>(gr::globalBlockRegistry()); + +#endif // TRIGGER_HPP diff --git a/blocks/basic/test/CMakeLists.txt b/blocks/basic/test/CMakeLists.txt index 6be6a8ac..e7b3fb6f 100644 --- a/blocks/basic/test/CMakeLists.txt +++ b/blocks/basic/test/CMakeLists.txt @@ -2,15 +2,17 @@ add_ut_test(qa_Converter) add_ut_test(qa_Selector) add_ut_test(qa_sources) add_ut_test(qa_DataSink) +add_ut_test(qa_TriggerBlocks) -if (ENABLE_BLOCK_REGISTRY AND ENABLE_BLOCK_PLUGINS) - add_ut_test(qa_BasicKnownBlocks) -endif () +if(ENABLE_BLOCK_REGISTRY AND ENABLE_BLOCK_PLUGINS) + add_ut_test(qa_BasicKnownBlocks) +endif() add_ut_test(qa_StreamToDataSet) add_ut_test(qa_SyncBlock) message(STATUS "###Python Include Dirs: ${Python3_INCLUDE_DIRS}") -if (PYTHON_AVAILABL AND ENABLE_BLOCK_REGISTRY AND ENABLE_BLOCK_PLUGINS) - add_ut_test(qa_PythonBlock) -endif () - +if(PYTHON_AVAILABLE + AND ENABLE_BLOCK_REGISTRY + AND ENABLE_BLOCK_PLUGINS) + add_ut_test(qa_PythonBlock) +endif() diff --git a/blocks/basic/test/qa_TriggerBlocks.cpp b/blocks/basic/test/qa_TriggerBlocks.cpp new file mode 100644 index 00000000..84d127af --- /dev/null +++ b/blocks/basic/test/qa_TriggerBlocks.cpp @@ -0,0 +1,137 @@ +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +using namespace boost::ut; + +const suite<"SchmittTrigger Block"> triggerTests = [] { + using namespace gr::basic; + using namespace gr::testing; + + constexpr static float sample_rate = 1000.f; // 100 Hz + bool enableVisualTests = false; + if (std::getenv("DISABLE_SENSITIVE_TESTS") == nullptr) { + // conditionally enable visual tests outside the CI + boost::ext::ut::cfg = {.tag = std::vector{"visual", "benchmarks"}}; + enableVisualTests = true; + } + + using enum gr::trigger::InterpolationMethod; + "SchmittTrigger"_test = + [&enableVisualTests] { + Graph graph; + + // create blocks + auto& clockSrc = graph.emplaceBlock>({// + {"sample_rate", sample_rate}, {"n_samples_max", 1000U}, {"name", "ClockSource"}, + {"tag_times", + std::vector{ + 0U, // 0 ms - start - 50ms of bottom plateau + 100'000'000U, // 100 ms - start - ramp-up + 400'000'000U, // 300 ms - 50ms of bottom plateau + 500'000'000U, // 500 ms - start ramp-down + 800'000'000U // 700 ms - 100ms of bottom plateau + }}, + {"tag_values", + std::vector{ + "CMD_BP_START/FAIR.SELECTOR.C=1:S=1:P=0", // + "CMD_BP_START/FAIR.SELECTOR.C=1:S=1:P=1", // + "CMD_BP_START/FAIR.SELECTOR.C=1:S=1:P=2", // + "CMD_BP_START/FAIR.SELECTOR.C=1:S=1:P=3", // + "CMD_BP_START/FAIR.SELECTOR.C=1:S=1:P=4" // + }}, + {"do_zero_order_hold", true}}); + + auto& funcGen = graph.emplaceBlock>({{"sample_rate", sample_rate}, {"name", "FunctionGenerator"}, {"start_value", 0.1f}}); + using namespace function_generator; + expect(funcGen.settings().set(createConstPropertyMap(0.1f), SettingsCtx{.context = "CMD_BP_START/FAIR.SELECTOR.C=1:S=1:P=0"}).empty()); + expect(funcGen.settings().set(createParabolicRampPropertyMap(0.1f, 1.1f, .3f, 0.02f), SettingsCtx{.context = "CMD_BP_START/FAIR.SELECTOR.C=1:S=1:P=1"}).empty()); + expect(funcGen.settings().set(createConstPropertyMap(1.1f), SettingsCtx{.context = "CMD_BP_START/FAIR.SELECTOR.C=1:S=1:P=2"}).empty()); + expect(funcGen.settings().set(createParabolicRampPropertyMap(1.1f, 0.1f, .3f, 0.02f), SettingsCtx{.context = "CMD_BP_START/FAIR.SELECTOR.C=1:S=1:P=3"}).empty()); + expect(funcGen.settings().set(createConstPropertyMap(0.1f), SettingsCtx{.context = "CMD_BP_START/FAIR.SELECTOR.C=1:S=1:P=4"}).empty()); + + auto& schmittTrigger = graph.emplaceBlock>({ + {"name", "SchmittTrigger"}, // + {"threshold", .1f}, // + {"offset", .6f}, // + {"trigger_name_rising_edge", "MY_RISING_EDGE"}, // + {"trigger_name_falling_edge", "MY_FALLING_EDGE"} // + }); + auto& tagSink = graph.emplaceBlock>({{"name", "TagSink"}, {"log_tags", true}, {"log_samples", false}, {"verbose_console", false}}); + + // connect non-UI blocks + expect(eq(ConnectionResult::SUCCESS, graph.connect<"out">(clockSrc).to<"in">(funcGen))) << "connect clockSrc->funcGen"; + expect(eq(ConnectionResult::SUCCESS, graph.connect<"out">(funcGen).to<"in">(schmittTrigger))) << "connect funcGen->schmittTrigger"; + expect(eq(ConnectionResult::SUCCESS, graph.connect<"out">(schmittTrigger).template to<"in">(tagSink))) << "connect schmittTrigger->tagSink"; + + gr::scheduler::Simple sched{std::move(graph)}; // declared here to ensure life-time of graph and blocks inside. + if (enableVisualTests) { // execute UI-based tests + auto& uiSink1 = sched.graph().emplaceBlock>({{"name", "ImChartSink1"}}); + auto& uiSink2 = sched.graph().emplaceBlock>({{"name", "ImChartSink2"}}); + // connect UI blocks + expect(eq(ConnectionResult::SUCCESS, sched.graph().connect<"out">(funcGen).to<"in">(uiSink1))) << "connect funcGen->uiSink1"; + expect(eq(ConnectionResult::SUCCESS, sched.graph().connect<"out">(schmittTrigger).template to<"in">(uiSink2))) << "connect schmittTrigger->uiSink2"; + + std::thread uiLoop([&uiSink1, &uiSink2]() { + bool drawUI = true; + while (drawUI) { + using enum gr::work::Status; + drawUI = false; + drawUI |= uiSink1.draw({{"reset_view", true}}) != DONE; + drawUI |= uiSink2.draw({}) != DONE; + std::this_thread::sleep_for(std::chrono::milliseconds(40)); + } + std::this_thread::sleep_for(std::chrono::seconds(1)); // wait before shutting down + }); + + expect(sched.runAndWait().has_value()) << "runAndWait"; + + uiLoop.join(); + enableVisualTests = false; // only for first test + } else { + // non-UI test + expect(sched.runAndWait().has_value()) << "runAndWait"; + } + + expect(eq(tagSink._tags.size(), 7UZ)) << fmt::format("test {} : expected total number of tags", magic_enum::enum_name(Method::value)); + + // filter tags for those generated on rising and falling edges + std::vector rising_edge_indices; + std::vector falling_edge_indices; + + for (const auto& tag : tagSink._tags) { + if (!tag.map.contains(std::string(gr::tag::TRIGGER_NAME.shortKey()))) { + continue; + } + std::string trigger_name = std::get(tag.map.at(std::string(gr::tag::TRIGGER_NAME.shortKey()))); + if (trigger_name == "MY_RISING_EDGE") { + rising_edge_indices.push_back(tag.index); + } else if (trigger_name == "MY_FALLING_EDGE") { + falling_edge_indices.push_back(tag.index); + } + } + expect(eq(rising_edge_indices.size(), 1UZ)) << fmt::format("test {} : expected one rising edge", magic_enum::enum_name(Method::value)); + expect(eq(falling_edge_indices.size(), 1UZ)) << fmt::format("test {} : expected one falling edge", magic_enum::enum_name(Method::value)); + + if (Method::value == NO_INTERPOLATION) { // edge position once crossing the threshold + expect(approx(rising_edge_indices[0], 278UZ, 2UZ)) << fmt::format("test {} : detected rising edge index", magic_enum::enum_name(Method::value)); + expect(approx(falling_edge_indices[0], 678UZ, 2UZ)) << fmt::format("test {} : detected falling edge index", magic_enum::enum_name(Method::value)); + } else { // exact edge position + expect(approx(rising_edge_indices[0], 250UZ, 2UZ)) << fmt::format("test {} : detected rising edge index", magic_enum::enum_name(Method::value)); + expect(approx(falling_edge_indices[0], 650UZ, 2UZ)) << fmt::format("test {} : detected falling edge index", magic_enum::enum_name(Method::value)); + } + } | + std::tuple, // + std::integral_constant, // + std::integral_constant>{}; +}; + +int main() { /* not needed for UT */ }