diff --git a/rims_app/src/common.hpp b/rims_app/src/common.hpp index d2e05b7514e..f22bda001a8 100644 --- a/rims_app/src/common.hpp +++ b/rims_app/src/common.hpp @@ -1,15 +1,10 @@ #pragma once -#include "log.hpp" -#include "syscalls/kernel.h" -#include "zephyr/sys/time_units.h" -#include "zephyr/sys_clock.h" +#include "function.hpp" #include #include #include - -#include #include #include #include @@ -17,6 +12,8 @@ #include #include #include +#include +#include #include #include @@ -127,7 +124,7 @@ class Timer { /* This is the time until the first expiration of the timer after you call k_timer_start(). It's a one-time initial delay before the timer "fires" the first time. */ k_timeout_t _duration; - + /* After the first expiration, the timer will automatically restart and continue to expire repeatedly with this interval (period time) between each expiration. If you set period to K_NO_WAIT, the timer only fires once (it becomes a one-shot timer).*/ k_timeout_t _period; @@ -164,7 +161,7 @@ class RecurringSemaphoreTimer : public Timer { class SingleShootTimer : public Timer { public: - SingleShootTimer(std::function cb, std::chrono::microseconds duration) : Timer(duration), _cb{std::move(cb)} { + SingleShootTimer(rims::function cb, std::chrono::microseconds duration) : Timer(duration), _cb{std::move(cb)} { } SingleShootTimer(const SingleShootTimer &) = delete; @@ -176,7 +173,7 @@ class SingleShootTimer : public Timer { void expiry_cb() override { _cb(); } - + void fire_at(clock::time_point target) { auto now = clock::now(); if (target <= now) { @@ -184,27 +181,16 @@ class SingleShootTimer : public Timer { _duration = chronoToKTimeout(std::chrono::microseconds{1}); } else { auto delay = std::chrono::duration_cast(target - now); - _duration = chronoToKTimeout(delay); + _duration = chronoToKTimeout(delay); } - + _period = K_NO_WAIT; // Ensure it's a one-shot start(); } - std::function _cb; + rims::function _cb; }; -// class RecurringTimer : public Timer { -// public: -// RecurringTimer(std::function cb, std::chrono::milliseconds interval) : Timer(interval, interval), _cb{std::move(cb)} { -// } -// void expiry_cb() override { -// _cb(); -// } - -// std::function _cb; -// }; - struct TStackBase { z_thread_stack_element *_sp; std::size_t _size; @@ -297,6 +283,4 @@ constexpr auto ChannelNumber = 2; constexpr int GPIO_PIN_PB5 = 5; constexpr int GPIO_PIN_PB6 = 6; - - } // namespace rims diff --git a/rims_app/src/fifo_queue.hpp b/rims_app/src/fifo_queue.hpp index a7e206c57bd..1361af96bcc 100644 --- a/rims_app/src/fifo_queue.hpp +++ b/rims_app/src/fifo_queue.hpp @@ -2,7 +2,9 @@ #include "ring_buffer.hpp" -namespace rims{ +#include + +namespace rims { template struct ZephyrFifoElement { void *_reserved; @@ -14,19 +16,19 @@ template class fifo_queue { // static_assert(std::is_trivial_v); fifo_queue(k_fifo &fifo) : _fifo{fifo} { } - + void k_event_init(k_poll_event &event) { zephyr::event_pool::k_init(event, _fifo); } - + template // bool try_consume(Fn fn) { // std::lock_guard _lock{_mutex}; if (_elements.empty()) return false; - - /// read from queue + + /// read from queue ZephyrFifoElement *el = reinterpret_cast *>(k_fifo_get(&_fifo, K_NO_WAIT)); - + try { if (not el) return false; // should be a assert fn(el->item); // consume item, fn can throw @@ -35,15 +37,15 @@ template class fifo_queue { _elements.pop_front(); throw; } - + return true; }; - + template // bool try_produce(Fn fn) { // std::lock_guard _lock{_mutex}; if (_elements.full()) return false; - + auto tmp = ZephyrFifoElement{}; if (fn(tmp.item)) // fill new data { @@ -51,13 +53,13 @@ template class fifo_queue { k_fifo_put(&_fifo, &el); // put data into a queue return true; } - + return false; } - + private: // ZephyrMutex _mutex{}; - ring_buffer, N> _elements{}; + RingBuffer, N> _elements{}; k_fifo &_fifo; }; -} +} // namespace rims diff --git a/rims_app/src/function.hpp b/rims_app/src/function.hpp index 44763bdc6bf..fe7eefc18b9 100644 --- a/rims_app/src/function.hpp +++ b/rims_app/src/function.hpp @@ -1,81 +1,198 @@ #pragma once -#include -#include +#include "details/function.hpp" +#include +#include +#include #include #include -#include // For std::memset -namespace rims{ -template -class Function; +namespace rims { +template struct trivial_function; +template struct function; + +// A simple std::function alternative that never allocates. It contains +// enough space to store a lambda that captures N pointer-sized objects. +// The lambda's destructor and copy/move constructors are never used, +// which makes it very cheap to pass around. This also implies that only +// trivial types, such as pointers and references, can safely be captured. +template struct trivial_function { + constexpr trivial_function() noexcept = default; + constexpr ~trivial_function() = default; + constexpr trivial_function(trivial_function &&) noexcept = default; + constexpr trivial_function(const trivial_function &) noexcept = default; + constexpr trivial_function &operator=(trivial_function &&) noexcept = default; + constexpr trivial_function &operator=(const trivial_function &) noexcept = default; + + template trivial_function(function &&) = delete; + template trivial_function(const function &) = delete; + + constexpr trivial_function(std::nullptr_t) noexcept : trivial_function{} { + } + + template + requires(not detail::is_function_instance>) + explicit trivial_function(F &&func) : trivial_function{create(std::forward(func))} { + } + + template trivial_function &operator=(F &&func) noexcept { + return *this = trivial_function{std::forward(func)}; + } + + template + requires(M < N) + trivial_function(const trivial_function &other) noexcept : call{other.call} { + std::memcpy(storage, &other.storage, sizeof(other.storage)); + } + + R operator()(A... args) const { + return call(&storage, std::forward(args)...); + } + + constexpr bool valid() const noexcept { + return call != nullptr; + } + explicit constexpr operator bool() const noexcept { + return valid(); + } -template -class Function { private: - static constexpr size_t BufferSize = 16; // Adjust this size as needed for your use case. - using InvokeFn = Ret (*)(void*, Args&&...); - - alignas(std::max_align_t) char buffer[BufferSize]; - InvokeFn invokeFn = nullptr; - - template - static Ret invokeImpl(void* callable, Args&&... args) { - return (*reinterpret_cast(callable))(std::forward(args)...); + template static trivial_function create(F &&func) { + using functor = detail::functor>; + static_assert(std::is_trivially_destructible_v); + static_assert(sizeof(functor) <= sizeof(dummy)); + static_assert(alignof(functor) <= alignof(dummy)); + trivial_function f; + new (&f.storage) functor{std::forward(func)}; + f.call = functor::template call; + return f; } - - template - void storeCallable(Callable&& callable) { - using CallableType = std::decay_t; - static_assert(sizeof(CallableType) <= BufferSize, "Callable too large for Function buffer"); - static_assert(alignof(CallableType) <= alignof(std::max_align_t), "Callable alignment exceeds max_align_t"); - - new (buffer) CallableType(std::forward(callable)); - invokeFn = &invokeImpl; + + template friend struct trivial_function; + template friend struct function; + using dummy = detail::functor>()](A...) {})>; + + union { + struct { + } nothing{}; + alignas(dummy) std::byte storage[sizeof(dummy)]; + }; + R (*call)(const void *, A &&...){nullptr}; +}; + +template ::type> +trivial_function(F) -> trivial_function; + +// A fixed-size function object that can store non-trivial lambdas. It is +// larger than trivial_function and requires the use of virtual function +// calls on copy/move/destroy. +template struct function { + constexpr function() noexcept = default; + constexpr ~function() { + if (call != nullptr) vtable->destroy(&storage); } - - public: - Function() = default; - - template - Function(Callable&& callable) { - storeCallable(std::forward(callable)); + + template function &operator=(F &&func) { + return assign(std::forward(func)); } - - Function(const Function&) = delete; // Disable copying for simplicity - Function& operator=(const Function&) = delete; - - Function(Function&& other) noexcept { - std::memcpy(buffer, other.buffer, BufferSize); - invokeFn = other.invokeFn; - other.invokeFn = nullptr; + + constexpr function(std::nullptr_t) noexcept : function{} { } - - Function& operator=(Function&& other) noexcept { - if (this != &other) { - this->~Function(); - std::memcpy(buffer, other.buffer, BufferSize); - invokeFn = other.invokeFn; - other.invokeFn = nullptr; - } - return *this; + + template + requires(not detail::is_function_instance>) + explicit function(F &&func) : function{create(std::forward(func))} { } - - ~Function() { - if (invokeFn) { - reinterpret_cast(buffer)(buffer); - } + + template + requires(M <= N) + function(function &&other) : vtable{other.vtable}, call{other.call} { + if (call == nullptr) return; + vtable->move(&storage, &other.storage); + other.call = nullptr; } - - Ret operator()(Args... args) const { - if (!invokeFn) { - // throw std::bad_function_call(); - assert(false); ///TODO handle asserts - } - return invokeFn(const_cast(reinterpret_cast(buffer)), std::forward(args)...); + + template + requires(M <= N) + function(const function &other) noexcept : vtable{other.vtable}, call{other.call} { + if (call == nullptr) return; + vtable->copy(&storage, &other.storage); } - - explicit operator bool() const noexcept { - return invokeFn != nullptr; + + template + requires(M <= N) + function(const trivial_function &other) noexcept + : vtable{detail::functor_vtable::trivial()}, call{other.call} { + std::memcpy(&storage, &other.storage, sizeof(storage)); } -};} + + R operator()(A... args) const { + return call(&storage, std::forward(args)...); + } + + constexpr bool valid() const noexcept { + return call != nullptr; + } + explicit constexpr operator bool() const noexcept { + return valid(); + } + + private: + template static function create(F &&func) { + using functor = detail::functor>; + static_assert(sizeof(functor) <= sizeof(dummy)); + static_assert(alignof(functor) <= alignof(dummy)); + function f; + new (&f.storage) functor{std::forward(func)}; + f.call = functor::template call; + f.vtable = detail::functor_vtable::create>(); + return f; + } + + template function &assign(F &&other) { + this->~function(); + return *new (this) function{std::forward(other)}; + } + + template friend struct function; + using dummy = trivial_function::dummy; + + union { + struct { + } nothing{}; + struct { + alignas(dummy) std::byte storage[sizeof(dummy)]; + const detail::functor_vtable *vtable; + }; + }; + R (*call)(const void *, A &&...){nullptr}; +}; + +template ::type> +function(F) -> function; + +// A single-use function object with stored arguments. +template struct callable_tuple { + template constexpr callable_tuple(E &&...elements) : tuple{std::forward(elements)...} { + } + + template decltype(auto) operator()(A &&...args) { + return call(std::make_index_sequence>{}, std::forward(args)...); + } + + private: + template decltype(auto) call(std::index_sequence, A &&...args) { + return std::invoke(std::get(std::move(tuple))..., std::forward(args)...); + } + + T tuple; +}; + +template callable_tuple(E...) -> callable_tuple...>>; +} // namespace rims + +namespace rims::detail { +template inline constexpr bool is_function_instance> = true; + +template inline constexpr bool is_function_instance> = true; +} // namespace rims::detail diff --git a/rims_app/src/gpio.cpp b/rims_app/src/gpio.cpp index 030234f055a..baa18f8c33a 100644 --- a/rims_app/src/gpio.cpp +++ b/rims_app/src/gpio.cpp @@ -1,6 +1,7 @@ #include "gpio.hpp" #include "proto/gpio.pb.h" #include "zephyr.hpp" +#include "log.hpp" #include namespace rims { diff --git a/rims_app/src/gpio.hpp b/rims_app/src/gpio.hpp index d5c78475a67..3d13c5bc221 100644 --- a/rims_app/src/gpio.hpp +++ b/rims_app/src/gpio.hpp @@ -1,10 +1,8 @@ #include "common.hpp" -#include "ring_buffer.hpp" +#include "fifo_queue.hpp" #include "proto/gpio.pb.h" -#include "zephyr.hpp" -#include "zephyr/kernel.h" #include namespace rims { diff --git a/rims_app/src/log.hpp b/rims_app/src/log.hpp index c03aa0230bb..fcdda173022 100644 --- a/rims_app/src/log.hpp +++ b/rims_app/src/log.hpp @@ -2,11 +2,11 @@ #include #include -#include #include #include #include "fifo_queue.hpp" +#include "function.hpp" #include "proto/log.pb.h" namespace rims { @@ -28,7 +28,7 @@ static LogLevel g_logLevel = LogLevel::Debug; class Log { public: using Level = LogLevel; - using Sink = std::function; + using Sink = rims::function; // static void registerSink(std::string_view name, Sink sink); // static void unregisterSink(std::string_view name); diff --git a/rims_app/src/messenger.cpp b/rims_app/src/messenger.cpp index f950d26fe3a..3a59a50b8e0 100644 --- a/rims_app/src/messenger.cpp +++ b/rims_app/src/messenger.cpp @@ -37,7 +37,7 @@ #include "zephyr.hpp" namespace rims { -K_MSGQ_DEFINE(messenger_buffer_arrived_queue, sizeof(rims::buffer), 2, 1); +K_MSGQ_DEFINE(messenger_buffer_arrived_queue, sizeof(rims::BufferView), 2, 1); namespace { bool crcIsInvalid(std::span data, std::uint32_t crc) { @@ -222,7 +222,7 @@ void MessengerThread::event_dataArrived() { std::span submessage{}; WireFormatID id{}; uint32_t crc{}; - buffer buf; + BufferView buf; try { k_msgq_get(&messenger_buffer_arrived_queue, &buf, K_NO_WAIT); @@ -493,8 +493,8 @@ void MessengerThread::putActiveRequest(WireFormatID wireId, uint32_t id, AsyncUA throw messenger::request_queue_full{}; } -std::optional MessengerThread::takeActiveRequest(int wireId_receiver, uint32_t id) { - std::optional ret{}; +std::optional MessengerThread::takeActiveRequest(int wireId_receiver, uint32_t id) { + std::optional ret{}; // find first of active type/requestId pair for (auto &req : _activeRequests) { if (req.wireId.receiver == wireId_receiver && req.requestId == id) { diff --git a/rims_app/src/messenger.hpp b/rims_app/src/messenger.hpp index 4187047ab00..1f584e807e0 100644 --- a/rims_app/src/messenger.hpp +++ b/rims_app/src/messenger.hpp @@ -5,12 +5,13 @@ #include "id.hpp" #include "proto/message.pb.h" +#include #include namespace rims { class AsyncUART; -struct buffer { +struct BufferView { std::span data; AsyncUART *device; }; @@ -28,7 +29,7 @@ class MessengerThread : public ZephyrThread { void threadMain() override; private: - struct requestData { + struct RequestData { WireFormatID wireId; // original ID of IngressMessage uint32_t requestId; // requestID from message AsyncUART *cb; @@ -53,10 +54,10 @@ class MessengerThread : public ZephyrThread { void egressPush(void *submessage, const pb_msgdesc_t &fields, int id, std::optional requestId); void putActiveRequest(WireFormatID wireId, uint32_t id, AsyncUART *cb); - std::optional takeActiveRequest(int type, uint32_t id); + std::optional takeActiveRequest(int type, uint32_t id); std::array _events; - std::array _activeRequests; + std::array _activeRequests; uint8_t _activeRequestsNr; }; } // namespace rims diff --git a/rims_app/src/power_control.cpp b/rims_app/src/power_control.cpp index 7eea547b342..ab099c8f5b4 100644 --- a/rims_app/src/power_control.cpp +++ b/rims_app/src/power_control.cpp @@ -1,6 +1,5 @@ #include "power_control.hpp" -#include "ring_buffer.hpp" #include "common.hpp" #include "log.hpp" #include "proto/ctrl.pb.h" @@ -15,7 +14,6 @@ #include #include - namespace rims { namespace ctrl { @@ -58,8 +56,8 @@ constexpr std::array pins = { PhaseModulation::PhaseModulation(const gpio_dt_spec &gpio) : PinControlStrategyBase(gpio), // - _triacTimerStart{[this]() { this->on(); }, std::chrono::milliseconds{0}}, - _triacTimerStop{[this]() { this->off(); }, std::chrono::milliseconds{0}} { + _triacTimerStart{rims::function{[this]() { this->on(); }}, std::chrono::milliseconds{0}}, + _triacTimerStop{rims::function{[this]() { this->off(); }}, std::chrono::milliseconds{0}} { ULOG_INFO("PhaseModulation started for gpio %d", gpio.pin); } @@ -83,25 +81,24 @@ void PhaseModulation::zeroCrossDetectionTick(ZeroCrossDetectionEvent::Data data) // Calculate offset: higher power -> shorter delay, lower power -> longer delay // _power in percent const float clampedPower = std::clamp(_power, 0.0f, 100.0f); - + const auto usToNext = data.cycle_duration; - + const auto maxDelay = usToNext - minimalGatePulse - minimalLatchTime - minimalSafeMargin; const auto onDelay = std::min(delay(clampedPower, usToNext), maxDelay); if (onDelay > maxDelay) { return; } - - + // Start timer to turn triac ON at calculated phase delay // _triacTimerStart.fire_at(data.restored() + data.to_zcd + onDelay - minimalLatchTime); // Schedule triac to turn OFF shortly after firing (e.g., 100 us pulse) // _triacTimerStop.fire_at(data.restored() + data.to_zcd + onDelay + minimalGatePulse); // _triacTimerStop.setDuration(onDelay + minimalGatePulse); - + // // Start timer to turn triac ON at calculated phase delay - _triacTimerStart.setDuration(onDelay+data.to_zcd); + _triacTimerStart.setDuration(onDelay + data.to_zcd); _triacTimerStart.start(); // // Schedule triac to turn OFF shortly after firing (e.g., 100 us pulse) @@ -362,7 +359,7 @@ void PhaseModulationOrchestrator::checkCurrentMode(const uint8_t channel, ModeOf void PhaseModulationOrchestrator::setMode(uint8_t ch, ModeOfOperation mode) { checkChannel(ch); - + switch (mode) { case ctrl_ModeOfOperation_Disabled: _chModeOfOperationStorage[ch].emplace(); @@ -399,7 +396,7 @@ ModeOfOperation PhaseModulationOrchestrator::mode(uint8_t ch) { void PhaseModulationOrchestrator::setPowerControlAlgorithm(uint8_t ch, PowerControlAlgorithm alg) { checkChannel(ch); - + switch (alg) { case ctrl_PowerControlAlgorithm_NoModulation: _chPowerControlStrategy[ch].emplace(); diff --git a/rims_app/src/power_control.hpp b/rims_app/src/power_control.hpp index 56dfe507e48..76ecad92263 100644 --- a/rims_app/src/power_control.hpp +++ b/rims_app/src/power_control.hpp @@ -1,7 +1,6 @@ #pragma once /* Kernel event for notifying other threads */ -#include "ring_buffer.hpp" #include "common.hpp" #include "inplace_vector.hpp" #include "proto/ctrl.pb.h" diff --git a/rims_app/src/ring_buffer.hpp b/rims_app/src/ring_buffer.hpp index 1e94fa21dd0..c0c343ced5c 100644 --- a/rims_app/src/ring_buffer.hpp +++ b/rims_app/src/ring_buffer.hpp @@ -6,7 +6,6 @@ #include #include -#include #include #include @@ -151,14 +150,14 @@ struct RingIndex { bool _full{false}; }; -template class ring_buffer { +template class RingBuffer { public: static_assert(std::is_trivial_v); static_assert(std::is_trivially_destructible_v); static_assert(std::is_trivially_copyable_v); using value_type = T; - explicit ring_buffer() : _index{N} { + explicit RingBuffer() : _index{N} { } T &push_back(const T &item) { diff --git a/rims_app/src/temperature_measurements.hpp b/rims_app/src/temperature_measurements.hpp index ec028d9db22..c24590357a7 100644 --- a/rims_app/src/temperature_measurements.hpp +++ b/rims_app/src/temperature_measurements.hpp @@ -1,6 +1,6 @@ #pragma once -#include "ring_buffer.hpp" +#include "fifo_queue.hpp" #include "common.hpp" #include "proto/temperature.pb.h" @@ -82,7 +82,7 @@ class TemperatureSampler { zephyr::semaphore::sem _broadcastSem{0, 1}; RecurringSemaphoreTimer _broadcastTimer{_broadcastSem, std::chrono::seconds{60}}; - ring_buffer _samples; + RingBuffer _samples; // number of samples taken to filter std::uint8_t _samplesNumber{MaxSampleSize}; diff --git a/rims_app/src/uart.cpp b/rims_app/src/uart.cpp index fee2c80fee1..4ec064887fb 100644 --- a/rims_app/src/uart.cpp +++ b/rims_app/src/uart.cpp @@ -196,7 +196,7 @@ uint8_t AsyncUART::rxByte() { } void AsyncUART::processMessage() { - buffer buf{.data = {rxBuffer().data(), rxBuffer().size()}, .device = this}; + BufferView buf{.data = {rxBuffer().data(), rxBuffer().size()}, .device = this}; switchRxBuffer(); k_msgq_put(&messenger_buffer_arrived_queue, &buf, K_NO_WAIT); } diff --git a/rims_app/src/uart.hpp b/rims_app/src/uart.hpp index 7678b328a12..54a8a498cb4 100644 --- a/rims_app/src/uart.hpp +++ b/rims_app/src/uart.hpp @@ -19,7 +19,7 @@ class uart_rx_not_ready_error; class AsyncUART { using rx_buffer_t = beman::inplace_vector; - using tx_buffer_t = ring_buffer; + using tx_buffer_t = RingBuffer; public: AsyncUART(); diff --git a/rims_app/src/zephyr.hpp b/rims_app/src/zephyr.hpp index ed490911c82..9ea3533288e 100644 --- a/rims_app/src/zephyr.hpp +++ b/rims_app/src/zephyr.hpp @@ -1,12 +1,12 @@ #pragma once -#include "zephyr/drivers/adc.h" #include #include #include #include #include +#include #include #include #include diff --git a/rims_app/src/zero_cross_detection.hpp b/rims_app/src/zero_cross_detection.hpp index 0afd910c728..223db1e7b62 100644 --- a/rims_app/src/zero_cross_detection.hpp +++ b/rims_app/src/zero_cross_detection.hpp @@ -1,8 +1,8 @@ #pragma once /* Kernel event for notifying other threads */ -#include "ring_buffer.hpp" #include "common.hpp" +#include "fifo_queue.hpp" #include "zephyr.hpp" #include @@ -16,8 +16,8 @@ struct ZeroCrossDetectionEvent { return clock::time_point{std::chrono::nanoseconds{time_point}}; } void store_now() { - auto now = clock::now(); - time_point = std::chrono::duration_cast(now.time_since_epoch()).count(); + auto now = clock::now(); + time_point = std::chrono::duration_cast(now.time_since_epoch()).count(); } uint64_t time_point; // timepoint microseconds_u16_t cycle_duration; @@ -52,14 +52,21 @@ class ZeroCrossDetection { zephyr::semaphore::sem _intCheckSem; RecurringSemaphoreTimer _intCheckTimer; - clock::time_point _pulseDown{}; - rims::ring_buffer _pulsWidths; - rims::ring_buffer _cyclesWidths; + clock::time_point _pulseDown{}; + rims::RingBuffer _pulsWidths; + rims::RingBuffer _cyclesWidths; }; class ZeroCrossDetectionOrchestrator { public: ZeroCrossDetectionOrchestrator(); + + ZeroCrossDetectionOrchestrator(const ZeroCrossDetectionOrchestrator &) = delete; + ZeroCrossDetectionOrchestrator &operator=(const ZeroCrossDetectionOrchestrator &) = delete; + + ZeroCrossDetectionOrchestrator(ZeroCrossDetectionOrchestrator &&) = delete; + ZeroCrossDetectionOrchestrator &operator=(ZeroCrossDetectionOrchestrator &&) = delete; + void loop(); void event_zcdCheck(ZeroCrossDetection &channel);