From 76e687a87aba9db5e03ec29558f538bbc3a0343c Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Wed, 5 Mar 2025 20:11:42 +0100 Subject: [PATCH 01/18] exceptions --- rims_app/CMakeLists.txt | 15 +- rims_app/prj.conf | 3 +- rims_app/{ => proto}/common.proto | 2 + rims_app/{ => proto}/configuration.proto | 0 rims_app/{ => proto}/ctrl.proto | 0 rims_app/{ => proto}/log.proto | 0 rims_app/{ => proto}/message.proto | 10 +- rims_app/{ => proto}/temperature.proto | 0 rims_app/{ => proto}/utilization.proto | 0 rims_app/src/config.cpp | 2 +- rims_app/src/config.hpp | 2 +- rims_app/src/log.cpp | 4 +- rims_app/src/log.hpp | 2 +- rims_app/src/main.cpp | 40 +- rims_app/src/message_decode.cpp | 474 +++++++++++++++------- rims_app/src/message_decode.hpp | 22 +- rims_app/src/phase_modulation.cpp | 2 +- rims_app/src/phase_modulation.hpp | 2 +- rims_app/src/temperature_measurements.cpp | 2 +- rims_app/src/temperature_measurements.hpp | 2 +- rims_app/src/uart.cpp | 8 +- rims_app/src/uart.hpp | 4 +- 22 files changed, 396 insertions(+), 200 deletions(-) rename rims_app/{ => proto}/common.proto (98%) rename rims_app/{ => proto}/configuration.proto (100%) rename rims_app/{ => proto}/ctrl.proto (100%) rename rims_app/{ => proto}/log.proto (100%) rename rims_app/{ => proto}/message.proto (71%) rename rims_app/{ => proto}/temperature.proto (100%) rename rims_app/{ => proto}/utilization.proto (100%) diff --git a/rims_app/CMakeLists.txt b/rims_app/CMakeLists.txt index 8d7099cf169..8f312b837fa 100644 --- a/rims_app/CMakeLists.txt +++ b/rims_app/CMakeLists.txt @@ -1,20 +1,17 @@ -# SPDX-License-Identifier: Apache-2.0 - cmake_minimum_required(VERSION 3.13.1) find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) project(rims_controller C CXX) -set(CMAKE_C_STANDARD 11) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) list(APPEND CMAKE_MODULE_PATH ${ZEPHYR_BASE}/modules/nanopb) include(nanopb) -FILE(GLOB app_sources src/*.cpp src/*.c src/*.hpp src/*.h *.proto) -# file(GLOB app_sources src/mainc.c) +FILE(GLOB app_sources src/*.cpp src/*.c src/*.hpp src/*.h) +FILE(GLOB app_protos proto/*.proto) -target_sources(app PRIVATE ${app_sources} ) +target_sources(app PRIVATE ${app_sources} ${app_protos}) +target_compile_options(app PUBLIC -fexceptions) -zephyr_nanopb_sources(app message.proto configuration.proto temperature.proto ctrl.proto log.proto utilization.proto) - -target_compile_options(app PUBLIC -Wno-psabi -fno-strict-aliasing) +zephyr_nanopb_sources(app ${app_protos}) diff --git a/rims_app/prj.conf b/rims_app/prj.conf index ee777d885ea..9825a81ff62 100644 --- a/rims_app/prj.conf +++ b/rims_app/prj.conf @@ -20,8 +20,9 @@ CONFIG_CPP=y CONFIG_STD_CPP2B=y CONFIG_CPP_EXCEPTIONS=y CONFIG_CPP_RTTI=n -CONFIG_COMMON_LIBC_MALLOC_ARENA_SIZE=512 +#CONFIG_COMMON_LIBC_MALLOC_ARENA_SIZE=512 CONFIG_REQUIRES_FULL_LIBCPP=y +CONFIG_NEWLIB_LIBC_NANO=n CONFIG_THREAD_ANALYZER=y CONFIG_THREAD_NAME=y diff --git a/rims_app/common.proto b/rims_app/proto/common.proto similarity index 98% rename from rims_app/common.proto rename to rims_app/proto/common.proto index d1b1b893dac..88cac084a71 100644 --- a/rims_app/common.proto +++ b/rims_app/proto/common.proto @@ -1,3 +1,5 @@ +syntax = "proto3"; + /* Request/Response Architecture diff --git a/rims_app/configuration.proto b/rims_app/proto/configuration.proto similarity index 100% rename from rims_app/configuration.proto rename to rims_app/proto/configuration.proto diff --git a/rims_app/ctrl.proto b/rims_app/proto/ctrl.proto similarity index 100% rename from rims_app/ctrl.proto rename to rims_app/proto/ctrl.proto diff --git a/rims_app/log.proto b/rims_app/proto/log.proto similarity index 100% rename from rims_app/log.proto rename to rims_app/proto/log.proto diff --git a/rims_app/message.proto b/rims_app/proto/message.proto similarity index 71% rename from rims_app/message.proto rename to rims_app/proto/message.proto index e9bde3f089f..c3fa9f5b115 100644 --- a/rims_app/message.proto +++ b/rims_app/proto/message.proto @@ -6,7 +6,15 @@ enum Error { NoError = 0; BadCRC = 1; BadId = 2; - UnknownId = 3; + BadData = 3; + + NoCRC = 4; + NoId = 5; + NoData = 6; + + RequestQueueFull =8; + + UnknownId = 7; } message Message diff --git a/rims_app/temperature.proto b/rims_app/proto/temperature.proto similarity index 100% rename from rims_app/temperature.proto rename to rims_app/proto/temperature.proto diff --git a/rims_app/utilization.proto b/rims_app/proto/utilization.proto similarity index 100% rename from rims_app/utilization.proto rename to rims_app/proto/utilization.proto diff --git a/rims_app/src/config.cpp b/rims_app/src/config.cpp index 7f136c6da70..974df6ecf5e 100644 --- a/rims_app/src/config.cpp +++ b/rims_app/src/config.cpp @@ -1,5 +1,5 @@ #include "config.hpp" -#include "configuration.pb.h" +#include "proto/configuration.pb.h" namespace rims { diff --git a/rims_app/src/config.hpp b/rims_app/src/config.hpp index 5814870c629..33dfccaa328 100644 --- a/rims_app/src/config.hpp +++ b/rims_app/src/config.hpp @@ -1,7 +1,7 @@ #pragma once #include "circular_buffer.hpp" -#include "configuration.pb.h" +#include "proto/configuration.pb.h" namespace rims{ diff --git a/rims_app/src/log.cpp b/rims_app/src/log.cpp index 03fc8e72a0f..c05b31f1ed0 100644 --- a/rims_app/src/log.cpp +++ b/rims_app/src/log.cpp @@ -1,10 +1,8 @@ #include "log.hpp" -#include "log.pb.h" #include "zephyr/kernel.h" #include -#include -#include #include +#include namespace rims { K_FIFO_DEFINE(klogFifoQueue); diff --git a/rims_app/src/log.hpp b/rims_app/src/log.hpp index a8760d47ce7..c460cf4fdc9 100644 --- a/rims_app/src/log.hpp +++ b/rims_app/src/log.hpp @@ -7,7 +7,7 @@ #include #include "circular_buffer.hpp" -#include "log.pb.h" +#include "proto/log.pb.h" namespace rims { diff --git a/rims_app/src/main.cpp b/rims_app/src/main.cpp index 143f6510648..1b58bfa525f 100644 --- a/rims_app/src/main.cpp +++ b/rims_app/src/main.cpp @@ -1,5 +1,6 @@ #include "common.hpp" +#include "log.hpp" #include "message_decode.hpp" #include "phase_modulation.hpp" #include "temperature_measurements.hpp" @@ -10,15 +11,29 @@ #include #include #include +#include #include -#include +#include #include #include +extern "C" void _exit() { +} + +[[noreturn]] void Terminate() noexcept { + // insert your own termination handler code here... + while (true) { + } +} + +namespace __cxxabiv1 { +std::terminate_handler __terminate_handler = Terminate; +} + using namespace rims; -static K_THREAD_STACK_DEFINE(k_messengerStack, 1024); +static K_THREAD_STACK_DEFINE(k_messengerStack, 2048); TStack messengerStack{k_messengerStack, K_THREAD_STACK_SIZEOF(k_messengerStack)}; static K_THREAD_STACK_DEFINE(k_uartStack, 2048); @@ -38,9 +53,6 @@ TStack phaseModulationStack{k_phaseModulationStack, K_THREAD_STACK_SIZEOF(k_phas /// TODO power measurement thread /// TODO status led thread, print statues -extern "C" void _exit() { -} - // adc_channel_cfg adcch1 = ADC_CHANNEL_CFG_DT(DT_NODELABEL(adc_temp)); // extern int z_clock_hw_cycles_per_sec; @@ -65,6 +77,16 @@ LazyInit uartThread; LazyInit temperatureSampler; LazyInit zcd; LazyInit phaseModulation; +struct except : std::exception {}; + +// extern "C"{ + +// void * _sbrk(int p_amount){ +// static char b[1024]; +// return b; +// } + +// } int main() { using namespace rims; @@ -91,7 +113,13 @@ int main() { phaseModulation->start(); while (1) { - std::this_thread::sleep_for(std::chrono::milliseconds{50}); + std::this_thread::sleep_for(std::chrono::seconds{2}); + try { + throw except{}; + } catch (const except &e) { + + ULOG_ERROR("got exception"); + } } // auto getStats = [](auto &diffs) { diff --git a/rims_app/src/message_decode.cpp b/rims_app/src/message_decode.cpp index d705953d558..61cc374f643 100644 --- a/rims_app/src/message_decode.cpp +++ b/rims_app/src/message_decode.cpp @@ -2,25 +2,27 @@ #include "circular_buffer.hpp" #include "log.hpp" -#include "pb.h" -#include "pb_decode.h" #include "config.hpp" -#include "configuration.pb.h" -#include "ctrl.pb.h" -#include "log.pb.h" -#include "message.pb.h" -#include "pb_encode.h" #include "phase_modulation.hpp" -#include "temperature.pb.h" #include "temperature_measurements.hpp" - #include "uart.hpp" +#include "pb.h" +#include "pb_decode.h" +#include "pb_encode.h" +#include "proto/configuration.pb.h" +#include "proto/ctrl.pb.h" +#include "proto/log.pb.h" +#include "proto/message.pb.h" +#include "proto/temperature.pb.h" + #include #include +#include #include -#include +#include +#include #include #include @@ -30,10 +32,6 @@ #include "zephyr.hpp" namespace rims { -// this buffer can be borrowed directly from uart tx thread, filled and then given back -static pb_byte_t outBuf[300]; -static Message message; - K_MSGQ_DEFINE(messenger_buffer_arrived_queue, sizeof(rims::buffer), 2, 1); /* @@ -56,15 +54,20 @@ def verify_id(received_id: int) -> bool: */ -// static constexpr bool verifyID(uint32_t receivedID) { -// uint32_t data = receivedID >> 3; // Extract the first 29 bits -// uint32_t receivedChecksum = receivedID & 0x7; // Extract the last 3 bits (checksum) +static constexpr bool idIsInvalid(uint32_t receivedID) { + // uint32_t data = receivedID >> 3; // Extract the first 29 bits + // uint32_t receivedChecksum = receivedID & 0x7; // Extract the last 3 bits (checksum) -// // Recalculate checksum from the extracted 29-bit data -// uint32_t calculatedChecksum = (data ^ (data >> 3) ^ (data >> 6) ^ (data >> 9) ^ (data >> 12)) & 0x7; + // // Recalculate checksum from the extracted 29-bit data + // uint32_t calculatedChecksum = (data ^ (data >> 3) ^ (data >> 6) ^ (data >> 9) ^ (data >> 12)) & 0x7; -// return receivedChecksum == calculatedChecksum; // Return true if checksum matches -// } + // return receivedChecksum == calculatedChecksum; // Return true if checksum matches + return false; +} + +static bool crcIsInvalid(std::span data, std::uint32_t crc) { + return zephyr::crc::crc32_ieee(data) != crc; +} // static uint32_t encodeID(uint32_t id) { // /// TODO error handling @@ -73,6 +76,41 @@ def verify_id(received_id: int) -> bool: // return (data << 3) | checksum; // Store checksum in the last 3 bits // } +namespace messenger { +struct error : std::exception { + Error _err; + error(Error e) : _err{e} { + } +}; + +struct no_id : public error { + no_id() : error{Error_NoId} {}; +}; +struct bad_id : public error { + bad_id() : error{Error_BadId} {}; +}; + +struct no_crc : public error { + no_crc() : error{Error_NoCRC} {}; +}; +struct bad_crc : public error { + bad_crc() : error{Error_BadCRC} {}; +}; + +struct no_data : public error { + no_data() : error{Error_NoData} {}; +}; +struct bad_data : public error { + bad_data() : error{Error_BadData} {}; +}; + +struct request_queue_full : public error { + request_queue_full() : error{Error_RequestQueueFull} { + } +}; + +} // namespace messenger + MessengerThread::MessengerThread(TStackBase &stack) : ZephyrThread{stack, 10, 0, "Messenger"} { zephyr::event_pool::k_init(_events.at(0), messenger_buffer_arrived_queue); @@ -83,29 +121,28 @@ MessengerThread::MessengerThread(TStackBase &stack) : ZephyrThread{stack, 10, 0, } } -void rims::MessengerThread::event_temperatureEgress() -{ - temperatureEgressQueue.try_consume([&](temperature_EgressMessages &in) { ipcPush(in, temperature_EgressMessages_msg, 1); }); +void rims::MessengerThread::event_temperatureEgress() { + temperatureEgressQueue.try_consume([&](temperature_EgressMessages &in) { + ipcPush(&in, temperature_EgressMessages_msg, 1, in.has_requestID ? std::optional{in.requestID} : std::nullopt); + }); } -void rims::MessengerThread::event_ctrlEgress() -{ - ctrlEgressQueue.try_consume([&](ctrl_EgressMessages &in) { ipcPush(in, ctrl_EgressMessages_msg, 2); }); +void rims::MessengerThread::event_ctrlEgress() { + ctrlEgressQueue.try_consume([&](ctrl_EgressMessages &in) { + ipcPush(&in, ctrl_EgressMessages_msg, 2, in.has_requestID ? std::optional{in.requestID} : std::nullopt); + }); } -void rims::MessengerThread::event_configEgress() -{ - configEgressQueue.try_consume([&](config_EgressMessages &in) { ipcPush(in, config_EgressMessages_msg, 3); }); +void rims::MessengerThread::event_configEgress() { + configEgressQueue.try_consume([&](config_EgressMessages &in) { + ipcPush(&in, config_EgressMessages_msg, 3, in.has_requestID ? std::optional{in.requestID} : std::nullopt); + }); } -void rims::MessengerThread::event_logEgress() -{ - logEgressFifoQueueBuffer.try_consume([&](log_EgressMessages &in) { ipcPush(in, log_EgressMessages_msg, 4); }); -} - -void MessengerThread::handle_badCRC() -{ - /// TODO +void rims::MessengerThread::event_logEgress() { + logEgressFifoQueueBuffer.try_consume([&](log_EgressMessages &in) { + ipcPush(&in, log_EgressMessages_msg, 4, in.has_requestID ? std::optional{in.requestID} : std::nullopt); + }); } void MessengerThread::threadMain() { @@ -113,8 +150,8 @@ void MessengerThread::threadMain() { ctrlEgressQueue.k_event_init(_events.at(2)); configEgressQueue.k_event_init(_events.at(3)); logEgressFifoQueueBuffer.k_event_init(_events.at(4)); - - while (1) { + + while (1) { // TODO add try catch for incoming data errors auto ret = zephyr::event_pool::k_poll_forever(_events); if (ret == 0) { zephyr::event_pool::k_poll_handle(_events[0], std::bind(&MessengerThread::event_dataArrived, this)); @@ -126,92 +163,222 @@ void MessengerThread::threadMain() { } } +namespace cobs { + +class error : public std::exception {}; + +class decode_error : public error { + public: + cobs_ret_t ret; + decode_error(cobs_ret_t r) : ret{r} { + } +}; +class encode_error : public error { + public: + cobs_ret_t ret; + encode_error(cobs_ret_t r) : ret{r} { + } +}; + +std::size_t decode(const std::span in, std::span out) { + size_t out_dec_len{0}; + auto ret = cobs_decode(in.data(), in.size(), out.data(), out.size(), &out_dec_len); + + if (ret != COBS_RET_SUCCESS) { + throw decode_error{ret}; + } + + return out_dec_len; +} + +std::size_t encode(const std::span in, std::span out) { + size_t out_dec_len{0}; + auto ret = cobs_encode(in.data(), in.size(), out.data(), out.size(), &out_dec_len); + + if (ret != COBS_RET_SUCCESS) { + throw encode_error{ret}; + } + + return out_dec_len; +} +} // namespace cobs + +namespace pb { +class error : public std::exception {}; +class decode_error : public error {}; +class encode_error : public error {}; +class read_error : public error {}; + +inline std::tuple decode_tag(pb_istream_t &stream) { + uint32_t tag{0}; + bool eof{false}; + pb_wire_type_t wire_type; + + if (not pb_decode_tag(&stream, &wire_type, &tag, &eof)) { + throw decode_error{}; + } + + return {tag, wire_type}; +} + +inline uint32_t decode_fixed32(pb_istream_t &stream) { + uint32_t fixed32; + if (not pb_decode_fixed32(&stream, &fixed32)) { + throw decode_error{}; + } + return fixed32; +} + +inline uint32_t decode_varint32(pb_istream_t &stream) { + uint32_t varint32; + if (not pb_decode_varint32(&stream, &varint32)) { + throw decode_error{}; + } + return varint32; +} + +inline void skip_bytes(pb_istream_t &stream, std::size_t count) { + if (not pb_read(&stream, nullptr, count)) { + throw read_error{}; + } +} + +inline pb_istream_t istream_from_span(std::span bytes) { + return pb_istream_from_buffer(bytes.data(), bytes.size_bytes()); +} + +inline pb_ostream_t ostream_from_span(std::span bytes) { + return pb_ostream_from_buffer(bytes.data(), bytes.size_bytes()); +} + +inline void decode(pb_istream_t &stream, const pb_msgdesc_t &fields, void *dest_struct) { + if (not pb_decode(&stream, &fields, dest_struct)) { + throw decode_error{}; + } +} + +inline void encode(pb_ostream_t &stream, const pb_msgdesc_t &fields, const void *src_struct) { + if (not pb_encode(&stream, &fields, src_struct)) { + throw encode_error{}; + } +} + +} // namespace pb + void MessengerThread::event_dataArrived() { - // decode - // put on queue - buffer buf; - k_msgq_get(&messenger_buffer_arrived_queue, &buf, K_NO_WAIT); + try { + buffer buf; + k_msgq_get(&messenger_buffer_arrived_queue, &buf, K_NO_WAIT); - pb_istream_t stream = pb_istream_from_buffer(reinterpret_cast(buf.data), buf.size); + const std::size_t decoded_len = cobs::decode(buf.data, buf.data); + pb_istream_t stream = pb_istream_from_buffer(buf.data.data(), decoded_len); - // pb_istream_t datastream; - uint32_t datasize{}; - pb_byte_t *databegin{}; - uint32_t id{}, crc{}; - bool hasId{false}, hasCrc{false}, hasData{false}; + std::span submessage; + uint32_t id{}, crc{}; - auto decode = [&]() { - uint32_t tag; - bool eof; - pb_wire_type_t wire_t; + const auto decode_id = [&]() { + try { + id = pb::decode_fixed32(stream); + } catch (const pb::error &e) { + throw messenger::no_id{}; + } + if (idIsInvalid(id)) { + throw messenger::bad_id{}; + } + }; - pb_decode_tag(&stream, &wire_t, &tag, &eof); + const auto decode_crc = [&]() { + try { + crc = pb::decode_fixed32(stream); + } catch (const pb::error &e) { + throw messenger::no_crc{}; + } + }; - switch (tag) { - case 1: // crc - hasId = pb_decode_fixed32(&stream, &id); - break; - case 2: // data - pb_decode_varint32(&stream, &datasize); - databegin = (pb_byte_t *)stream.state; - hasData = pb_read(&stream, NULL, (size_t)datasize); // skip data - break; - case 3: // crc - hasCrc = pb_decode_fixed32(&stream, &crc); + const auto decode_data = [&]() { + try { + auto submessageSize = pb::decode_varint32(stream); + auto submessageBegin = (pb_byte_t *)stream.state; + pb::skip_bytes(stream, submessageSize); // skip data + submessage = {submessageBegin, submessageSize}; + + } catch (const pb::error &e) { + throw messenger::no_data{}; + } + }; + + auto decode = [&]() { + auto [tag, wire_t] = pb::decode_tag(stream); + + switch (tag) { + case Message_id_tag: + decode_id(); + break; + case Message_data_tag: + decode_data(); + break; + case Message_crc_tag: + decode_crc(); + break; + } + }; + + decode(); + decode(); + decode(); + + if (crcIsInvalid(submessage, crc)) { + throw messenger::bad_crc{}; + } + + stream = pb::istream_from_span(submessage); + switch (id) { + case 0x01: { /// TODO temperature endpoint ID + handle_temperatureIngressMsg(stream, buf.device); break; } - }; - - decode(); - decode(); - decode(); - - /// TODO check CRC for data - /// TODO check checksum od ID - if(zephyr::crc::crc32_ieee({databegin, datasize}) != crc){ - handle_badCRC(); - } - - stream = pb_istream_from_buffer(databegin, datasize); - switch (id) { - case 0x01: { /// TODO temperature endpoint ID - handle_temperatureIngressMsg(stream, buf.device); - break; - } - case 0x02: /// TODO configuration endpoint ID - { - handle_configIngressMsg(stream, buf.device); - break; - } - case 0x03: /// TODO phase controll endpoint ID - { - handle_ctrlIngressMsg(stream, buf.device); - break; - } - case 0x04: { - // logIngressFifoQueueBuffer.try_produce() - break; - } - default: { - AsyncUART::transmit(buf.device, {buf.data, buf.size}); - break; - } + case 0x02: /// TODO configuration endpoint ID + { + handle_configIngressMsg(stream, buf.device); + break; + } + case 0x03: /// TODO phase controll endpoint ID + { + handle_ctrlIngressMsg(stream, buf.device); + break; + } + case 0x04: { + // logIngressFifoQueueBuffer.try_produce() + break; + } + default: { + AsyncUART::transmit(buf.device, buf.data); + break; + } + } + } catch (const messenger::error &e) { + Message message = Message_init_zero; + message.error = e._err; + ipcPush(message, 16, std::nullopt); + } catch (const cobs::error *e) { + Message message = Message_init_zero; + message.error = Error_UnknownId; + ipcPush(message, 16, std::nullopt); + } catch (const std::exception &e) { + Message message = Message_init_zero; + message.error = Error_UnknownId; + ipcPush(message, 16, std::nullopt); } } void MessengerThread::handle_temperatureIngressMsg(pb_istream_t &stream, AsyncUART *cb) { /// TODO assert acllback /// register req id - + temperatureIngressQueue.try_produce([&](temperature_IngressMessages &request) { request = temperature_IngressMessages_init_zero; - auto ok = decode(stream, &temperature_IngressMessages_msg, &request); - if (not ok) { - return false; - /// TODO handle nok case - } else { - putActiveRequest(1, request.requestID, cb); - } + decode(stream, temperature_IngressMessages_msg, &request); + putActiveRequest(1, request.requestID, cb); return true; }); } @@ -220,13 +387,8 @@ void MessengerThread::handle_ctrlIngressMsg(pb_istream_t &stream, AsyncUART *cb) // register req id ctrlIngressQueue.try_consume([&](ctrl_IngressMessages &request) { request = ctrl_IngressMessages_init_zero; - auto ok = decode(stream, &ctrl_IngressMessages_msg, &request); - if (not ok) { - /// TODO handle nok case - return false; - } else { - putActiveRequest(2, request.requestID, cb); - } + decode(stream, ctrl_IngressMessages_msg, &request); + putActiveRequest(2, request.requestID, cb); return true; }); } @@ -235,62 +397,61 @@ void MessengerThread::handle_configIngressMsg(pb_istream_t &stream, AsyncUART *c // register req id configIngressQueue.try_consume([&](config_IngressMessages &request) { request = config_IngressMessages_init_zero; - auto ok = decode(stream, &config_IngressMessages_msg, &request); - if (not ok) { - /// TODO handle nok case - return false; - } else { - putActiveRequest(2, request.requestID, cb); - } + decode(stream, config_IngressMessages_msg, &request); + putActiveRequest(2, request.requestID, cb); return true; }); } -template void MessengerThread::ipcPush(Msg &m, const pb_msgdesc_t &fields, int id) { - // encode embedded message directly to the buffer of output message to save stack - auto ostream = pb_ostream_from_buffer(message.data.bytes, sizeof(message.data.bytes)); // TODO max data size - pb_encode(&ostream, &fields, &m); - +void MessengerThread::ipcPush(Message &message, int id, std::optional requestId) { // set IF od sender - message.id = id; /// TODO make response ID - message.data.size = ostream.bytes_written; + message.id = id; /// TODO make response ID message.crc = zephyr::crc::crc32_ieee({message.data.bytes, message.data.size}); - // for requests, we have to ahve a callback function to return from where we received message - if (m.has_requestID) { - auto cb = takeActiveRequest(message.id, m.requestID); + // for requests, we have to have a callback function to return from where we received message + if (requestId.has_value()) { + auto cb = takeActiveRequest(message.id, requestId.value()); if (cb == nullptr) { cb = defaultUart(); } - encode(message, cb); + transmit(message, cb); } else { - encode(message, defaultUart()); + transmit(message, defaultUart()); } } -bool MessengerThread::decode(pb_istream_t &stream, const pb_msgdesc_t *fields, void *dest) const { - return pb_decode(&stream, fields, &dest); +void MessengerThread::ipcPush(void *submessage, const pb_msgdesc_t &fields, int id, std::optional requestId) { + // encode embedded message directly to the buffer of output message to save stack + Message message = Message_init_zero; + + auto ostream = pb::ostream_from_span(std::span{message.data.bytes}); // TODO max data size + pb::encode(ostream, fields, submessage); + message.data.size = ostream.bytes_written; + + ipcPush(message, id, requestId); } -bool MessengerThread::encode(Message &msg, AsyncUART *cb) { - auto outputStream = pb_ostream_from_buffer(outBuf+8, sizeof(outBuf)-8); - - pb_encode(&outputStream, &Message_msg, &message); - - unsigned encoded_len; - cobs_ret_t const result = cobs_encode(outBuf+8, outputStream.bytes_written, outBuf, sizeof(outBuf), &encoded_len); - - /// TODO create exceptions for this part insted of return values - - if (result == COBS_RET_SUCCESS) { - // encoding succeeded, 'encoded' and 'encoded_len' hold details. - cb->transmit(cb, std::span{reinterpret_cast(outBuf), encoded_len}); - return true; - } else { - // encoding failed, look to 'result' for details. - assert(false); +void MessengerThread::decode(pb_istream_t &stream, const pb_msgdesc_t &fields, void *dest) const { + try { + pb::decode(stream, fields, dest); + } catch (const pb::decode_error &) { + throw messenger::bad_data{}; } - +} + +bool MessengerThread::transmit(Message &msg, AsyncUART *cb) { + static_assert(LOG_PROTO_LOG_PB_H_MAX_SIZE <= sizeof(Message::data.bytes)); + static_assert(TEMPERATURE_PROTO_TEMPERATURE_PB_H_MAX_SIZE <= sizeof(Message::data.bytes)); + static_assert(CTRL_PROTO_CTRL_PB_H_MAX_SIZE <= sizeof(Message::data.bytes)); + static_assert(CONFIG_PROTO_CONFIGURATION_PB_H_MAX_SIZE <= sizeof(Message::data.bytes)); + + pb_byte_t outBuf[PROTO_MESSAGE_PB_H_MAX_SIZE + 8] = {}; + auto outputStream = std::span{outBuf + 8, sizeof(outBuf) - 8}; + auto pbOutputStream = pb::ostream_from_span(outputStream); + pb::encode(pbOutputStream, Message_msg, &msg); + + unsigned encoded_len = cobs::encode(std::span{outBuf + 8, pbOutputStream.bytes_written}, std::span{outBuf, sizeof(outBuf)}); + cb->transmit(cb, {outBuf, encoded_len}); return true; } @@ -303,6 +464,7 @@ void MessengerThread::putActiveRequest(int type, int id, AsyncUART *cb) { return; } } + throw messenger::request_queue_full{}; } AsyncUART *MessengerThread::takeActiveRequest(int type, int id) { diff --git a/rims_app/src/message_decode.hpp b/rims_app/src/message_decode.hpp index 548f92578ae..a93ca679350 100644 --- a/rims_app/src/message_decode.hpp +++ b/rims_app/src/message_decode.hpp @@ -1,18 +1,19 @@ #pragma once #include "common.hpp" -#include "message.pb.h" #include "uart.hpp" #include "zephyr/kernel.h" -#include +#include "proto/message.pb.h" + +#include #include +#include namespace rims { struct buffer { - std::byte *data; - std::size_t size; - AsyncUART *device; + std::span data; + AsyncUART *device; }; extern k_msgq messenger_buffer_arrived_queue; @@ -40,16 +41,15 @@ class MessengerThread : public ZephyrThread { void event_configEgress(); void event_logEgress(); - void handle_badCRC(); - void handle_temperatureIngressMsg(pb_istream_t &stream, AsyncUART *cb); void handle_ctrlIngressMsg(pb_istream_t &stream, AsyncUART *cb); void handle_configIngressMsg(pb_istream_t &stream, AsyncUART *cb); - bool decode(pb_istream_t &stream, const pb_msgdesc_t *fields, void *dest) const; - bool encode(Message &msg, AsyncUART *cb); - - template void ipcPush(Msg &msg, const pb_msgdesc_t &fields, int id); + void decode(pb_istream_t &stream, const pb_msgdesc_t &fields, void *dest) const; + bool transmit(Message &msg, AsyncUART *cb); + + void ipcPush(Message &message, int id, std::optional requestId); + void ipcPush(void*submessage, const pb_msgdesc_t &fields, int id, std::optional requestId); void putActiveRequest(int type, int id, AsyncUART *cb); AsyncUART *takeActiveRequest(int type, int id); diff --git a/rims_app/src/phase_modulation.cpp b/rims_app/src/phase_modulation.cpp index 956a881b035..006b4980e52 100644 --- a/rims_app/src/phase_modulation.cpp +++ b/rims_app/src/phase_modulation.cpp @@ -1,7 +1,7 @@ #include "phase_modulation.hpp" #include "circular_buffer.hpp" -#include "ctrl.pb.h" +#include "proto/ctrl.pb.h" #include "log.hpp" #include "pb_encode.h" #include "zephyr.hpp" diff --git a/rims_app/src/phase_modulation.hpp b/rims_app/src/phase_modulation.hpp index 12506fc926c..6fcd8df47da 100644 --- a/rims_app/src/phase_modulation.hpp +++ b/rims_app/src/phase_modulation.hpp @@ -4,7 +4,7 @@ #include "circular_buffer.hpp" #include "placement_unique_ptr.hpp" #include "common.hpp" -#include "ctrl.pb.h" +#include "proto/ctrl.pb.h" #include #include diff --git a/rims_app/src/temperature_measurements.cpp b/rims_app/src/temperature_measurements.cpp index c9e1af4ea99..d1ccbed4ce9 100644 --- a/rims_app/src/temperature_measurements.cpp +++ b/rims_app/src/temperature_measurements.cpp @@ -10,7 +10,7 @@ #include "common.hpp" #include "log.hpp" -#include "temperature.pb.h" +#include "proto/temperature.pb.h" #include #include diff --git a/rims_app/src/temperature_measurements.hpp b/rims_app/src/temperature_measurements.hpp index c4d00de66d0..c1c148bf33e 100644 --- a/rims_app/src/temperature_measurements.hpp +++ b/rims_app/src/temperature_measurements.hpp @@ -3,7 +3,7 @@ #include "circular_buffer.hpp" #include "common.hpp" -#include "temperature.pb.h" +#include "proto/temperature.pb.h" #include "zephyr.hpp" #include diff --git a/rims_app/src/uart.cpp b/rims_app/src/uart.cpp index c1b614fa4ea..903cb8ce27a 100644 --- a/rims_app/src/uart.cpp +++ b/rims_app/src/uart.cpp @@ -80,14 +80,14 @@ void AsyncUART::loop() { } // free function, only need to copy data to uart's TX_BUFFER and that's it -void AsyncUART::transmit(AsyncUART *dev, std::span bytes) { +void AsyncUART::transmit(AsyncUART *dev, std::span bytes) { if (bytes.empty()) return; bool first = true; for (auto byte : bytes) { if (first) { if (dev->tx_buffer.empty()) { - dev->tx_buffer.emplace_front((uint8_t)byte); + dev->tx_buffer.emplace_front(byte); uart_irq_tx_enable(dev->_dev); // enable interrupt continue; } @@ -97,7 +97,7 @@ void AsyncUART::transmit(AsyncUART *dev, std::span bytes) { while (dev->tx_buffer.full()) { std::this_thread::sleep_for(std::chrono::microseconds{12}); }; - dev->tx_buffer.emplace_front((uint8_t)byte); + dev->tx_buffer.emplace_front(byte); } } @@ -160,7 +160,7 @@ uint8_t AsyncUART::rxByte() { } void AsyncUART::processMessage() { - buffer buf{.data = reinterpret_cast(rxBuffer().data()), .size = rxBuffer().size(), .device = this}; + buffer buf{.data = {rxBuffer().data(), rxBuffer().size()}, .device = this}; switchRxBuffer(); k_msgq_put(&messenger_buffer_arrived_queue, &buf, K_MSEC(10)); } diff --git a/rims_app/src/uart.hpp b/rims_app/src/uart.hpp index 8da04004cf1..7fa32c04cfb 100644 --- a/rims_app/src/uart.hpp +++ b/rims_app/src/uart.hpp @@ -24,8 +24,8 @@ class AsyncUART { public: AsyncUART(); void loop(); - - static void transmit(AsyncUART *dev, std::span bytes); + + static void transmit(AsyncUART *dev, std::span bytes); static void workHandler(k_work *work); static void uartCallback(const struct device *dev, void *user_data); -- 2.45.2 From 4073332db9dcde35e12fa17f4f296a05c8058646 Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Thu, 6 Mar 2025 11:21:22 +0100 Subject: [PATCH 02/18] Add exception handling --- rims_app/CMakeLists.txt | 4 +--- rims_app/CMakeLists.txt.user | 32 +++++++++++++++++++++++++++----- rims_app/prj.conf | 32 ++++++++++++++++---------------- rims_app/src/main.cpp | 22 +++++----------------- 4 files changed, 49 insertions(+), 41 deletions(-) diff --git a/rims_app/CMakeLists.txt b/rims_app/CMakeLists.txt index 8f312b837fa..7e482c9cbaa 100644 --- a/rims_app/CMakeLists.txt +++ b/rims_app/CMakeLists.txt @@ -5,13 +5,11 @@ project(rims_controller C CXX) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) list(APPEND CMAKE_MODULE_PATH ${ZEPHYR_BASE}/modules/nanopb) - include(nanopb) -FILE(GLOB app_sources src/*.cpp src/*.c src/*.hpp src/*.h) +FILE(GLOB app_sources src/main.cpp src/*.cpp src/*.c src/*.hpp src/*.h) FILE(GLOB app_protos proto/*.proto) target_sources(app PRIVATE ${app_sources} ${app_protos}) -target_compile_options(app PUBLIC -fexceptions) zephyr_nanopb_sources(app ${app_protos}) diff --git a/rims_app/CMakeLists.txt.user b/rims_app/CMakeLists.txt.user index 6b065675621..b536f36898d 100644 --- a/rims_app/CMakeLists.txt.user +++ b/rims_app/CMakeLists.txt.user @@ -1,6 +1,6 @@ - + EnvironmentId @@ -219,14 +219,36 @@ false -e cpu-cycles --call-graph dwarf,4096 -F 250 - - ProjectExplorer.CustomExecutableRunConfiguration - + zephyr_final + CMakeProjectManager.CMakeRunConfiguration. + zephyr_final false true + true true + /home/bartoszek/zephyrproject/zephyr/rims_app/build/zephyr - 1 + + true + true + 0 + true + + + 2 + + false + -e cpu-cycles --call-graph dwarf,4096 -F 250 + zephyr_pre0 + CMakeProjectManager.CMakeRunConfiguration. + zephyr_pre0 + false + true + true + true + /home/bartoszek/zephyrproject/zephyr/rims_app/build/zephyr + + 2 diff --git a/rims_app/prj.conf b/rims_app/prj.conf index 9825a81ff62..dce83b24e2c 100644 --- a/rims_app/prj.conf +++ b/rims_app/prj.conf @@ -4,11 +4,11 @@ CONFIG_GEN_ISR_TABLES=y CONFIG_GEN_IRQ_VECTOR_TABLE=y # kernel configuration -#CONFIG_EVENTS=y -#CONFIG_POLL=y +CONFIG_EVENTS=y +CONFIG_POLL=y CONFIG_POSIX_API=y CONFIG_POSIX_TIMERS=y -CONFIG_TIMER_READS_ITS_FREQUENCY_AT_RUNTIME=y +# CONFIG_TIMER_READS_ITS_FREQUENCY_AT_RUNTIME=y #CONFIG_USERSPACE=y #CONFIG_HW_STACK_PROTECTION=y @@ -22,34 +22,34 @@ CONFIG_CPP_EXCEPTIONS=y CONFIG_CPP_RTTI=n #CONFIG_COMMON_LIBC_MALLOC_ARENA_SIZE=512 CONFIG_REQUIRES_FULL_LIBCPP=y +#CONFIG_NEWLIB_LIBC=y CONFIG_NEWLIB_LIBC_NANO=n - +# CONFIG_THREAD_ANALYZER=y CONFIG_THREAD_NAME=y - +# CONFIG_CONSOLE=n CONFIG_UART_CONSOLE=n CONFIG_EARLY_CONSOLE=n CONFIG_BOOT_BANNER=n - +# CONFIG_SERIAL=y # not working as expected for TX -CONFIG_UART_ASYNC_API=n -CONFIG_DMA=n - +#CONFIG_UART_ASYNC_API=n +# CONFIG_DMA=n +# CONFIG_UART_INTERRUPT_DRIVEN=y - +# CONFIG_ADC=y -#CONFIG_LOG=n -#CONFIG_LOG_MODE_IMMEDIATE=n # Log messages are output immediately -#CONFIG_LOG_BACKEND_UART=n # Use UART for log output - -#CONFIG_MAIN_STACK_SIZE=512 +CONFIG_LOG=n +CONFIG_LOG_MODE_IMMEDIATE=n # Log messages are output immediately +CONFIG_LOG_BACKEND_UART=n # Use UART for log output +CONFIG_MAIN_STACK_SIZE=1500 +# CONFIG_CRC=y - CONFIG_ASSERT=n #CONFIG_NUM_PREEMPT_PRIORITIES=0 diff --git a/rims_app/src/main.cpp b/rims_app/src/main.cpp index 1b58bfa525f..08134806334 100644 --- a/rims_app/src/main.cpp +++ b/rims_app/src/main.cpp @@ -7,12 +7,9 @@ #include "uart.hpp" #include "zero_cross_detection.hpp" -#include -#include #include #include #include -#include #include #include @@ -42,10 +39,10 @@ TStack uartStack{k_uartStack, K_THREAD_STACK_SIZEOF(k_uartStack)}; static K_THREAD_STACK_DEFINE(k_temperatureSamplerStack, 2048); TStack temperatureSamplerStack{k_temperatureSamplerStack, K_THREAD_STACK_SIZEOF(k_temperatureSamplerStack)}; -static K_THREAD_STACK_DEFINE(k_zeroCrossDetectionStack, 1500); +static K_THREAD_STACK_DEFINE(k_zeroCrossDetectionStack, 2048); TStack zeroCrossDetectionStack{k_zeroCrossDetectionStack, K_THREAD_STACK_SIZEOF(k_zeroCrossDetectionStack)}; -static K_THREAD_STACK_DEFINE(k_phaseModulationStack, 1500); +static K_THREAD_STACK_DEFINE(k_phaseModulationStack, 2048); TStack phaseModulationStack{k_phaseModulationStack, K_THREAD_STACK_SIZEOF(k_phaseModulationStack)}; /// TOP LEVEL TODOS ;) @@ -79,15 +76,6 @@ LazyInit zcd; LazyInit phaseModulation; struct except : std::exception {}; -// extern "C"{ - -// void * _sbrk(int p_amount){ -// static char b[1024]; -// return b; -// } - -// } - int main() { using namespace rims; @@ -113,12 +101,12 @@ int main() { phaseModulation->start(); while (1) { - std::this_thread::sleep_for(std::chrono::seconds{2}); + // std::this_thread::sleep_for(std::chrono::seconds{2}); + try { throw except{}; } catch (const except &e) { - - ULOG_ERROR("got exception"); + while(1){} } } -- 2.45.2 From a634cf5cfe222a39ca1081cd2661f1db93b854a1 Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Mon, 17 Mar 2025 15:10:58 +0100 Subject: [PATCH 03/18] refactor --- rims_app/CMakeLists.txt.user | 2 +- rims_app/proto/ctrl.proto | 148 ++++++-- rims_app/proto/temperature.proto | 21 +- rims_app/src/circular_buffer.hpp | 22 +- rims_app/src/common.hpp | 4 +- rims_app/src/log.cpp | 1 + rims_app/src/log.hpp | 2 +- rims_app/src/main.cpp | 16 +- .../src/{message_decode.cpp => messenger.cpp} | 221 ++++++------ .../src/{message_decode.hpp => messenger.hpp} | 13 +- rims_app/src/phase_modulation.cpp | 332 +++++++++++++++--- rims_app/src/phase_modulation.hpp | 91 +++-- rims_app/src/temperature_measurements.cpp | 121 ++++--- rims_app/src/temperature_measurements.hpp | 4 +- rims_app/src/uart.cpp | 2 +- rims_app/src/zephyr.hpp | 21 ++ 16 files changed, 740 insertions(+), 281 deletions(-) rename rims_app/src/{message_decode.cpp => messenger.cpp} (85%) rename rims_app/src/{message_decode.hpp => messenger.hpp} (74%) diff --git a/rims_app/CMakeLists.txt.user b/rims_app/CMakeLists.txt.user index b536f36898d..4e9a5f03d5a 100644 --- a/rims_app/CMakeLists.txt.user +++ b/rims_app/CMakeLists.txt.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/rims_app/proto/ctrl.proto b/rims_app/proto/ctrl.proto index 950b9e12932..05e70e747d6 100644 --- a/rims_app/proto/ctrl.proto +++ b/rims_app/proto/ctrl.proto @@ -7,43 +7,127 @@ enum Error { NoError = 0; WrongMode = 1; - WrongChannel = 2; + UnsupportedMode = 2; + + WrongChannel = 3; + + UnknownError = 32; } -message MaxPower -{ +// algorithm used to control the heating element on given channel +enum ModeOfOperation { + Disabled = 0; + + // GroupModulation mode adjusts power by delivering complete AC sine wave cycles in + // groups. It delivers N full cycles of the waveform followed by M cycles of zero + // voltage (off state). Example: For 50% power, it may deliver 5 cycles on followed + // by 5 cycles off. + GroupModulation = 1; + + // PhaseModulation mode adjusts power by controlling the phase angle of the AC waveform. + // By cutting off a portion of each sine wave cycle, it achieves finer control of the power + // output. Example: Setting a phase angle of 90 degrees allows only half of the waveform to + // be applied. + PhaseModulation = 2; + + // ConstantTemperatureModulation adjusts power based on a feedback loop maintaining a set + // temperature. The system monitors the temperature and modulates power accordingly to keep + // it constant. This can involve dynamically switching between modes or adjusting power + // level. + ConstantTemperatureModulation = 3; + + // TemperatureSlopeModulation controls the heating element to achieve a specified + // temperature change rate. Example: Heating up at a rate of 1°C per minute by adjusting + // power output dynamically. This mode is useful when a precise temperature ramp is required + // over time. + TemperatureSlopeModulation = 4; } -// Parameters for Phase Modulation -message PhaseModulationParams +message ModeOfOperationRequest { - /**/ + uint32 channel_id = 1; + optional ModeOfOperation mode = 2; // null indicates a reqd of current mode } -// Parameters for Group Modulation -message GroupModulationParams +message ModeOfOperationResponse { - uint32 cycles = 1; // Duration of each group in milliseconds (optional) + uint32 channel_id = 1; + ModeOfOperation mode = 2; + optional Error error = 254; } -message ManualPowerControlRequest +// Request message for configuring Group Modulation parameters. +message GroupModulationConfigRequest { - uint32 channelID = 1; // required - oneof modulation - { - PhaseModulationParams phase = 2; - GroupModulationParams group = 3; - } + // The channel ID to which the request is directed. + uint32 channel_id = 1; + + // The maximum number of ON cycles for Group Modulation. If + // absent, it indicates a read request. + optional uint32 cycles_max = 2; } -message ManualPowerControlResponse +// Response message for returning the Group Modulation configuration. +message GroupModulationConfigResponse { - oneof modulation - { - PhaseModulationParams phase = 2; - GroupModulationParams group = 3; - Error error = 254; - } + // The channel ID associated with the response. + uint32 channel_id = 1; + + // The current setting of maximum ON cycles for Group Modulation. + uint32 cycles_max = 2; + + // Optional error field. If present, indicates an error occurred. + optional Error error = 254; +} + +message GroupModulationControlRequest +{ + // The channel ID to which the request is directed. + uint32 channel_id = 1; + + // Number of cycles ON for GroupModulation. + optional uint32 cycles = 2; +} + +message GroupModulationControlResponse +{ + // The channel ID associated with the response. + uint32 channel_id = 1; + + uint32 cycles = 2; + + optional Error error = 254; +} + +message PhaseModulationConfigRequest +{ + // The channel ID to which the request is directed. + uint32 channel_id = 1; +} + +message PhaseModulationConfigResponse +{ + // The channel ID associated with the response. + uint32 channel_id = 1; + + optional Error error = 254; +} + +message PhaseModulationControlRequest +{ + // The channel ID to which the request is directed. + uint32 channel_id = 1; + + // Phase angle for PhaseModulation (0 to 180 degrees). + optional float phase_angle = 2; +} + +message PhaseModulationControlResponse +{ + // The channel ID associated with the response. + uint32 channel_id = 1; + float phase_angle = 2; + optional Error error = 254; } // only those messages are send through interface @@ -52,7 +136,13 @@ message IngressMessages uint32 requestID = 255; // Unique request ID oneof data { - ManualPowerControlRequest manualPowerControlRequest = 1; + ModeOfOperationRequest modeOfOperationRequest = 1; + + GroupModulationConfigRequest groupModulationConfigRequest = 2; + GroupModulationControlRequest groupModulationControlRequest = 3; + + PhaseModulationConfigRequest phaseModulationConfigRequest = 4; + PhaseModulationControlRequest phaseModulationControlRequest = 5; } }; @@ -61,7 +151,13 @@ message EgressMessages optional uint32 requestID = 255; // Unique request ID oneof data { - ManualPowerControlResponse manualPowerControlResponse = 1; - // also broadcast + ModeOfOperationResponse modeOfOperationResponse = 1; + + GroupModulationConfigResponse groupModulationConfigResponse = 2; + GroupModulationControlResponse groupModulationControlResponse = 3; + + PhaseModulationConfigResponse phaseModulationConfigResponse = 4; + PhaseModulationControlResponse phaseModulationControlResponse = 5; + // BROADCAST } }; diff --git a/rims_app/proto/temperature.proto b/rims_app/proto/temperature.proto index f67d9abbc8d..14a0a81cc41 100644 --- a/rims_app/proto/temperature.proto +++ b/rims_app/proto/temperature.proto @@ -21,28 +21,24 @@ message SamplerConfig // Last temp sample message TemperatureCurrent { - uint32 channel_id = 1; - // last update - float temp_c = 2; + float temp_c = 1; } // Statictics of temperature on given channel message TemperatureStatistics { - uint32 channel_id = 1; - // last update - float temp_c = 2; + float temp_c = 1; // avarage from last nsamples - float temp_avg_c = 3; + float temp_avg_c = 2; // standard deviation of temperature on given channel from last nsamples - float temp_stddev_c = 4; + float temp_stddev_c = 3; // samples taken - uint32 n_samples = 5; + uint32 n_samples = 4; } /* CONFIG API */ @@ -55,6 +51,7 @@ message SamplerConfigRequest message SamplerConfigResponse { + uint32 channel_id = 1; oneof data { SamplerConfig config = 2; @@ -70,9 +67,10 @@ message GetCurrentTemperatureRequest message GetCurrentTemperatureResponse { + uint32 channel_id = 1; oneof data { - TemperatureCurrent temperatureCurrent = 1; + TemperatureCurrent temperatureCurrent = 2; Error error = 254; } } @@ -84,9 +82,10 @@ message GetTemperatureRequest message GetTemperatureResponse { + uint32 channel_id = 1; oneof data { - TemperatureStatistics temperatureStats = 1; + TemperatureStatistics temperatureStats = 2; Error error = 254; } } diff --git a/rims_app/src/circular_buffer.hpp b/rims_app/src/circular_buffer.hpp index ddaf8f6b06f..e71daf20845 100644 --- a/rims_app/src/circular_buffer.hpp +++ b/rims_app/src/circular_buffer.hpp @@ -376,9 +376,14 @@ template class zephyr_fifo_buffer { /// read from queue ZephyrFifoElement *el = reinterpret_cast *>(k_fifo_get(&_fifo, K_NO_WAIT)); - if (not el) return false; // should be a assert - fn(el->item); // consume item - _elements.pop_back(); // clear item + try { + if (not el) return false; // should be a assert + fn(el->item); // consume item, fn can throw + _elements.pop_back(); // clear item from queue + } catch (...) { + _elements.pop_back(); + throw; + } return true; }; @@ -388,10 +393,13 @@ template class zephyr_fifo_buffer { // std::lock_guard _lock{_mutex}; if (_elements.full()) return false; - auto &el = _elements.emplace_front(ZephyrFifoElement{}); // create new element - if (fn(el.item)) // fill new data - k_fifo_put(&_fifo, &el); // put data into a queue - else _elements.pop_front(); // cleanup + auto tmp = ZephyrFifoElement{}; + if (fn(tmp.item)) // fill new data + { + auto &el = _elements.emplace_front(std::move(tmp)); + k_fifo_put(&_fifo, &el); // put data into a queue + } + return true; } diff --git a/rims_app/src/common.hpp b/rims_app/src/common.hpp index b7c0b2bb5df..39c5934f31a 100644 --- a/rims_app/src/common.hpp +++ b/rims_app/src/common.hpp @@ -52,7 +52,9 @@ class Timer { k_timer_init(&timer_, &Timer::expiryHandler, &Timer::stopHandler); k_timer_user_data_set(&timer_, this); } - virtual ~Timer() = default; + virtual ~Timer() { + stop(); + }; void start() { k_timer_start(&timer_, _interval, _period); diff --git a/rims_app/src/log.cpp b/rims_app/src/log.cpp index c05b31f1ed0..bbe10001e82 100644 --- a/rims_app/src/log.cpp +++ b/rims_app/src/log.cpp @@ -72,6 +72,7 @@ void Log::formatLog(Level level, const char *fmt, va_list args) const { logmsg.has_requestID = false; logmsg.which_data = log_EgressMessages_logEntry_tag; + logmsg.data.logEntry.systick = k_uptime_ticks(); logmsg.data.logEntry.level = toPbLogLevel(level); logmsg.data.logEntry.lineNumber = this->_sl.line(); diff --git a/rims_app/src/log.hpp b/rims_app/src/log.hpp index c460cf4fdc9..d1fa996281b 100644 --- a/rims_app/src/log.hpp +++ b/rims_app/src/log.hpp @@ -23,7 +23,7 @@ extern zephyr_fifo_buffer logIngressFifoQueueBuffer; extern zephyr_fifo_buffer logEgressFifoQueueBuffer; /// TODO move -static LogLevel g_logLevel = LogLevel::Debug; +static LogLevel g_logLevel = LogLevel::Critical; class Log { public: diff --git a/rims_app/src/main.cpp b/rims_app/src/main.cpp index 08134806334..75c1d942025 100644 --- a/rims_app/src/main.cpp +++ b/rims_app/src/main.cpp @@ -1,8 +1,9 @@ #include "common.hpp" #include "log.hpp" -#include "message_decode.hpp" +#include "messenger.hpp" #include "phase_modulation.hpp" +#include "placement_unique_ptr.hpp" #include "temperature_measurements.hpp" #include "uart.hpp" #include "zero_cross_detection.hpp" @@ -11,6 +12,7 @@ #include #include +#include #include #include #include @@ -101,13 +103,13 @@ int main() { phaseModulation->start(); while (1) { - // std::this_thread::sleep_for(std::chrono::seconds{2}); + std::this_thread::sleep_for(std::chrono::seconds{2}); - try { - throw except{}; - } catch (const except &e) { - while(1){} - } + // try { + // throw except{}; + // } catch (const except &e) { + // while(1){} + // } } // auto getStats = [](auto &diffs) { diff --git a/rims_app/src/message_decode.cpp b/rims_app/src/messenger.cpp similarity index 85% rename from rims_app/src/message_decode.cpp rename to rims_app/src/messenger.cpp index 61cc374f643..b0fd7f364e6 100644 --- a/rims_app/src/message_decode.cpp +++ b/rims_app/src/messenger.cpp @@ -1,4 +1,4 @@ -#include "message_decode.hpp" +#include "messenger.hpp" #include "circular_buffer.hpp" #include "log.hpp" @@ -75,94 +75,6 @@ static bool crcIsInvalid(std::span data, std::uint32_t crc) { // uint32_t checksum = (data ^ (data >> 3) ^ (data >> 6) ^ (data >> 9) ^ (data >> 12)) & 0x7; // Reduce to 3 bits // return (data << 3) | checksum; // Store checksum in the last 3 bits // } - -namespace messenger { -struct error : std::exception { - Error _err; - error(Error e) : _err{e} { - } -}; - -struct no_id : public error { - no_id() : error{Error_NoId} {}; -}; -struct bad_id : public error { - bad_id() : error{Error_BadId} {}; -}; - -struct no_crc : public error { - no_crc() : error{Error_NoCRC} {}; -}; -struct bad_crc : public error { - bad_crc() : error{Error_BadCRC} {}; -}; - -struct no_data : public error { - no_data() : error{Error_NoData} {}; -}; -struct bad_data : public error { - bad_data() : error{Error_BadData} {}; -}; - -struct request_queue_full : public error { - request_queue_full() : error{Error_RequestQueueFull} { - } -}; - -} // namespace messenger - -MessengerThread::MessengerThread(TStackBase &stack) : ZephyrThread{stack, 10, 0, "Messenger"} { - zephyr::event_pool::k_init(_events.at(0), messenger_buffer_arrived_queue); - - for (auto &ar : _activeRequests) { - ar.type = -1; - ar.cb = nullptr; - ar.requestId = 0; - } -} - -void rims::MessengerThread::event_temperatureEgress() { - temperatureEgressQueue.try_consume([&](temperature_EgressMessages &in) { - ipcPush(&in, temperature_EgressMessages_msg, 1, in.has_requestID ? std::optional{in.requestID} : std::nullopt); - }); -} - -void rims::MessengerThread::event_ctrlEgress() { - ctrlEgressQueue.try_consume([&](ctrl_EgressMessages &in) { - ipcPush(&in, ctrl_EgressMessages_msg, 2, in.has_requestID ? std::optional{in.requestID} : std::nullopt); - }); -} - -void rims::MessengerThread::event_configEgress() { - configEgressQueue.try_consume([&](config_EgressMessages &in) { - ipcPush(&in, config_EgressMessages_msg, 3, in.has_requestID ? std::optional{in.requestID} : std::nullopt); - }); -} - -void rims::MessengerThread::event_logEgress() { - logEgressFifoQueueBuffer.try_consume([&](log_EgressMessages &in) { - ipcPush(&in, log_EgressMessages_msg, 4, in.has_requestID ? std::optional{in.requestID} : std::nullopt); - }); -} - -void MessengerThread::threadMain() { - temperatureEgressQueue.k_event_init(_events.at(1)); - ctrlEgressQueue.k_event_init(_events.at(2)); - configEgressQueue.k_event_init(_events.at(3)); - logEgressFifoQueueBuffer.k_event_init(_events.at(4)); - - while (1) { // TODO add try catch for incoming data errors - auto ret = zephyr::event_pool::k_poll_forever(_events); - if (ret == 0) { - zephyr::event_pool::k_poll_handle(_events[0], std::bind(&MessengerThread::event_dataArrived, this)); - zephyr::event_pool::k_poll_handle(_events[1], std::bind(&MessengerThread::event_temperatureEgress, this)); - zephyr::event_pool::k_poll_handle(_events[2], std::bind(&MessengerThread::event_ctrlEgress, this)); - zephyr::event_pool::k_poll_handle(_events[3], std::bind(&MessengerThread::event_configEgress, this)); - zephyr::event_pool::k_poll_handle(_events[4], std::bind(&MessengerThread::event_logEgress, this)); - } - } -} - namespace cobs { class error : public std::exception {}; @@ -173,6 +85,7 @@ class decode_error : public error { decode_error(cobs_ret_t r) : ret{r} { } }; + class encode_error : public error { public: cobs_ret_t ret; @@ -265,6 +178,97 @@ inline void encode(pb_ostream_t &stream, const pb_msgdesc_t &fields, const void } // namespace pb +namespace messenger { +struct error : std::exception { + Error _err; + error(Error e) : _err{e} { + } +}; + +struct no_id : public error { + no_id() : error{Error_NoId} {}; +}; +struct bad_id : public error { + bad_id() : error{Error_BadId} {}; +}; + +struct no_crc : public error { + no_crc() : error{Error_NoCRC} {}; +}; +struct bad_crc : public error { + bad_crc() : error{Error_BadCRC} {}; +}; + +struct no_data : public error { + no_data() : error{Error_NoData} {}; +}; +struct bad_data : public error { + bad_data() : error{Error_BadData} {}; +}; + +struct request_queue_full : public error { + request_queue_full() : error{Error_RequestQueueFull} { + } +}; + +} // namespace messenger + +MessengerThread::MessengerThread(TStackBase &stack) : ZephyrThread{stack, 10, 0, "Messenger"} { + zephyr::event_pool::k_init(_events.at(0), messenger_buffer_arrived_queue); + + for (auto &ar : _activeRequests) { + ar.type = -1; + ar.cb = nullptr; + ar.requestId = 0; + } +} + +void MessengerThread::threadMain() { + temperatureEgressQueue.k_event_init(_events.at(1)); + ctrlEgressQueue.k_event_init(_events.at(2)); + configEgressQueue.k_event_init(_events.at(3)); + logEgressFifoQueueBuffer.k_event_init(_events.at(4)); + + while (1) { // TODO add try catch for incoming data errors + try { + auto ret = zephyr::event_pool::k_poll_forever(_events); + if (ret == 0) { + zephyr::event_pool::k_poll_handle(_events[0], std::bind(&MessengerThread::event_dataArrived, this)); + zephyr::event_pool::k_poll_handle(_events[1], std::bind(&MessengerThread::event_temperatureEgress, this)); + zephyr::event_pool::k_poll_handle(_events[2], std::bind(&MessengerThread::event_ctrlEgress, this)); + zephyr::event_pool::k_poll_handle(_events[3], std::bind(&MessengerThread::event_configEgress, this)); + zephyr::event_pool::k_poll_handle(_events[4], std::bind(&MessengerThread::event_logEgress, this)); + } + } catch (const std::exception &e) { + /// TODO + } + } +} + +void rims::MessengerThread::event_temperatureEgress() { + temperatureEgressQueue.try_consume([&](temperature_EgressMessages &in) { + egressPussh(&in, temperature_EgressMessages_msg, 1, in.has_requestID ? std::optional{in.requestID} : std::nullopt); + }); +} + +void rims::MessengerThread::event_ctrlEgress() { + ctrlEgressQueue.try_consume([&](ctrl_EgressMessages &in) { + egressPussh(&in, ctrl_EgressMessages_msg, 2, in.has_requestID ? std::optional{in.requestID} : std::nullopt); + }); +} + +void rims::MessengerThread::event_configEgress() { + configEgressQueue.try_consume([&](config_EgressMessages &in) { + egressPussh(&in, config_EgressMessages_msg, 3, in.has_requestID ? std::optional{in.requestID} : std::nullopt); + }); +} + +void rims::MessengerThread::event_logEgress() { + logEgressFifoQueueBuffer.try_consume([&](log_EgressMessages &in) { + egressPussh(&in, log_EgressMessages_msg, 4, in.has_requestID ? std::optional{in.requestID} : std::nullopt); + }); +} + void MessengerThread::event_dataArrived() { try { buffer buf; @@ -357,17 +361,20 @@ void MessengerThread::event_dataArrived() { } } } catch (const messenger::error &e) { - Message message = Message_init_zero; - message.error = e._err; - ipcPush(message, 16, std::nullopt); + Message message = Message_init_zero; + message.error = e._err; + message.has_error = true; + egressPush(message, 16, std::nullopt); } catch (const cobs::error *e) { - Message message = Message_init_zero; - message.error = Error_UnknownId; - ipcPush(message, 16, std::nullopt); + Message message = Message_init_zero; + message.error = Error_UnknownId; + message.has_error = true; + egressPush(message, 16, std::nullopt); } catch (const std::exception &e) { - Message message = Message_init_zero; - message.error = Error_UnknownId; - ipcPush(message, 16, std::nullopt); + Message message = Message_init_zero; + message.error = Error_UnknownId; + message.has_error = true; + egressPush(message, 16, std::nullopt); } } @@ -398,14 +405,14 @@ void MessengerThread::handle_configIngressMsg(pb_istream_t &stream, AsyncUART *c configIngressQueue.try_consume([&](config_IngressMessages &request) { request = config_IngressMessages_init_zero; decode(stream, config_IngressMessages_msg, &request); - putActiveRequest(2, request.requestID, cb); + putActiveRequest(3, request.requestID, cb); return true; }); } -void MessengerThread::ipcPush(Message &message, int id, std::optional requestId) { +void MessengerThread::egressPush(Message &message, int id, std::optional requestId) { // set IF od sender - message.id = id; /// TODO make response ID + message.id = id; /// TODO make response ID message.crc = zephyr::crc::crc32_ieee({message.data.bytes, message.data.size}); // for requests, we have to have a callback function to return from where we received message @@ -420,15 +427,15 @@ void MessengerThread::ipcPush(Message &message, int id, std::optional } } -void MessengerThread::ipcPush(void *submessage, const pb_msgdesc_t &fields, int id, std::optional requestId) { +void MessengerThread::egressPussh(void *submessage, const pb_msgdesc_t &fields, int id, std::optional requestId) { // encode embedded message directly to the buffer of output message to save stack Message message = Message_init_zero; auto ostream = pb::ostream_from_span(std::span{message.data.bytes}); // TODO max data size - pb::encode(ostream, fields, submessage); + pb::encode(ostream, fields, submessage); message.data.size = ostream.bytes_written; - - ipcPush(message, id, requestId); + + egressPush(message, id, requestId); } void MessengerThread::decode(pb_istream_t &stream, const pb_msgdesc_t &fields, void *dest) const { @@ -455,7 +462,7 @@ bool MessengerThread::transmit(Message &msg, AsyncUART *cb) { return true; } -void MessengerThread::putActiveRequest(int type, int id, AsyncUART *cb) { +void MessengerThread::putActiveRequest(int type, uint32_t id, AsyncUART *cb) { for (auto &req : _activeRequests) { if (req.type == -1) { // Find an empty slot req.type = type; @@ -467,7 +474,7 @@ void MessengerThread::putActiveRequest(int type, int id, AsyncUART *cb) { throw messenger::request_queue_full{}; } -AsyncUART *MessengerThread::takeActiveRequest(int type, int id) { +AsyncUART *MessengerThread::takeActiveRequest(int type, uint32_t id) { for (auto &req : _activeRequests) { if (req.type == type && req.requestId == id) { req.type = -1; // Mark as available diff --git a/rims_app/src/message_decode.hpp b/rims_app/src/messenger.hpp similarity index 74% rename from rims_app/src/message_decode.hpp rename to rims_app/src/messenger.hpp index a93ca679350..31fe0e938be 100644 --- a/rims_app/src/message_decode.hpp +++ b/rims_app/src/messenger.hpp @@ -31,7 +31,7 @@ class MessengerThread : public ZephyrThread { private: struct requestData { int type; - int requestId; + uint32_t requestId; AsyncUART *cb; }; @@ -47,14 +47,15 @@ class MessengerThread : public ZephyrThread { void decode(pb_istream_t &stream, const pb_msgdesc_t &fields, void *dest) const; bool transmit(Message &msg, AsyncUART *cb); - - void ipcPush(Message &message, int id, std::optional requestId); - void ipcPush(void*submessage, const pb_msgdesc_t &fields, int id, std::optional requestId); - void putActiveRequest(int type, int id, AsyncUART *cb); - AsyncUART *takeActiveRequest(int type, int id); + void egressPush(Message &message, int id, std::optional requestId); + void egressPussh(void *submessage, const pb_msgdesc_t &fields, int id, std::optional requestId); + + void putActiveRequest(int type, uint32_t id, AsyncUART *cb); + AsyncUART *takeActiveRequest(int type, uint32_t id); std::array _events; std::array _activeRequests; + uint8_t _activeRequestsNr; }; } // namespace rims diff --git a/rims_app/src/phase_modulation.cpp b/rims_app/src/phase_modulation.cpp index 006b4980e52..fa4e105d127 100644 --- a/rims_app/src/phase_modulation.cpp +++ b/rims_app/src/phase_modulation.cpp @@ -1,20 +1,65 @@ #include "phase_modulation.hpp" #include "circular_buffer.hpp" -#include "proto/ctrl.pb.h" #include "log.hpp" -#include "pb_encode.h" +#include "messenger.hpp" +#include "placement_unique_ptr.hpp" +#include "proto/ctrl.pb.h" #include "zephyr.hpp" #include "zero_cross_detection.hpp" #include #include +#include +#include #include -#include -#include +#include + +// Primary template (for general case) +template struct NthArgument; + +// Specialization for normal functions +template struct NthArgument { + using type = std::tuple_element_t>; +}; + +// Specialization for member functions (non-const) +template struct NthArgument { + using type = std::tuple_element_t>; +}; + +// Specialization for member functions (const) +template struct NthArgument { + using type = std::tuple_element_t>; +}; + +// Helper alias template +template using NthArgument_t = typename NthArgument::type; namespace rims { +namespace ctrl { + +struct error : std::exception { + ctrl_Error _err; + error(ctrl_Error e) : _err{e} { + } +}; + +struct wrong_channel : public error { + wrong_channel() : error{ctrl_Error_WrongChannel} {}; +}; + +struct wrong_mode : public error { + wrong_mode() : error{ctrl_Error_WrongMode} {}; +}; + +struct unsuported_mode : public error { + unsuported_mode() : error{ctrl_Error_UnsupportedMode} {}; +}; + +} // namespace ctrl + K_FIFO_DEFINE(ctrlIngress); K_FIFO_DEFINE(ctrlEggress); @@ -36,18 +81,18 @@ PhaseControlBase::PhaseControlBase(const gpio_dt_spec &gpio) : _gpio{gpio} { ULOG_INFO("triac GPIO %s/%d", gpio.port->name, gpio.pin); } -void PhaseControlBase::action_triacEnable() const { - gpio_pin_set_dt(&_gpio, 1); +void PhaseControlBase::enable() const { + zephyr::gpio::pin_set_dt(_gpio, 1); } -void PhaseControlBase::action_triacDisable() const { - gpio_pin_set_dt(&_gpio, 0); +void PhaseControlBase::disable() const { + zephyr::gpio::pin_set_dt(_gpio, 0); } PhaseModulation::PhaseModulation(const gpio_dt_spec &gpio) : PhaseControlBase{gpio}, // - _triacTimer{[this]() { this->action_triacEnable(); }, std::chrono::milliseconds{0}}, - _off{[this]() { this->action_triacDisable(); }, std::chrono::milliseconds{0}} { + _triacTimerStart{[this]() { this->enable(); }, std::chrono::milliseconds{0}}, + _triacTimerStop{[this]() { this->disable(); }, std::chrono::milliseconds{0}} { ULOG_INFO("Started"); } @@ -58,28 +103,33 @@ void PhaseModulation::tickFallingEdge() { // power // auto fullCycleTime = std::chrono::milliseconds{10}; auto offset = (1000 - int(_power * 10)) * std::chrono::microseconds{10}; - _triacTimer.setInterval(offset); - _triacTimer.start(); - _off.setInterval(offset + std::chrono::microseconds{100}); - _off.start(); + _triacTimerStart.setInterval(offset); + _triacTimerStart.start(); + _triacTimerStop.setInterval(offset + std::chrono::microseconds{100}); + _triacTimerStop.start(); } -PhaseModulationOrchestrator::PhaseModulationOrchestrator() - : _channel{placement_unique(_buffer[0])(), placement_unique(_buffer[1])()} { - _channel[0] = placement_unique(_buffer[0])(pins[0]); - _channel[1] = placement_unique(_buffer[1])(pins[1]); +GroupModulation::GroupModulation(const gpio_dt_spec &gpio) : PhaseControlBase(gpio) { +} + +PhaseModulationOrchestrator::PhaseModulationOrchestrator() : _channel{Disabled{}, Disabled{}} { ZeroCrossDetectionEventQueue.k_event_init(_events[0]); ctrlIngressQueue.k_event_init(_events[1]); } +// thread main void PhaseModulationOrchestrator::loop() { /// TODO power limiter (after power measurement) while (1) { - auto ret = zephyr::event_pool::k_poll_forever(_events); - if (ret == 0) { - zephyr::event_pool::k_poll_handle(_events[0], [&]() { event_zeroCrossDetection(); }); - zephyr::event_pool::k_poll_handle(_events[1], [&]() { event_ctrlMessageArrived(); }); + try { + auto ret = zephyr::event_pool::k_poll_forever(_events); + if (ret == 0) { + zephyr::event_pool::k_poll_handle(_events[0], [&]() { event_zeroCrossDetection(); }); + zephyr::event_pool::k_poll_handle(_events[1], [&]() { event_ctrlMessageArrived(); }); + } + } catch (const std::exception &e) { + ULOG_ERROR("EXCEPTION %s", e.what()); } } } @@ -90,28 +140,26 @@ int PhaseModulationThread::do_hardwarenInit() { return 0; } -constexpr float PI = 3.141592f; -constexpr float FREQUENCY = 0.5f; // 2 Hz -constexpr float MIN_VALUE = 10.0f; -constexpr float MAX_VALUE = 25.0f; +// constexpr float PI = 3.141592f; +// constexpr float FREQUENCY = 0.5f; // 2 Hz +// constexpr float MIN_VALUE = 10.0f; +// constexpr float MAX_VALUE = 25.0f; -float sinewave(std::chrono::steady_clock::time_point timePoint) { - using namespace std::chrono; - static auto startTime = steady_clock::now(); +// float sinewave(std::chrono::steady_clock::time_point timePoint) { +// using namespace std::chrono; +// static auto startTime = steady_clock::now(); - float elapsedSeconds = duration(timePoint - startTime).count(); - float sineValue = sinf(2.0f * PI * FREQUENCY * elapsedSeconds); +// float elapsedSeconds = duration(timePoint - startTime).count(); +// float sineValue = sinf(2.0f * PI * FREQUENCY * elapsedSeconds); - // return std::sin(2.0 * PI * FREQUENCY * elapsedSeconds); - return MIN_VALUE + (sineValue + 1.0f) * 0.5f * (MAX_VALUE - MIN_VALUE); -} +// // return std::sin(2.0 * PI * FREQUENCY * elapsedSeconds); +// return MIN_VALUE + (sineValue + 1.0f) * 0.5f * (MAX_VALUE - MIN_VALUE); +// } void PhaseModulationOrchestrator::event_zeroCrossDetection() { - auto setPower = [](auto &channel) { channel->setPower(sinewave(std::chrono::steady_clock::now())); }; - auto tickFallingEdge = [](auto &channel) { channel->tickFallingEdge(); }; - auto tickRisingEdge = [](auto &channel) { channel->tickRisingEdge(); }; + auto tickFallingEdge = [](auto &channel) { channel.tickFallingEdge(); }; + auto tickRisingEdge = [](auto &channel) { channel.tickRisingEdge(); }; - std::visit(setPower, _channel[0]); /// TODO check proper channel ZeroCrossDetectionEventQueue.try_consume([&](ZeroCrossDetectionEvent &event) { if (event.state) { @@ -123,26 +171,208 @@ void PhaseModulationOrchestrator::event_zeroCrossDetection() { } void PhaseModulationOrchestrator::event_ctrlMessageArrived() { - ctrlIngressQueue.try_consume([&](const ctrl_IngressMessages &request) { + constexpr auto modeOfOperationRequestHandler = std::make_tuple( + ctrl_IngressMessages_modeOfOperationRequest_tag, + ctrl_EgressMessages_modeOfOperationResponse_tag, + &PhaseModulationOrchestrator::handler_modeOfOperationRequest, + ctrl_ModeOfOperationResponse ctrl_ModeOfOperationResponse_init_zero + ); + + constexpr auto groupModulationConfigRequestHandler = std::make_tuple( + ctrl_IngressMessages_groupModulationConfigRequest_tag, + ctrl_EgressMessages_groupModulationConfigResponse_tag, + &PhaseModulationOrchestrator::handler_groupModulationConfigRequest, + ctrl_GroupModulationConfigResponse ctrl_GroupModulationConfigResponse_init_zero + ); + + constexpr auto groupModulationControlRequestHandler = std::make_tuple( + ctrl_IngressMessages_groupModulationControlRequest_tag, + ctrl_EgressMessages_groupModulationControlResponse_tag, + &PhaseModulationOrchestrator::handler_groupModulationControlRequest, + ctrl_GroupModulationControlResponse ctrl_GroupModulationControlResponse_init_zero + ); + + constexpr auto phaseModulationConfigRequestHandler = std::make_tuple( + ctrl_IngressMessages_phaseModulationConfigRequest_tag, + ctrl_EgressMessages_phaseModulationConfigResponse_tag, + &PhaseModulationOrchestrator::handler_phaseModulationConfigRequest, + ctrl_PhaseModulationConfigResponse ctrl_PhaseModulationConfigResponse_init_zero + ); + + constexpr auto phaseModulationControlRequestHandler = std::make_tuple( + ctrl_IngressMessages_phaseModulationControlRequest_tag, + ctrl_EgressMessages_phaseModulationControlResponse_tag, + &PhaseModulationOrchestrator::handler_phaseModulationControlRequest, + ctrl_PhaseModulationControlResponse ctrl_PhaseModulationControlResponse_init_zero + ); + + auto genericHandler = [&](const auto &request, auto &response, auto handler) { + auto fn = std::get<2>(handler); + response.which_data = std::get<1>(handler); + + // cast request and response union to needed type + const auto &req = reinterpret_cast>(response.data); + auto &resp = reinterpret_cast>(response.data); + + try { + // invoke message handler + std::invoke(fn, this, req, resp); + } catch (const ctrl::error &e) { + // clear message + resp = std::get<3>(handler); + // set error on exception + resp.has_error = true; + resp.error = e._err; + } catch (const std::exception &e) { + resp = std::get<3>(handler); + resp.has_error = true; + resp.error = ctrl_Error_UnknownError; + } + }; + + ctrlIngressQueue.try_consume([&](const ctrl_IngressMessages &req) { ULOG_INFO("control request message handler"); - ctrlEgressQueue.try_produce([&](ctrl_EgressMessages &response) { + ctrlEgressQueue.try_produce([&](ctrl_EgressMessages &resp) { ULOG_INFO("control response message handler"); - switch (request.which_data) - case ctrl_IngressMessages_manualPowerControlRequest_tag: { - response.which_data = ctrl_EgressMessages_manualPowerControlResponse_tag; - handler_manualPowerControlResponse( // - request.data.manualPowerControlRequest, - response.data.manualPowerControlResponse - ); - response.requestID = request.requestID; + resp.requestID = req.requestID; + resp.has_requestID = true; + + switch (req.which_data) { + case std::get<1>(modeOfOperationRequestHandler): + genericHandler(req, resp, modeOfOperationRequestHandler); + break; + + case std::get<1>(groupModulationConfigRequestHandler): + genericHandler(req, resp, groupModulationConfigRequestHandler); + break; + case std::get<1>(groupModulationControlRequestHandler): + genericHandler(req, resp, groupModulationControlRequestHandler); + break; + + case std::get<1>(phaseModulationConfigRequestHandler): + genericHandler(req, resp, phaseModulationConfigRequestHandler); + break; + case std::get<1>(phaseModulationControlRequestHandler): + genericHandler(req, resp, phaseModulationControlRequestHandler); break; } - return true; - }); + return true; + } + + ); }); } -void PhaseModulationOrchestrator::handler_manualPowerControlResponse(const ManualPowerControlRequest &req, ManualPowerControlResponse &resp) { +void PhaseModulationOrchestrator::handler_modeOfOperationRequest( // + const ModeOfOperationRequest &request, + ModeOfOperationResponse &resp +) { + ULOG_INFO("ModeOfOperationRequest request handler"); + checkChannel(request.channel_id); + + if (request.has_mode) { + setMode(request.channel_id, request.mode); + } + + resp.mode = getMode(request.mode); +} + +void PhaseModulationOrchestrator::handler_groupModulationConfigRequest( // + const GroupModulationConfigRequest &request, + GroupModulationConfigResponse &resp +) { + ULOG_INFO("GroupModulationConfigRequest request handler"); + + checkChannel(request.channel_id); + checkCurrentMode(request.channel_id, ctrl_ModeOfOperation_GroupModulation); + + auto &alg = std::get(_channel[request.channel_id]); + + if(request.has_cycles_max){ + alg.setCyclesMax(request.cycles_max); + } + + resp.cycles_max = alg.getCyclesMax(); +} + +void PhaseModulationOrchestrator::handler_groupModulationControlRequest( // + const GroupModulationControlRequest &request, + GroupModulationControlResponse &resp +) { + ULOG_INFO("GroupModulationControlRequest request handler"); + checkChannel(request.channel_id); + checkCurrentMode(request.channel_id, ctrl_ModeOfOperation_GroupModulation); + + auto &alg = std::get(_channel[request.channel_id]); + + if(request.has_cycles){ + alg.setCycles(request.cycles); + } + + resp.cycles = alg.getCycles(); +} + +void PhaseModulationOrchestrator::handler_phaseModulationConfigRequest( // + const PhaseModulationConfigRequest &request, + PhaseModulationConfigResponse &resp +) { + ULOG_INFO("PhaseModulationConfigRequest request handler"); + checkChannel(request.channel_id); + checkCurrentMode(request.channel_id, ctrl_ModeOfOperation_PhaseModulation); +} + +void PhaseModulationOrchestrator::handler_phaseModulationControlRequest( // + const PhaseModulationControlRequest &request, + PhaseModulationControlResponse &resp +) { + ULOG_INFO("PhaseModulationControlRequest request handler"); + checkChannel(request.channel_id); + checkCurrentMode(request.channel_id, ctrl_ModeOfOperation_PhaseModulation); +} + +void PhaseModulationOrchestrator::checkChannel(uint8_t channel_id) { + if (channel_id >= Channels) throw ctrl::wrong_channel{}; +} + +void PhaseModulationOrchestrator::checkCurrentMode(const uint8_t channel, ModeOfOperation mode) { + if (getMode(channel) != mode) throw ctrl::wrong_mode{}; +} + +void PhaseModulationOrchestrator::setMode(uint8_t ch, ModeOfOperation mode) { + switch (mode) { + case ctrl_ModeOfOperation_Disabled: + _channel[ch] = Disabled{}; + break; + case ctrl_ModeOfOperation_GroupModulation: + _channel[ch].emplace(pins[ch]); + break; + case ctrl_ModeOfOperation_PhaseModulation: + _channel[ch].emplace(pins[ch]); + break; + case ctrl_ModeOfOperation_TemperatureSlopeModulation: + throw ctrl::unsuported_mode{}; + + case ctrl_ModeOfOperation_ConstantTemperatureModulation: + throw ctrl::unsuported_mode{}; + + default: + throw ctrl::wrong_mode{}; + break; + } +} + +ModeOfOperation PhaseModulationOrchestrator::getMode(uint8_t ch) { + if (std::holds_alternative(_channel.at(ch))) { + return ctrl_ModeOfOperation_Disabled; + } + if (std::holds_alternative(_channel.at(ch))) { + return ctrl_ModeOfOperation_PhaseModulation; + } + if (std::holds_alternative(_channel.at(ch))) { + return ctrl_ModeOfOperation_GroupModulation; + } + + return ctrl_ModeOfOperation_Disabled; } void PhaseModulationThread::threadMain() { diff --git a/rims_app/src/phase_modulation.hpp b/rims_app/src/phase_modulation.hpp index 6fcd8df47da..47c2843738a 100644 --- a/rims_app/src/phase_modulation.hpp +++ b/rims_app/src/phase_modulation.hpp @@ -2,29 +2,43 @@ /* Kernel event for notifying other threads */ #include "circular_buffer.hpp" -#include "placement_unique_ptr.hpp" #include "common.hpp" #include "proto/ctrl.pb.h" -#include +#include #include #include namespace rims { extern zephyr_fifo_buffer ctrlIngressQueue; -extern zephyr_fifo_buffer ctrlEgressQueue; +extern zephyr_fifo_buffer ctrlEgressQueue; -using ManualPowerControlRequest = ctrl_ManualPowerControlRequest; -using ManualPowerControlResponse = ctrl_ManualPowerControlResponse; +using ModeOfOperation = ctrl_ModeOfOperation; + +using ModeOfOperationRequest = ctrl_ModeOfOperationRequest; +using ModeOfOperationResponse = ctrl_ModeOfOperationResponse; + +using GroupModulationConfigRequest = ctrl_GroupModulationConfigRequest; +using GroupModulationConfigResponse = ctrl_GroupModulationConfigResponse; +using GroupModulationControlRequest = ctrl_GroupModulationControlRequest; +using GroupModulationControlResponse = ctrl_GroupModulationControlResponse; + +using PhaseModulationConfigRequest = ctrl_PhaseModulationConfigRequest; +using PhaseModulationConfigResponse = ctrl_PhaseModulationConfigResponse; +using PhaseModulationControlRequest = ctrl_PhaseModulationControlRequest; +using PhaseModulationControlResponse = ctrl_PhaseModulationControlResponse; constexpr int Channels = 2; class PhaseControlBase { public: PhaseControlBase(const gpio_dt_spec &gpio); - void action_triacDisable() const; - void action_triacEnable() const; + ~PhaseControlBase() { + disable(); + } + void disable() const; + void enable() const; const gpio_dt_spec &_gpio; }; @@ -32,29 +46,56 @@ class PhaseControlBase { class PhaseModulation : public PhaseControlBase { public: PhaseModulation(const gpio_dt_spec &gpio); + ~PhaseModulation() = default; // function triggered 100 times / sec - void setPower(float percent){ + void setPower(float percent) { _power = percent; } void tickFallingEdge(); void tickRisingEdge() {}; private: - SingleShootTimer _triacTimer, _off; - float _power; + SingleShootTimer _triacTimerStart, _triacTimerStop; + float _power; }; class GroupModulation : public PhaseControlBase { public: - void setPower(float percent){} + GroupModulation(const gpio_dt_spec &gpio); + + ~GroupModulation() = default; + void tickFallingEdge() {}; void tickRisingEdge() {}; + + void setCyclesMax(uint32_t cycles) { + _cyclesMax = cycles; + } + uint32_t getCyclesMax() { + return _cyclesMax; + } + + void setCycles(uint32_t cycles) { + _cycles = cycles; + } + uint32_t getCycles() { + return _cycles; + } + + private: + // config variables + uint32_t _cyclesMax{}; // number of cycles in a "full pass" + uint32_t _cycles{}; // number of ON cycles in a "full pass" AKA "power" + + uint32_t _cyclesLeft{}; + uint32_t _cyclesOnLeft{}; }; -class NoneModulation { +class Disabled { public: - void setPower(float percent){} + void setPower(float percent) { + } void tickFallingEdge() {}; void tickRisingEdge() {}; }; @@ -62,9 +103,9 @@ class NoneModulation { /// manages all phase controll messages class PhaseModulationOrchestrator { using Variant = std::variant< // - unique_placed_ptr, - unique_placed_ptr, - unique_placed_ptr>; + Disabled, + PhaseModulation, + GroupModulation>; public: PhaseModulationOrchestrator(); @@ -74,15 +115,25 @@ class PhaseModulationOrchestrator { void event_zeroCrossDetection(); void event_ctrlMessageArrived(); - void handler_manualPowerControlResponse(const ManualPowerControlRequest &req, ManualPowerControlResponse &resp); + void handler_modeOfOperationRequest(const ModeOfOperationRequest &request, ModeOfOperationResponse &resp); + + void handler_groupModulationConfigRequest(const GroupModulationConfigRequest &request, GroupModulationConfigResponse &resp); + void handler_groupModulationControlRequest(const GroupModulationControlRequest &request, GroupModulationControlResponse &resp); + + void handler_phaseModulationConfigRequest(const PhaseModulationConfigRequest &request, PhaseModulationConfigResponse &resp); + void handler_phaseModulationControlRequest(const PhaseModulationControlRequest &request, PhaseModulationControlResponse &resp); + + void setMode(uint8_t ch, ModeOfOperation mode); + ModeOfOperation getMode(uint8_t ch); bool setup(); int gpio_init(uint_fast8_t channel); - std::byte _buffer[Channels][std::max(sizeof(PhaseModulation), sizeof(GroupModulation))]; - Variant _channel[Channels]; + void checkChannel(const uint8_t channel); + void checkCurrentMode(const uint8_t channel, ModeOfOperation mode); - std::array _events; // event from ZCD and from CAN + std::array _channel; + std::array _events; // event from ZCD and from CAN }; class PhaseModulationThread : public ZephyrThread { diff --git a/rims_app/src/temperature_measurements.cpp b/rims_app/src/temperature_measurements.cpp index d1ccbed4ce9..af53e0d23c0 100644 --- a/rims_app/src/temperature_measurements.cpp +++ b/rims_app/src/temperature_measurements.cpp @@ -5,6 +5,8 @@ #include #include #include +#include +#include #include #include @@ -22,6 +24,23 @@ namespace rims { +namespace temp { +static char errorMessageBuffer[50]; // Static buffer to store the error message + +struct error : public std::exception {}; + +struct unknown_channel_id : error { + uint32_t _ch; + unknown_channel_id(uint32_t channel) : _ch{channel} { + } + const char *what() const noexcept override { + std::snprintf(errorMessageBuffer, sizeof(errorMessageBuffer), "Unknown channel ID: %u", _ch); + return errorMessageBuffer; + } +}; + +} // namespace temp + K_FIFO_DEFINE(temperatureIngress); K_FIFO_DEFINE(temperatureEggress); @@ -68,13 +87,12 @@ TemperatureStatistics TemperatureSampler::temperature_statistics() const { temp.temp_avg_c = mean; temp.n_samples = samples.size(); temp.temp_stddev_c = stddev; - temp.channel_id = _channel; return temp; }; TemperatureCurrent TemperatureSampler::temperature() const { - return TemperatureCurrent{.channel_id = _channel, .temp_c = this->_samples.back()}; + return TemperatureCurrent{.temp_c = this->_samples.back()}; }; TemperatureSampler::Sample TemperatureSampler::to_temperature(int adc_value) const { @@ -117,7 +135,6 @@ TemperatureSampler::Sample TemperatureSampler::adc_take_sample() const { .oversampling = 0, }; - // adc_sequence_init_dt(&adc_spec, &sequence); auto err = adc_read_dt(&adc_spec, &sequence); if (err != 0) { ULOG_ERROR("ADC got error %d at sequence.cahnnelid %d", err, sequence.channels); @@ -132,6 +149,7 @@ TemperatureSampler::Sample TemperatureSampler::adc_take_sample() const { template constexpr bool unknownChannelError(const Req &req, Resp &resp) { if (req.channel_id > ChannelNumber) { + resp.which_data = 254; // error_tag resp.data.error = temperature_Error_UnknownChannel; return true; } @@ -140,8 +158,8 @@ template constexpr bool unknownChannelError(const TemperatureSamplerOrchestrator::TemperatureSamplerOrchestrator() : // - _current_channel{0}, // - _channel{TemperatureSampler{1}, TemperatureSampler{2}}, // + _temperatureSamplerChannel{0}, // + _temperatureSamplerChannels{TemperatureSampler{1}, TemperatureSampler{2}}, // _samplerSem{0, 1}, // _samplerTimer{_samplerSem, std::chrono::milliseconds{125}}, // _broadcastSem{zephyr::semaphore::sem{0, 1}, zephyr::semaphore::sem{0, 1}}, // @@ -167,51 +185,72 @@ void TemperatureSamplerOrchestrator::loop() { _broadcastTimer[1].start(); while (1) { - auto ret = zephyr::event_pool::k_poll_forever(_events); - if (ret == 0) { - zephyr::event_pool::k_poll_handle(_events[0], [&]() { event_messageArrived(); }); - zephyr::event_pool::k_poll_handle(_events[1], [&]() { action_takeSample(); }); - zephyr::event_pool::k_poll_handle(_events[2], [&]() { action_sendTemperature(0); }); - zephyr::event_pool::k_poll_handle(_events[3], [&]() { action_sendTemperature(1); }); + try { + auto ret = zephyr::event_pool::k_poll_forever(_events); + if (ret == 0) { + zephyr::event_pool::k_poll_handle(_events[0], [&]() { event_messageArrived(); }); + zephyr::event_pool::k_poll_handle(_events[1], [&]() { action_takeSample(); }); + zephyr::event_pool::k_poll_handle(_events[2], [&]() { action_sendTemperature(0); }); + zephyr::event_pool::k_poll_handle(_events[3], [&]() { action_sendTemperature(1); }); + } + } catch (const std::exception &e) { + ULOG_ERROR("EXCEPTION %s", e.what()); } } } void TemperatureSamplerOrchestrator::event_messageArrived() { - temperatureEgressQueue.try_produce([&](temperature_EgressMessages &response) { + constexpr auto currentTemperatureRequestHandler = std::make_tuple( + temperature_IngressMessages_currentTemperatureRequest_tag, + temperature_EgressMessages_currentTemperatureResponse_tag, + &TemperatureSamplerOrchestrator::handle_getCurrentTemperatureRequest + ); + + constexpr auto temperatureRequestHandler = std::make_tuple( + temperature_IngressMessages_temperatureRequest_tag, + temperature_EgressMessages_temperatureResponse_tag, + &TemperatureSamplerOrchestrator::handle_getTemperatureRequest + ); + + constexpr auto samplerConfigHandler = std::make_tuple( + temperature_IngressMessages_samplerConfigRequest_tag, + temperature_EgressMessages_samplerConfigResponse_tag, + &TemperatureSamplerOrchestrator::handle_samplerConfigRequest + ); + + auto genericHandler = [&](const auto &request, auto &response, auto handler) { + auto fn = std::get<2>(handler); + std::invoke(fn, this, request, response); + }; + + temperatureEgressQueue.try_produce([&](temperature_EgressMessages &resp) { ULOG_INFO("Producing temperature_EgressMessages response"); - auto ok = temperatureIngressQueue.try_consume([&](temperature_IngressMessages &request) { + auto ok = temperatureIngressQueue.try_consume([&](temperature_IngressMessages &req) { ULOG_INFO("Consuming temperature_IngressMessages request"); - switch (request.which_data) { - case temperature_IngressMessages_currentTemperatureRequest_tag: - response.which_data = temperature_EgressMessages_currentTemperatureResponse_tag; - handle_getCurrentTemperatureRequest( // - request.data.currentTemperatureRequest, - response.data.currentTemperatureResponse - ); + resp.requestID = req.requestID; + resp.has_requestID = true; + + switch (req.which_data) { + case std::get<1>(currentTemperatureRequestHandler): + /// to jest źle, ustawiany jest xły tag + resp.which_data = std::get<1>(currentTemperatureRequestHandler); + genericHandler(req.data.currentTemperatureRequest, resp.data.currentTemperatureResponse, currentTemperatureRequestHandler); break; - case temperature_IngressMessages_temperatureRequest_tag: - response.which_data = temperature_EgressMessages_temperatureResponse_tag; - handle_getTemperatureRequest( // - request.data.temperatureRequest, - response.data.temperatureResponse - ); + case std::get<1>(temperatureRequestHandler): + resp.which_data = std::get<1>(temperatureRequestHandler); + genericHandler(req.data.temperatureRequest, resp.data.temperatureResponse, temperatureRequestHandler); break; - case temperature_IngressMessages_samplerConfigRequest_tag: - response.which_data = temperature_EgressMessages_samplerConfigResponse_tag; - handle_samplerConfigRequest( // - request.data.samplerConfigRequest, - response.data.samplerConfigResponse - ); + case std::get<1>(samplerConfigHandler): + resp.which_data = std::get<1>(samplerConfigHandler); + genericHandler(req.data.samplerConfigRequest, resp.data.samplerConfigResponse, samplerConfigHandler); break; default: - ULOG_WARNING("Got unknown request with id %d", request.which_data); /// TODO make better logs + ULOG_WARNING("Got unknown request with id %d", req.which_data); break; } - response.requestID = request.requestID; }); if (not ok) { ULOG_ERROR("Producing temperature_EgressMessages response FAILED"); @@ -228,7 +267,7 @@ void TemperatureSamplerOrchestrator::handle_getTemperatureRequest( // ) const { if (unknownChannelError(req, resp)) return; - resp.data.temperatureStats = _channel[req.channel_id].temperature_statistics(); + resp.data.temperatureStats = _temperatureSamplerChannels[req.channel_id].temperature_statistics(); } void TemperatureSamplerOrchestrator::handle_getCurrentTemperatureRequest( @@ -236,8 +275,10 @@ void TemperatureSamplerOrchestrator::handle_getCurrentTemperatureRequest( GetCurrentTemperatureResponse &resp ) const { if (unknownChannelError(req, resp)) return; - - resp.data.temperatureCurrent = _channel[req.channel_id].temperature(); + + resp.channel_id = req.channel_id; + resp.data.temperatureCurrent = _temperatureSamplerChannels[req.channel_id].temperature(); + resp.which_data = temperature_GetCurrentTemperatureResponse_temperatureCurrent_tag; } void TemperatureSamplerOrchestrator::handle_samplerConfigRequest( // @@ -255,14 +296,14 @@ void TemperatureSamplerOrchestrator::handle_samplerConfigRequest( // void TemperatureSamplerOrchestrator::action_takeSample() { zephyr::semaphore::k_sem_take_now(_samplerSem); - _current_channel = (_current_channel + 1) % channelNumber; - _channel[_current_channel].take_sample(); + _temperatureSamplerChannel = (_temperatureSamplerChannel + 1) % channelNumber; + _temperatureSamplerChannels[_temperatureSamplerChannel].take_sample(); /// TODO change channel } void TemperatureSamplerOrchestrator::action_sendTemperature(uint8_t ch) { zephyr::semaphore::k_sem_take_now(_broadcastSem[ch]); - PB_encode_egress(_channel[ch].temperature_statistics()); + PB_encode_egress(_temperatureSamplerChannels[ch].temperature_statistics()); } int TemperatureSamplerThread::do_hardwarenInit() { diff --git a/rims_app/src/temperature_measurements.hpp b/rims_app/src/temperature_measurements.hpp index c1c148bf33e..c048f474156 100644 --- a/rims_app/src/temperature_measurements.hpp +++ b/rims_app/src/temperature_measurements.hpp @@ -72,8 +72,8 @@ class TemperatureSamplerOrchestrator { void action_takeSample(); void action_sendTemperature(uint8_t ch); - uint8_t _current_channel{0}; - std::array _channel; + uint8_t _temperatureSamplerChannel{0}; + std::array _temperatureSamplerChannels; zephyr::semaphore::sem _samplerSem{0, 1}; RecurringSemaphoreTimer _samplerTimer; diff --git a/rims_app/src/uart.cpp b/rims_app/src/uart.cpp index 903cb8ce27a..a8fbf570f60 100644 --- a/rims_app/src/uart.cpp +++ b/rims_app/src/uart.cpp @@ -1,7 +1,7 @@ #include "uart.hpp" #include "log.hpp" -#include "message_decode.hpp" +#include "messenger.hpp" #include "syscalls/uart.h" #include diff --git a/rims_app/src/zephyr.hpp b/rims_app/src/zephyr.hpp index 99077a6082c..9d4fda74eb9 100644 --- a/rims_app/src/zephyr.hpp +++ b/rims_app/src/zephyr.hpp @@ -24,6 +24,27 @@ constexpr static k_timeout_t chronoToKTimeout(std::chrono::nanoseconds duration) return K_NSEC(duration.count()); } +namespace zephyr::gpio{ +struct error : public std::exception{ +}; + +struct io_error : public error{ +}; + +inline void pin_set_dt(const gpio_dt_spec &spec, int value){ + if(auto ret = gpio_pin_set_dt(&spec, value); ret!=0){ + throw io_error{}; + } +} + +inline void pin_toggle_dt(const gpio_dt_spec &spec){ + if(auto ret = gpio_pin_toggle_dt(&spec); ret!=0){ + throw io_error{}; + } +} + +} + namespace zephyr::crc { inline uint32_t crc32_ieee(std::span data) { -- 2.45.2 From fb083f05384cb9bb3cbe3bb28822b2726f5ee009 Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Tue, 15 Apr 2025 14:19:31 +0200 Subject: [PATCH 04/18] update temperature messages --- rims_app/proto/configuration.proto | 45 -------- rims_app/proto/message.proto | 2 +- rims_app/proto/temperature.proto | 125 +++++++++++++++++----- rims_app/proto/utilization.proto | 3 - rims_app/src/main.cpp | 50 --------- rims_app/src/temperature_measurements.cpp | 47 ++++++-- rims_app/src/temperature_measurements.hpp | 12 ++- 7 files changed, 152 insertions(+), 132 deletions(-) diff --git a/rims_app/proto/configuration.proto b/rims_app/proto/configuration.proto index 78127e7a552..e5fa3b9d906 100644 --- a/rims_app/proto/configuration.proto +++ b/rims_app/proto/configuration.proto @@ -7,58 +7,13 @@ enum Error { NoError = 0; } -enum ModeOfOperationType { - /** - Device is currently OFF - */ - Off = 0; - - /** - In manual operation, no PID loop will be enabled on the device. - In this mode there is no way to set output temperature, you can only set the output - power - - Mode will be applied to all channels - */ - Manual = 1; - - /** - TODO not implemented ;( - */ - Automatic = 2; -} - -/// Request / Response API -message ModeOfOperationRequest -{ - // not specyfying this, resulte in a "read only" request - ModeOfOperationType type = 1; -}; - -message ModeOfOperationResponse -{ - oneof resp - { - ModeOfOperationType type = 1; - Error error = 254; - } -} - // only those messages are send message IngressMessages { uint32 requestID = 255; // Unique request ID - oneof data - { - ModeOfOperationRequest modeOfOperationRequest = 1; - } }; message EgressMessages { optional uint32 requestID = 255; // Unique request ID - oneof data - { - ModeOfOperationResponse modeOfOperationResponse = 1; - } }; diff --git a/rims_app/proto/message.proto b/rims_app/proto/message.proto index c3fa9f5b115..4fc07b5fbed 100644 --- a/rims_app/proto/message.proto +++ b/rims_app/proto/message.proto @@ -12,7 +12,7 @@ enum Error { NoId = 5; NoData = 6; - RequestQueueFull =8; + RequestQueueFull = 8; UnknownId = 7; } diff --git a/rims_app/proto/temperature.proto b/rims_app/proto/temperature.proto index 14a0a81cc41..6ec114a5410 100644 --- a/rims_app/proto/temperature.proto +++ b/rims_app/proto/temperature.proto @@ -2,6 +2,23 @@ syntax = "proto3"; package temperature; +/* +messages with name + +SetXXX{} +are write only messages + +GetXXX{} +are read only messages + +XXX{} +are read write messages, meanning that if you set fileds in the message they will be set on device +the value of register will be returned without modification otherwise + +each Response message contains a optional Error field, if set the state of the device was not +changed and error code describes what went wrong +*/ + enum Error { NoError = 0; UnknownChannel = 1; @@ -9,11 +26,34 @@ enum Error { TemperatureSensorBroken = 3; } +enum FilterType { + None = 0; + Kalman = 1; + EMA = 2; + Avarage = 3; +} + /* CONFIGURATION */ message SamplerConfig { - optional uint32 samples = 3; // optional, if ommited return current value - optional uint32 period_ms = 4; // optional + optional uint32 samples = 3; + optional uint32 period_ms = 4; +} + +message KalmanFilterParams +{ + optional float Q = 1; + optional float R = 2; +} + +message EMAFilterParams +{ + optional float alpha = 1; +} + +message AverageFilterParams +{ + optional uint32 samples = 1; } /* INTERNAL STRUCTURES */ @@ -31,10 +71,10 @@ message TemperatureStatistics // last update float temp_c = 1; - // avarage from last nsamples + // average from last nsamples in Celcius float temp_avg_c = 2; - // standard deviation of temperature on given channel from last nsamples + // standard deviation of temperature on given channel from last nsamples in Celcius float temp_stddev_c = 3; // samples taken @@ -42,7 +82,7 @@ message TemperatureStatistics } /* CONFIG API */ - +// Read/Write message SamplerConfigRequest { uint32 channel_id = 1; @@ -52,45 +92,78 @@ message SamplerConfigRequest message SamplerConfigResponse { uint32 channel_id = 1; - oneof data - { - SamplerConfig config = 2; - Error error = 254; - } + SamplerConfig config = 2; + optional Error error = 254; +} + +/* +Filter configuration, can be set to any filter, despite what filter is currently enabled +*/ +message FilterConfigRequest +{ + uint32 channel_id = 1; + // skips info about current parameters in response message + bool skipFullResponse = 5; + + optional KalmanFilterParams kalmanFilterParams = 2; + optional EMAFilterParams emaFilterParams = 3; + optional AverageFilterParams averageFilterParams = 4; +} +message FilterConfigResponse +{ + uint32 channel_id = 1; + + KalmanFilterParams kalmanFilterParams = 2; + EMAFilterParams emaFilterParams = 3; + AverageFilterParams averageFilterParams = 4; + + optional Error error = 254; +} + +message FilterRequest +{ + uint32 channel_id = 1; + optional FilterType filterType = 2; +} +message FilterResponse +{ + uint32 channel_id = 1; + FilterType filterType = 2; + optional Error error = 254; } // API +// Read message GetCurrentTemperatureRequest { uint32 channel_id = 1; }; - message GetCurrentTemperatureResponse { uint32 channel_id = 1; - oneof data - { - TemperatureCurrent temperatureCurrent = 2; - Error error = 254; - } + TemperatureCurrent temperatureCurrent = 2; + optional Error error = 254; } +// Read message GetTemperatureRequest { uint32 channel_id = 1; }; - message GetTemperatureResponse { uint32 channel_id = 1; - oneof data - { - TemperatureStatistics temperatureStats = 2; - Error error = 254; - } + TemperatureStatistics temperatureStats = 2; + optional Error error = 254; } -// only those messages are send through wire at temperature endpoint +// broadcast +message BroadcastTemperatureStatistics { + uint32 channel_id = 1; + TemperatureStatistics temperatureStats = 2; +} + +// only those messages are send through wire. message IngressMessages { uint32 requestID = 255; @@ -102,6 +175,8 @@ message IngressMessages // configuration SamplerConfigRequest samplerConfigRequest = 3; + FilterConfigRequest filterConfigRequest = 4; + FilterRequest filterRequest = 5; } }; @@ -116,8 +191,10 @@ message EgressMessages // configuration SamplerConfigResponse samplerConfigResponse = 3; + FilterConfigResponse filterConfigResponse = 4; + FilterResponse filterResponse = 5; // broadcast - TemperatureStatistics broadcastTemperatureStatistics = 16; + BroadcastTemperatureStatistics broadcastTemperatureStatistics = 16; } }; diff --git a/rims_app/proto/utilization.proto b/rims_app/proto/utilization.proto index 5424bd6cec2..d265246fb1b 100644 --- a/rims_app/proto/utilization.proto +++ b/rims_app/proto/utilization.proto @@ -15,9 +15,6 @@ message Utilization message IngressMessages { uint32 requestID = 255; -// oneof data -// { -// } }; message EgressMessages diff --git a/rims_app/src/main.cpp b/rims_app/src/main.cpp index 75c1d942025..54ef0c90435 100644 --- a/rims_app/src/main.cpp +++ b/rims_app/src/main.cpp @@ -1,6 +1,5 @@ #include "common.hpp" -#include "log.hpp" #include "messenger.hpp" #include "phase_modulation.hpp" #include "placement_unique_ptr.hpp" @@ -52,9 +51,6 @@ TStack phaseModulationStack{k_phaseModulationStack, K_THREAD_STACK_SIZEOF(k_phas /// TODO power measurement thread /// TODO status led thread, print statues -// adc_channel_cfg adcch1 = ADC_CHANNEL_CFG_DT(DT_NODELABEL(adc_temp)); -// extern int z_clock_hw_cycles_per_sec; - template class LazyInit { alignas(T) std::byte _buf[sizeof(T)]; unique_placed_ptr obj; @@ -76,7 +72,6 @@ LazyInit uartThread; LazyInit temperatureSampler; LazyInit zcd; LazyInit phaseModulation; -struct except : std::exception {}; int main() { using namespace rims; @@ -104,50 +99,5 @@ int main() { while (1) { std::this_thread::sleep_for(std::chrono::seconds{2}); - - // try { - // throw except{}; - // } catch (const except &e) { - // while(1){} - // } } - - // auto getStats = [](auto &diffs) { - // if (diffs.full()) { - // auto sum = std::accumulate(diffs.begin(), diffs.end(), std::chrono::steady_clock::duration{}); - // auto average = sum / diffs.size(); - - // // Compute the standard deviation - // auto variancens = std::chrono::steady_clock::duration{}.count(); - // for (const auto &value : diffs) { - // variancens += (value - average).count() * (value - average).count(); - // } - // variancens /= diffs.size(); - // double std_dev = std::sqrt(variancens); - - // printk("avg :%fus stddev :%fus\n", average.count() / 1000.0, std_dev / 1000.0); - // return average; - // } - // return std::chrono::steady_clock::duration{}; - // }; - - // while (1) { - // k_msleep(1000); - // gpio_pin_toggle_dt(&status_pin_spec); - // auto noDetectAvg = getStats(_fallDiffs); - // auto detectAvg = getStats(_risesDiffs); - // auto t = 2 * (noDetectAvg + detectAvg).count() / 1000.0 / 1000.0; - // auto Hz = 1.0 / (t / 1000.0); - // printk("total : %fms, f:%fHz\n", t, Hz); - - // if (std::abs(Hz - 50.0) > 0.01) { - // auto nz_clock_hw_cycles_per_sec = int(50.0 * z_clock_hw_cycles_per_sec / Hz); - // auto diff = z_clock_hw_cycles_per_sec - nz_clock_hw_cycles_per_sec; - // printk("old: %d new: %d diff=%d\n", z_clock_hw_cycles_per_sec, z_clock_hw_cycles_per_sec - (diff / 4), diff); - - // z_clock_hw_cycles_per_sec -= diff / 4; - // } - - // printk("EOL\n"); - // } } diff --git a/rims_app/src/temperature_measurements.cpp b/rims_app/src/temperature_measurements.cpp index af53e0d23c0..c226445497c 100644 --- a/rims_app/src/temperature_measurements.cpp +++ b/rims_app/src/temperature_measurements.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include "common.hpp" @@ -58,7 +59,7 @@ template void PB_encode_egress(const T &tempresp) { ULOG_DEBUG("Sending SamplerConfigResponse message"); egress.data.samplerConfigResponse = tempresp; egress.which_data = temperature_EgressMessages_samplerConfigResponse_tag; - } else if constexpr (std::is_same_v) { + } else if constexpr (std::is_same_v) { ULOG_DEBUG("Sending TemperatureStatistics message"); egress.data.broadcastTemperatureStatistics = tempresp; egress.which_data = temperature_EgressMessages_broadcastTemperatureStatistics_tag; @@ -149,8 +150,9 @@ TemperatureSampler::Sample TemperatureSampler::adc_take_sample() const { template constexpr bool unknownChannelError(const Req &req, Resp &resp) { if (req.channel_id > ChannelNumber) { - resp.which_data = 254; // error_tag - resp.data.error = temperature_Error_UnknownChannel; + /// fixme + // resp.which_data = 254; // error_tag + // resp.data.error = temperature_Error_UnknownChannel; return true; } return false; @@ -218,8 +220,20 @@ void TemperatureSamplerOrchestrator::event_messageArrived() { &TemperatureSamplerOrchestrator::handle_samplerConfigRequest ); + constexpr auto filterConfigHandler = std::make_tuple( + temperature_IngressMessages_filterConfigRequest_tag, + temperature_EgressMessages_filterConfigResponse_tag, + &TemperatureSamplerOrchestrator::handle_filterConfigRequest + ); + + constexpr auto filterHandler = std::make_tuple( + temperature_IngressMessages_filterRequest_tag, + temperature_EgressMessages_filterResponse_tag, + &TemperatureSamplerOrchestrator::handle_filterRequest + ); + auto genericHandler = [&](const auto &request, auto &response, auto handler) { - auto fn = std::get<2>(handler); + auto fn = std::get<2>(handler); std::invoke(fn, this, request, response); }; @@ -246,6 +260,16 @@ void TemperatureSamplerOrchestrator::event_messageArrived() { resp.which_data = std::get<1>(samplerConfigHandler); genericHandler(req.data.samplerConfigRequest, resp.data.samplerConfigResponse, samplerConfigHandler); break; + + case std::get<1>(filterConfigHandler): + resp.which_data = std::get<1>(filterConfigHandler); + genericHandler(req.data.filterConfigRequest, resp.data.filterConfigResponse, filterConfigHandler); + break; + + case std::get<1>(filterHandler): + resp.which_data = std::get<1>(filterHandler); + genericHandler(req.data.filterRequest, resp.data.filterResponse, filterHandler); + break; default: ULOG_WARNING("Got unknown request with id %d", req.which_data); @@ -267,7 +291,7 @@ void TemperatureSamplerOrchestrator::handle_getTemperatureRequest( // ) const { if (unknownChannelError(req, resp)) return; - resp.data.temperatureStats = _temperatureSamplerChannels[req.channel_id].temperature_statistics(); + resp.temperatureStats = _temperatureSamplerChannels[req.channel_id].temperature_statistics(); } void TemperatureSamplerOrchestrator::handle_getCurrentTemperatureRequest( @@ -277,8 +301,7 @@ void TemperatureSamplerOrchestrator::handle_getCurrentTemperatureRequest( if (unknownChannelError(req, resp)) return; resp.channel_id = req.channel_id; - resp.data.temperatureCurrent = _temperatureSamplerChannels[req.channel_id].temperature(); - resp.which_data = temperature_GetCurrentTemperatureResponse_temperatureCurrent_tag; + resp.temperatureCurrent = _temperatureSamplerChannels[req.channel_id].temperature(); } void TemperatureSamplerOrchestrator::handle_samplerConfigRequest( // @@ -353,4 +376,14 @@ void TemperatureSamplerThread::threadMain() { thread.loop(); } +void TemperatureSamplerOrchestrator::handle_filterConfigRequest(const FilterConfigRequest &req, FilterConfigResponse &resp) const +{ + +} + +void TemperatureSamplerOrchestrator::handle_filterRequest(const FilterRequest &req, FilterResponse &resp) const +{ + +} + } // namespace rims diff --git a/rims_app/src/temperature_measurements.hpp b/rims_app/src/temperature_measurements.hpp index c048f474156..37a4ded00f5 100644 --- a/rims_app/src/temperature_measurements.hpp +++ b/rims_app/src/temperature_measurements.hpp @@ -25,8 +25,14 @@ using GetCurrentTemperatureRequest = temperature_GetCurrentTemperatureRequest; using GetCurrentTemperatureResponse = temperature_GetCurrentTemperatureResponse; using SamplerConfigRequest = temperature_SamplerConfigRequest; using SamplerConfigResponse = temperature_SamplerConfigResponse; -using TemperatureStatistics = temperature_TemperatureStatistics; -using TemperatureCurrent = temperature_TemperatureCurrent; +using FilterConfigRequest = temperature_FilterConfigRequest; +using FilterConfigResponse = temperature_FilterConfigResponse; +using FilterRequest = temperature_FilterRequest; +using FilterResponse = temperature_FilterResponse; + +using TemperatureStatistics = temperature_TemperatureStatistics; +using TemperatureCurrent = temperature_TemperatureCurrent; +using BroadcastTemperatureStatistics = temperature_BroadcastTemperatureStatistics; class TemperatureSampler { public: @@ -68,6 +74,8 @@ class TemperatureSamplerOrchestrator { void handle_getTemperatureRequest(const GetTemperatureRequest &req, GetTemperatureResponse &resp) const; void handle_getCurrentTemperatureRequest(const GetCurrentTemperatureRequest &req, GetCurrentTemperatureResponse &resp) const; void handle_samplerConfigRequest(const SamplerConfigRequest &req, SamplerConfigResponse &resp) const; + void handle_filterConfigRequest(const FilterConfigRequest &req, FilterConfigResponse &resp) const; + void handle_filterRequest(const FilterRequest &req, FilterResponse &resp) const; void action_takeSample(); void action_sendTemperature(uint8_t ch); -- 2.45.2 From f5be4c4e18dbcbb8dd8ef4d7e2779a6ff7e1101b Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Fri, 18 Apr 2025 14:44:09 +0200 Subject: [PATCH 05/18] fix temperature readings --- .gitignore | 1 + boards/bartoszek/rims/rims_h503cbt.dts | 121 +++++++---- rims_app/CMakeLists.txt.user | 14 +- rims_app/src/circular_buffer.hpp | 76 +++++++ rims_app/src/common.hpp | 1 + rims_app/src/log.cpp | 2 +- rims_app/src/log.hpp | 4 +- rims_app/src/main.cpp | 39 ++-- rims_app/src/messenger.cpp | 2 +- rims_app/src/phase_modulation.cpp | 30 +-- rims_app/src/phase_modulation.hpp | 3 + rims_app/src/temperature_measurements.cpp | 238 ++++++++++++++++------ rims_app/src/temperature_measurements.hpp | 37 +++- rims_app/src/zephyr.cpp | 3 +- rims_app/src/zephyr.hpp | 35 ++-- rims_app/src/zero_cross_detection.cpp | 12 +- 16 files changed, 447 insertions(+), 171 deletions(-) diff --git a/.gitignore b/.gitignore index aab3981f52f..1a0c5ba8a9e 100644 --- a/.gitignore +++ b/.gitignore @@ -12,6 +12,7 @@ \#*\# build*/ +config_default*/ !doc/build/ !scripts/build !tests/drivers/build_all diff --git a/boards/bartoszek/rims/rims_h503cbt.dts b/boards/bartoszek/rims/rims_h503cbt.dts index 08238b99154..56c3d194b2d 100644 --- a/boards/bartoszek/rims/rims_h503cbt.dts +++ b/boards/bartoszek/rims/rims_h503cbt.dts @@ -60,38 +60,68 @@ compatible= "gpio-pin"; temp_channel_sel: temp_channel_sel { - gpios = <&gpioa 1 GPIO_ACTIVE_LOW>; + gpios = <&gpioc 13 GPIO_ACTIVE_HIGH>; label = "CHSEL1"; }; - triac_enable_ch1: triac_enable_ch1 { + rmin_en: rmin_en { + gpios = <&gpioc 14 GPIO_ACTIVE_HIGH>; + label = "Rmin_EN"; + }; + + rmax_en: rmax_en { + gpios = <&gpioc 15 GPIO_ACTIVE_HIGH>; + label = "Rmax_EN"; + }; + + rpt_en: rpt_en { gpios = <&gpiob 6 GPIO_ACTIVE_HIGH>; - label = "POWER_ENABLE_1"; + label = "Rpt_EN"; }; - triac_enable_ch2: triac_enable_ch2 { + gprelay_1_en: gprelay_1_en { + gpios = <&gpiob 1 GPIO_ACTIVE_HIGH>; + label = "GPRelay_1_EN"; + }; + + gprelay_2_en: gprelay_2_en { + gpios = <&gpiob 0 GPIO_ACTIVE_HIGH>; + label = "GPRelay_2_EN"; + }; + + power_led: power_led { + gpios = <&gpioa 6 GPIO_ACTIVE_HIGH>; + label = "POWER_LED"; + }; + + status_led: status_led { + gpios = <&gpioa 7 GPIO_ACTIVE_HIGH>; + label = "STATUS_LED"; + }; + + can_stby: can_stby { + gpios = <&gpioa 9 GPIO_ACTIVE_HIGH>; + label = "CAN_STBY"; + }; + + ch1_zcd: ch1_zcd { gpios = <&gpiob 4 GPIO_ACTIVE_HIGH>; - label = "POWER_ENABLE_2"; + label = "CH1_ZCD"; }; - //line_enable_ch1: line_enable_ch1 { - // gpios = <&gpiob 3 GPIO_ACTIVE_LOW>; - // label = "POWER_ENABLE_1"; - //}; - - //line_enable_ch2: line_enable_ch2 { - // gpios = <&gpiob 4 GPIO_ACTIVE_LOW>; - // label = "POWER_ENABLE_2"; - //}; - - zcd_state_1: zcd_state_1 { - gpios = <&gpiob 5 GPIO_ACTIVE_LOW>; - label = "ZCD_IN_1"; + ch1_en: ch1_en { + gpios = <&gpiob 5 GPIO_ACTIVE_HIGH>; + label = "CH1_EN"; }; - zcd_state_2: zcd_state_2 { - gpios = <&gpiob 3 GPIO_ACTIVE_LOW>; - label = "ZCD_IN_2"; + ch2_zcd: ch2_zcd { + gpios = <&gpioa 15 GPIO_ACTIVE_HIGH>; + label = "CH2_ZCD"; + }; + + ch2_en: ch2_en { + gpios = <&gpiob 3 GPIO_ACTIVE_HIGH>; + label = "CH2_EN"; }; }; @@ -119,15 +149,15 @@ zephyr,acquisition-time = ; zephyr,resolution = <12>; }; - adc_current_ch1: channel@18 { // @pinctrl.dtsi as adc1_inp18_pa4: adc1_inp18_pa4 - reg = <18>; + adc_current_ch1: channel@14 { // @pinctrl.dtsi as adc1_inp14_pa2: adc1_inp14_pa2 + reg = <14>; zephyr,gain = "ADC_GAIN_1"; zephyr,reference = "ADC_REF_INTERNAL"; zephyr,acquisition-time = ; zephyr,resolution = <12>; }; - adc_current_ch2: channel@19 { // @pinctrl.dtsi as adc1_inp19_pa5: adc1_inp19_pa5 - reg = <19>; + adc_current_ch2: channel@18 { // @pinctrl.dtsi as adc1_inn18_pa5: adc1_inn18_pa5 + reg = <18>; zephyr,gain = "ADC_GAIN_1"; zephyr,reference = "ADC_REF_INTERNAL"; zephyr,acquisition-time = ; @@ -143,10 +173,14 @@ status = "okay"; }; +&gpioc{ + status = "okay"; +}; + &clk_csi { status = "okay"; }; -// 240886428 + &clk_hsi { /* 64MHz, ok*/ clock-frequency = ; status = "okay"; @@ -157,22 +191,23 @@ }; &clk_hse { - status = "disabled"; + status = "okay"; + clock-frequency = ; }; -&pll { - div-m = <4>; - mul-n = <30>; - div-r = <6>; +&pll1 { + div-m = <1>; + mul-n = <20>; + div-r = <2>; div-p = <2>; div-q = <2>; - clocks = <&clk_hsi>; + clocks = <&clk_hse>; status = "okay"; }; &rcc { - clocks = <&pll>; - clock-frequency = ; + clocks = <&pll1>; + clock-frequency = ; ahb-prescaler = <1>; apb1-prescaler = <1>; apb2-prescaler = <1>; @@ -180,22 +215,24 @@ status = "okay"; }; -//&gpdma1 { -// status = "okay"; -//}; - &usart1 { pinctrl-0 = <&usart1_tx_pb14 &usart1_rx_pb15>; pinctrl-names = "default"; -// dmas = <&gpdma1 7 22 (STM32_DMA_PERIPH_TX | STM32_DMA_PRIORITY_LOW) // GPDMA1_REQUEST_USART1_TX -// &gpdma1 2 21 (STM32_DMA_PERIPH_RX | STM32_DMA_PRIORITY_LOW)>; // GPDMA1_REQUEST_USART1_RX -// dma-names = "tx", "rx"; + current-speed = <3000000>; - current-speed = <921600>; +// clocks = <&rcc STM32_CLOCK(APB2, 14)>, <&rcc STM32_SRC_PLL2_Q USART1_SEL(1)>; status = "okay"; }; +&usart1_tx_pb14 { + slew-rate = "very-high-speed"; +}; + +&usart1_rx_pb15 { + slew-rate = "very-high-speed"; +}; + /* &rng { status = "okay"; diff --git a/rims_app/CMakeLists.txt.user b/rims_app/CMakeLists.txt.user index 4e9a5f03d5a..f68b96de663 100644 --- a/rims_app/CMakeLists.txt.user +++ b/rims_app/CMakeLists.txt.user @@ -1,6 +1,6 @@ - + EnvironmentId @@ -108,14 +108,14 @@ 2 false - -DCMAKE_COLOR_DIAGNOSTICS:BOOL=ON --DCMAKE_PREFIX_PATH:PATH=%{Qt:QT_INSTALL_PREFIX} --DCMAKE_BUILD_TYPE:STRING=Build --DCMAKE_PROJECT_INCLUDE_BEFORE:FILEPATH=%{BuildConfig:BuildDirectory:NativeFilePath}/.qtc/package-manager/auto-setup.cmake + -DCMAKE_BUILD_TYPE:STRING=Build +-DCMAKE_COLOR_DIAGNOSTICS:BOOL=ON +-DCMAKE_CXX_COMPILER:FILEPATH=%{Compiler:Executable:Cxx} -DCMAKE_C_COMPILER:FILEPATH=%{Compiler:Executable:C} --DQT_QMAKE_EXECUTABLE:FILEPATH=%{Qt:qmakeExecutable} -DCMAKE_GENERATOR:STRING=Ninja --DCMAKE_CXX_COMPILER:FILEPATH=%{Compiler:Executable:Cxx} +-DQT_QMAKE_EXECUTABLE:FILEPATH=%{Qt:qmakeExecutable} +-DCMAKE_PREFIX_PATH:PATH=%{Qt:QT_INSTALL_PREFIX} +-DCMAKE_PROJECT_INCLUDE_BEFORE:FILEPATH=%{BuildConfig:BuildDirectory:NativeFilePath}/.qtc/package-manager/auto-setup.cmake /home/bartoszek/zephyrproject/zephyr/rims_app /home/bartoszek/zephyrproject/zephyr/rims_app/build diff --git a/rims_app/src/circular_buffer.hpp b/rims_app/src/circular_buffer.hpp index e71daf20845..d3a9d8af05e 100644 --- a/rims_app/src/circular_buffer.hpp +++ b/rims_app/src/circular_buffer.hpp @@ -197,6 +197,44 @@ template class circular_buffer { ++(*this); return tmp; } + + iterator operator+(difference_type n) const { + std::size_t new_index = (index_ + n) % N; + return iterator(buf_, new_index, tail_, head_); + } + + iterator operator-(difference_type n) const { + std::size_t new_index = (index_ + N - (n % N)) % N; + return iterator(buf_, new_index, tail_, head_); + } + + iterator& operator+=(difference_type n) { + index_ = (index_ + n) % N; + return *this; + } + + iterator& operator-=(difference_type n) { + index_ = (index_ + N - (n % N)) % N; + return *this; + } + + reference operator[](difference_type n) { + std::size_t idx = (index_ + n) % N; + return *reinterpret_cast(std::addressof(buf_[idx])); + } + + bool operator<(const iterator& other) const { + return (*this - other) < 0; + } + bool operator>(const iterator& other) const { + return other < *this; + } + bool operator<=(const iterator& other) const { + return !(*this > other); + } + bool operator>=(const iterator& other) const { + return !(*this < other); + } bool operator==(const iterator &other) const { return index_ == other.index_ && looped_; @@ -251,6 +289,44 @@ template class circular_buffer { ++(*this); return tmp; } + + const_iterator operator+(difference_type n) const { + std::size_t new_index = (index_ + n) % N; + return {buf_, new_index, tail_, head_}; + } + + const_iterator operator-(difference_type n) const { + std::size_t new_index = (index_ + N - (n % N)) % N; + return {buf_, new_index, tail_, head_}; + } + + const_iterator& operator+=(difference_type n) { + index_ = (index_ + n) % N; + return *this; + } + + const_iterator& operator-=(difference_type n) { + index_ = (index_ + N - (n % N)) % N; + return *this; + } + + reference operator[](difference_type n) { + std::size_t idx = (index_ + n) % N; + return *reinterpret_cast(std::addressof(buf_[idx])); + } + + bool operator<(const iterator& other) const { + return (*this - other) < 0; + } + bool operator>(const iterator& other) const { + return other < *this; + } + bool operator<=(const iterator& other) const { + return !(*this > other); + } + bool operator>=(const iterator& other) const { + return !(*this < other); + } bool operator==(const const_iterator &other) const { return index_ == other.index_ && looped_; diff --git a/rims_app/src/common.hpp b/rims_app/src/common.hpp index 39c5934f31a..20c3dcab401 100644 --- a/rims_app/src/common.hpp +++ b/rims_app/src/common.hpp @@ -203,6 +203,7 @@ class ZephyrThread { // Optionally start the thread if delayed or suspended void start() { k_thread_start(&_threadData); + k_yield(); } protected: diff --git a/rims_app/src/log.cpp b/rims_app/src/log.cpp index bbe10001e82..834181ae02d 100644 --- a/rims_app/src/log.cpp +++ b/rims_app/src/log.cpp @@ -6,7 +6,7 @@ namespace rims { K_FIFO_DEFINE(klogFifoQueue); -zephyr_fifo_buffer logEgressFifoQueueBuffer{klogFifoQueue}; +zephyr_fifo_buffer logEgressFifoQueueBuffer{klogFifoQueue}; static std::size_t g_droppedLogs{0}; diff --git a/rims_app/src/log.hpp b/rims_app/src/log.hpp index d1fa996281b..993345bf50f 100644 --- a/rims_app/src/log.hpp +++ b/rims_app/src/log.hpp @@ -20,10 +20,10 @@ enum class LogLevel : int { // }; extern zephyr_fifo_buffer logIngressFifoQueueBuffer; -extern zephyr_fifo_buffer logEgressFifoQueueBuffer; +extern zephyr_fifo_buffer logEgressFifoQueueBuffer; /// TODO move -static LogLevel g_logLevel = LogLevel::Critical; +static LogLevel g_logLevel = LogLevel::Debug; class Log { public: diff --git a/rims_app/src/main.cpp b/rims_app/src/main.cpp index 54ef0c90435..6b785b435a0 100644 --- a/rims_app/src/main.cpp +++ b/rims_app/src/main.cpp @@ -5,29 +5,21 @@ #include "placement_unique_ptr.hpp" #include "temperature_measurements.hpp" #include "uart.hpp" +#include "zephyr.hpp" #include "zero_cross_detection.hpp" +#include #include #include #include + #include #include #include #include -extern "C" void _exit() { -} - -[[noreturn]] void Terminate() noexcept { - // insert your own termination handler code here... - while (true) { - } -} - -namespace __cxxabiv1 { -std::terminate_handler __terminate_handler = Terminate; -} +#include using namespace rims; @@ -37,7 +29,7 @@ TStack messengerStack{k_messengerStack, K_THREAD_STACK_SIZEOF(k_messengerStack)} static K_THREAD_STACK_DEFINE(k_uartStack, 2048); TStack uartStack{k_uartStack, K_THREAD_STACK_SIZEOF(k_uartStack)}; -static K_THREAD_STACK_DEFINE(k_temperatureSamplerStack, 2048); +static K_THREAD_STACK_DEFINE(k_temperatureSamplerStack, 4096); TStack temperatureSamplerStack{k_temperatureSamplerStack, K_THREAD_STACK_SIZEOF(k_temperatureSamplerStack)}; static K_THREAD_STACK_DEFINE(k_zeroCrossDetectionStack, 2048); @@ -73,9 +65,15 @@ LazyInit temperatureSampler; LazyInit zcd; LazyInit phaseModulation; +gpio_dt_spec status = GPIO_DT_SPEC_GET(DT_NODELABEL(status_led), gpios); +gpio_dt_spec gprelay_1_en = GPIO_DT_SPEC_GET(DT_NODELABEL(gprelay_1_en), gpios); +gpio_dt_spec gprelay_2_en = GPIO_DT_SPEC_GET(DT_NODELABEL(gprelay_2_en), gpios); // pompka + int main() { using namespace rims; + const auto wait = []() { std::this_thread::sleep_for(std::chrono::milliseconds{100}); }; + messengerTread.init(messengerStack); uartThread.init(uartStack); temperatureSampler.init(temperatureSamplerStack); @@ -84,20 +82,33 @@ int main() { messengerTread->init_hw(); messengerTread->start(); + wait(); uartThread->init_hw(); uartThread->start(); + wait(); temperatureSampler->init_hw(); temperatureSampler->start(); + wait(); zcd->init_hw(); zcd->start(); + wait(); phaseModulation->init_hw(); phaseModulation->start(); + wait(); + + zephyr::gpio::pin_configure(status, GPIO_OUTPUT_INACTIVE); + zephyr::gpio::pin_configure(gprelay_1_en, GPIO_OUTPUT_INACTIVE); + zephyr::gpio::pin_configure(gprelay_2_en, GPIO_OUTPUT_INACTIVE); while (1) { - std::this_thread::sleep_for(std::chrono::seconds{2}); + std::this_thread::sleep_for(std::chrono::seconds{5}); + // zephyr::gpio::pin_toggle_dt(gprelay_2_en); + + // const unsigned char data[] = {'b', 0x55}; + // uart_fifo_fill(defaultUart()->_dev, data, 1); } } diff --git a/rims_app/src/messenger.cpp b/rims_app/src/messenger.cpp index b0fd7f364e6..0fd053f386e 100644 --- a/rims_app/src/messenger.cpp +++ b/rims_app/src/messenger.cpp @@ -213,7 +213,7 @@ struct request_queue_full : public error { } // namespace messenger -MessengerThread::MessengerThread(TStackBase &stack) : ZephyrThread{stack, 10, 0, "Messenger"} { +MessengerThread::MessengerThread(TStackBase &stack) : ZephyrThread{stack, 9, 0, "Messenger"} { zephyr::event_pool::k_init(_events.at(0), messenger_buffer_arrived_queue); for (auto &ar : _activeRequests) { diff --git a/rims_app/src/phase_modulation.cpp b/rims_app/src/phase_modulation.cpp index fa4e105d127..c9e0c848a1a 100644 --- a/rims_app/src/phase_modulation.cpp +++ b/rims_app/src/phase_modulation.cpp @@ -67,8 +67,8 @@ zephyr_fifo_buffer ctrlIngressQueue{ctrlIngress}; zephyr_fifo_buffer ctrlEgressQueue{ctrlEggress}; std::array pins = { - gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(triac_enable_ch1), gpios), - gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(triac_enable_ch2), gpios) + gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_en), gpios), + gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_en), gpios) }; struct PhaseControl { @@ -140,26 +140,28 @@ int PhaseModulationThread::do_hardwarenInit() { return 0; } -// constexpr float PI = 3.141592f; -// constexpr float FREQUENCY = 0.5f; // 2 Hz -// constexpr float MIN_VALUE = 10.0f; -// constexpr float MAX_VALUE = 25.0f; +constexpr float PI = 3.141592f; +constexpr float FREQUENCY = 0.5f; // 2 Hz +constexpr float MIN_VALUE = 10.0f; +constexpr float MAX_VALUE = 25.0f; -// float sinewave(std::chrono::steady_clock::time_point timePoint) { -// using namespace std::chrono; -// static auto startTime = steady_clock::now(); +float sinewave(std::chrono::steady_clock::time_point timePoint) { + using namespace std::chrono; + static auto startTime = steady_clock::now(); -// float elapsedSeconds = duration(timePoint - startTime).count(); -// float sineValue = sinf(2.0f * PI * FREQUENCY * elapsedSeconds); + float elapsedSeconds = duration(timePoint - startTime).count(); + float sineValue = sinf(2.0f * PI * FREQUENCY * elapsedSeconds); -// // return std::sin(2.0 * PI * FREQUENCY * elapsedSeconds); -// return MIN_VALUE + (sineValue + 1.0f) * 0.5f * (MAX_VALUE - MIN_VALUE); -// } + // return std::sin(2.0 * PI * FREQUENCY * elapsedSeconds); + return MIN_VALUE + (sineValue + 1.0f) * 0.5f * (MAX_VALUE - MIN_VALUE); +} void PhaseModulationOrchestrator::event_zeroCrossDetection() { auto tickFallingEdge = [](auto &channel) { channel.tickFallingEdge(); }; auto tickRisingEdge = [](auto &channel) { channel.tickRisingEdge(); }; + auto setPower = [](auto &channel) { channel.setPower(sinewave(std::chrono::steady_clock::now())); }; + std::visit(setPower, _channel[0]); /// TODO check proper channel ZeroCrossDetectionEventQueue.try_consume([&](ZeroCrossDetectionEvent &event) { if (event.state) { diff --git a/rims_app/src/phase_modulation.hpp b/rims_app/src/phase_modulation.hpp index 47c2843738a..52a3cb5a405 100644 --- a/rims_app/src/phase_modulation.hpp +++ b/rims_app/src/phase_modulation.hpp @@ -68,6 +68,9 @@ class GroupModulation : public PhaseControlBase { void tickFallingEdge() {}; void tickRisingEdge() {}; + + void setPower(float percent) { + } void setCyclesMax(uint32_t cycles) { _cyclesMax = cycles; diff --git a/rims_app/src/temperature_measurements.cpp b/rims_app/src/temperature_measurements.cpp index c226445497c..b645701a8ae 100644 --- a/rims_app/src/temperature_measurements.cpp +++ b/rims_app/src/temperature_measurements.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -48,6 +49,29 @@ K_FIFO_DEFINE(temperatureEggress); zephyr_fifo_buffer temperatureIngressQueue{temperatureIngress}; zephyr_fifo_buffer temperatureEgressQueue{temperatureEggress}; +constexpr gpio_dt_spec rmin_en_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(rmin_en), gpios); +constexpr gpio_dt_spec rmax_en_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(rmax_en), gpios); +constexpr gpio_dt_spec rpt_en_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(rpt_en), gpios); +constexpr gpio_dt_spec temp_channel_sel_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(temp_channel_sel), gpios); + +void select_rmin(){ + zephyr::gpio::pin_set_low(rmax_en_gpio_dt); + zephyr::gpio::pin_set_high(rmin_en_gpio_dt); + zephyr::gpio::pin_set_low(rpt_en_gpio_dt); +} + +void select_rmax(){ + zephyr::gpio::pin_set_high(rmax_en_gpio_dt); + zephyr::gpio::pin_set_low(rmin_en_gpio_dt); + zephyr::gpio::pin_set_low(rpt_en_gpio_dt); +} + +void select_rpt(){ + zephyr::gpio::pin_set_low(rmax_en_gpio_dt); + zephyr::gpio::pin_set_low(rmin_en_gpio_dt); + zephyr::gpio::pin_set_high(rpt_en_gpio_dt); +} + template void PB_encode_egress(const T &tempresp) { /// TODO repeat on fail temperatureEgressQueue.try_produce([&](temperature_EgressMessages &egress) { @@ -69,64 +93,109 @@ template void PB_encode_egress(const T &tempresp) { } void TemperatureSampler::take_sample() { + ULOG_DEBUG("Samples on ch %d, size %d", this->_channel, this->_samples.size()); _samples.emplace_front(adc_take_sample()); } -TemperatureStatistics TemperatureSampler::temperature_statistics() const { - const auto &samples = this->_samples; +TemperatureSampler::TemperatureSampler(uint8_t channel) + : /*_samplerSem{0, 1}, _samplerTimer{_samplerSem, std::chrono::milliseconds{125}}, */ _channel{channel} { + calibration(); +} - auto sum = std::accumulate(samples.cbegin(), samples.cend(), Sample{}); - auto mean = sum / samples.size(); - auto sq_sum = std::accumulate(samples.cbegin(), samples.cend(), Sample{}, [&](auto currentSum, auto sample) { +void TemperatureSampler::calibration() { + /// TODO Change to current channal + ULOG_INFO("Starting temp adc calibration"); + select_rmin(); + this->_adc_Tmin = takeStableRead(); + + select_rmax(); + this->_adc_Tmax = takeStableRead(); + ULOG_INFO("Calibration done, min{%d}/max{%d}", _adc_Tmin, _adc_Tmax); + + select_rpt(); +} + +void TemperatureSampler::tick() { + // if (zephyr::semaphore::k_sem_take_now(_samplerSem) == 0) { + // take_sample(); + // } +} + +BroadcastTemperatureStatistics TemperatureSampler::temperature_statistics() const { + const auto &samples = this->_samples; + std::size_t total_samples = samples.size(); + + // Use only the last N samples or all if fewer + std::size_t count = std::max(std::min(this->_samplesNumber, total_samples), 1U); + auto start_it = samples.cend() - count; + + auto sum = std::accumulate(start_it, samples.cend(), Sample{}); + auto mean = sum / count; + + auto sq_sum = std::accumulate(start_it, samples.cend(), Sample{}, [&](auto currentSum, auto sample) { return currentSum + ((sample - mean) * (sample - mean)); }); - auto stddev = std::sqrt(sq_sum / samples.size()); + auto stddev = std::sqrt(sq_sum / count); - TemperatureStatistics temp = temperature_TemperatureStatistics_init_zero; - temp.temp_c = samples.back(); - temp.temp_avg_c = mean; - temp.n_samples = samples.size(); - temp.temp_stddev_c = stddev; + BroadcastTemperatureStatistics btemp; + btemp.channel_id = this->_channel; + btemp.has_temperatureStats = true; - return temp; -}; + TemperatureStatistics &temp = btemp.temperatureStats; + temp.temp_c = samples.back(); + temp.temp_avg_c = mean; + temp.n_samples = count; + temp.temp_stddev_c = stddev; + + return btemp; +} TemperatureCurrent TemperatureSampler::temperature() const { return TemperatureCurrent{.temp_c = this->_samples.back()}; -}; - -TemperatureSampler::Sample TemperatureSampler::to_temperature(int adc_value) const { - // Constants for ADC calculation - /// TODO procedure to get VREF - const float V_REF = 3.27f; // Reference voltage - const int ADC_RESOLUTION = 4096; // 12-bit ADC resolution - - // Convert ADC value to voltage - const float voltage = (adc_value * V_REF) / (ADC_RESOLUTION - 1); - - // Linear interpolation coefficients - const float V1 = 0.0389f; // Voltage corresponding to T1 - const float T1 = 103.945f; // Temperature at V1 - const float V2 = 0.634f; // Voltage corresponding to T2 - const float T2 = 22.0f; // Temperature at V2 - - // Calculate temperature using linear interpolation - return T1 + (T2 - T1) * (voltage - V1) / (V2 - V1); } -TemperatureSampler::Sample TemperatureSampler::adc_take_sample() const { +void TemperatureSampler::setSamplerConfig(const temperature_SamplerConfig &config) noexcept(false) { + if (config.has_samples) { + ULOG_INFO("change samples num from %d to %d", this->_samplesNumber, config.samples); + this->_samplesNumber = config.samples; + } + + // if (config.has_period_ms) { + // ULOG_INFO("change period from %lld to %d", this->_samplerTimer.interval().count(), config.samples); + // this->_samplerTimer.setInterval(std::chrono::milliseconds{config.period_ms}); + // } +} + +TemperatureSampler::Sample TemperatureSampler::to_temperature(uint32_t adcReading) const { + const auto tempMin = this->_Tmin; + const auto tempMax = this->_Tmax; + const auto adcMax = this->_adc_Tmax; + const auto adcMin = this->_adc_Tmin; + + // Clamp adcReading within adcMin and adcMax range + if (adcReading < adcMax) adcReading = adcMax; + if (adcReading > adcMin) adcReading = adcMin; + + // Inverse linear interpolation + float temperature = tempMin + (float)(adcMin - adcReading) * (tempMax - tempMin) / (float)(adcMin - adcMax); + + return temperature; +} + +uint32_t TemperatureSampler::adc() const +{ std::array samples{}; // TODO add emssage for samples fer measurement constexpr auto ADC_RESOLUTION = 12; const adc_dt_spec adc_spec = ADC_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 0); - + const adc_sequence_options options = { .interval_us = 0, .callback = nullptr, .user_data = nullptr, .extra_samplings = 3, }; - + adc_sequence sequence = { .options = &options, .channels = BIT(15), @@ -135,24 +204,27 @@ TemperatureSampler::Sample TemperatureSampler::adc_take_sample() const { .resolution = ADC_RESOLUTION, .oversampling = 0, }; - + auto err = adc_read_dt(&adc_spec, &sequence); if (err != 0) { ULOG_ERROR("ADC got error %d at sequence.cahnnelid %d", err, sequence.channels); return 0; } - + auto sum = std::accumulate(samples.begin(), samples.end(), 0); auto avg = sum / samples.size(); + + return avg; +} - return to_temperature(avg); +TemperatureSampler::Sample TemperatureSampler::adc_take_sample() const { + return to_temperature(adc()); } template constexpr bool unknownChannelError(const Req &req, Resp &resp) { if (req.channel_id > ChannelNumber) { - /// fixme - // resp.which_data = 254; // error_tag - // resp.data.error = temperature_Error_UnknownChannel; + resp.error = temperature_Error_UnknownChannel; + resp.has_error = true; return true; } return false; @@ -160,10 +232,10 @@ template constexpr bool unknownChannelError(const TemperatureSamplerOrchestrator::TemperatureSamplerOrchestrator() : // - _temperatureSamplerChannel{0}, // - _temperatureSamplerChannels{TemperatureSampler{1}, TemperatureSampler{2}}, // + _temperatureSamplerChannel{0}, // + _temperatureSamplerChannels{TemperatureSampler{1}, TemperatureSampler{2}}, // _samplerSem{0, 1}, // - _samplerTimer{_samplerSem, std::chrono::milliseconds{125}}, // + _samplerTimer{_samplerSem, std::chrono::milliseconds{500}}, // _broadcastSem{zephyr::semaphore::sem{0, 1}, zephyr::semaphore::sem{0, 1}}, // _broadcastTimer{ // RecurringSemaphoreTimer{_broadcastSem.at(0), std::chrono::seconds{4}}, @@ -260,12 +332,12 @@ void TemperatureSamplerOrchestrator::event_messageArrived() { resp.which_data = std::get<1>(samplerConfigHandler); genericHandler(req.data.samplerConfigRequest, resp.data.samplerConfigResponse, samplerConfigHandler); break; - + case std::get<1>(filterConfigHandler): resp.which_data = std::get<1>(filterConfigHandler); genericHandler(req.data.filterConfigRequest, resp.data.filterConfigResponse, filterConfigHandler); break; - + case std::get<1>(filterHandler): resp.which_data = std::get<1>(filterHandler); genericHandler(req.data.filterRequest, resp.data.filterResponse, filterHandler); @@ -291,7 +363,7 @@ void TemperatureSamplerOrchestrator::handle_getTemperatureRequest( // ) const { if (unknownChannelError(req, resp)) return; - resp.temperatureStats = _temperatureSamplerChannels[req.channel_id].temperature_statistics(); + resp.temperatureStats = _temperatureSamplerChannels[req.channel_id].temperature_statistics().temperatureStats; } void TemperatureSamplerOrchestrator::handle_getCurrentTemperatureRequest( @@ -299,29 +371,49 @@ void TemperatureSamplerOrchestrator::handle_getCurrentTemperatureRequest( GetCurrentTemperatureResponse &resp ) const { if (unknownChannelError(req, resp)) return; - - resp.channel_id = req.channel_id; + + resp.channel_id = req.channel_id; resp.temperatureCurrent = _temperatureSamplerChannels[req.channel_id].temperature(); } void TemperatureSamplerOrchestrator::handle_samplerConfigRequest( // const SamplerConfigRequest &req, SamplerConfigResponse &resp -) const { +) { if (unknownChannelError(req, resp)) return; if (req.has_config) { + _temperatureSamplerChannels[req.channel_id].setSamplerConfig(req.config); /// TODO apply configuration } else { /// TODO get configuration only } } +void TemperatureSamplerOrchestrator::handle_filterConfigRequest(const FilterConfigRequest &req, FilterConfigResponse &resp) const { + if (unknownChannelError(req, resp)) return; + + if (req.has_emaFilterParams) { + } + + if (req.has_kalmanFilterParams) { + } + + if (req.has_averageFilterParams) { + } + + if (not req.skipFullResponse) { + } +} + +void TemperatureSamplerOrchestrator::handle_filterRequest(const FilterRequest &req, FilterResponse &resp) const { + if (unknownChannelError(req, resp)) return; +} + void TemperatureSamplerOrchestrator::action_takeSample() { zephyr::semaphore::k_sem_take_now(_samplerSem); - _temperatureSamplerChannel = (_temperatureSamplerChannel + 1) % channelNumber; _temperatureSamplerChannels[_temperatureSamplerChannel].take_sample(); - /// TODO change channel + _temperatureSamplerChannel = (_temperatureSamplerChannel + 1) % channelNumber; } void TemperatureSamplerOrchestrator::action_sendTemperature(uint8_t ch) { @@ -365,6 +457,11 @@ int TemperatureSamplerThread::do_hardwarenInit() { auto ret = configurechannel(adc_temp); // configurechannel(adc_current_ch1); // configurechannel(adc_current_ch2); + + zephyr::gpio::pin_configure(rmax_en_gpio_dt, GPIO_OUTPUT_INACTIVE); + zephyr::gpio::pin_configure(rmin_en_gpio_dt, GPIO_OUTPUT_INACTIVE); + zephyr::gpio::pin_configure(rpt_en_gpio_dt, GPIO_OUTPUT_INACTIVE); + zephyr::gpio::pin_configure(temp_channel_sel_gpio_dt, GPIO_OUTPUT_INACTIVE); return ret; } @@ -376,14 +473,37 @@ void TemperatureSamplerThread::threadMain() { thread.loop(); } -void TemperatureSamplerOrchestrator::handle_filterConfigRequest(const FilterConfigRequest &req, FilterConfigResponse &resp) const -{ - -} +uint32_t TemperatureSampler::takeStableRead() { + const auto start = std::chrono::steady_clock::now(); + ULOG_INFO("Starting ADC stable read"); -void TemperatureSamplerOrchestrator::handle_filterRequest(const FilterRequest &req, FilterResponse &resp) const -{ - + int maxIterations = 1000; + int STABILITY_COUNT_REQUIRED = 5; + int STABILITY_THRESHOLD = 5; + + int prevAdc = 0; + uint8_t stableCount = 0; + + while (stableCount < STABILITY_COUNT_REQUIRED && maxIterations > 0) { + std::this_thread::sleep_for(std::chrono::microseconds{1000}); // small delay between samples + uint16_t adcraw = adc(); + + if (std::abs(adcraw - prevAdc) < STABILITY_THRESHOLD) { + stableCount++; + } else { + stableCount = 0; + } + + prevAdc = adcraw; + maxIterations--; + } + + const auto stop = std::chrono::steady_clock::now(); + auto ms = (stop - start).count() / 1000 / 1000; + auto itleft = (1000 - maxIterations); + ULOG_INFO("ADC stable read %d end after %lld ms in %d iterations", prevAdc, ms, itleft); + + return prevAdc; } } // namespace rims diff --git a/rims_app/src/temperature_measurements.hpp b/rims_app/src/temperature_measurements.hpp index 37a4ded00f5..2c2dde405a0 100644 --- a/rims_app/src/temperature_measurements.hpp +++ b/rims_app/src/temperature_measurements.hpp @@ -38,25 +38,40 @@ class TemperatureSampler { public: using Sample = float; - constexpr TemperatureSampler(std::uint8_t channel) : _channel{channel} { - } + TemperatureSampler(std::uint8_t channel); TemperatureSampler(const TemperatureSampler &) = delete; TemperatureSampler &operator=(const TemperatureSampler &) = delete; TemperatureSampler(TemperatureSampler &&) = delete; TemperatureSampler &operator=(TemperatureSampler &&) = delete; - - void take_sample(); - - TemperatureStatistics temperature_statistics() const; + + void calibration(); + void tick(); + + BroadcastTemperatureStatistics temperature_statistics() const; TemperatureCurrent temperature() const; - + + void take_sample(); + void setSamplerConfig(const temperature_SamplerConfig &config) noexcept(false); + protected: - Sample to_temperature(int adc_value) const; + uint32_t takeStableRead(); + + Sample to_temperature(uint32_t adc_value) const; + uint32_t adc() const; Sample adc_take_sample() const; - + + uint32_t _adc_Tmin{}; + uint32_t _adc_Tmax{}; + float _Tmin{-25.0f}; + float _Tmax{103.0f}; + + // zephyr::semaphore::sem _samplerSem{0, 1}; + // RecurringSemaphoreTimer _samplerTimer; + circular_buffer _samples; + std::size_t _samplesNumber{MaxSampleSize}; const std::uint8_t _channel{}; }; @@ -73,7 +88,7 @@ class TemperatureSamplerOrchestrator { void handle_getTemperatureRequest(const GetTemperatureRequest &req, GetTemperatureResponse &resp) const; void handle_getCurrentTemperatureRequest(const GetCurrentTemperatureRequest &req, GetCurrentTemperatureResponse &resp) const; - void handle_samplerConfigRequest(const SamplerConfigRequest &req, SamplerConfigResponse &resp) const; + void handle_samplerConfigRequest(const SamplerConfigRequest &req, SamplerConfigResponse &resp); void handle_filterConfigRequest(const FilterConfigRequest &req, FilterConfigResponse &resp) const; void handle_filterRequest(const FilterRequest &req, FilterResponse &resp) const; @@ -94,7 +109,7 @@ class TemperatureSamplerOrchestrator { class TemperatureSamplerThread : public ZephyrThread { public: - TemperatureSamplerThread(TStackBase &stack) : ZephyrThread(stack, 10, 0, "TemperatureSampler"){}; + TemperatureSamplerThread(TStackBase &stack) : ZephyrThread(stack, 15, 0, "TemperatureSampler"){}; int do_hardwarenInit() override; void threadMain() override; }; diff --git a/rims_app/src/zephyr.cpp b/rims_app/src/zephyr.cpp index dcbe1a48b7b..4447e5dfb81 100644 --- a/rims_app/src/zephyr.cpp +++ b/rims_app/src/zephyr.cpp @@ -4,7 +4,8 @@ #include void zephyr::gpio::pin_configure(const gpio_dt_spec &pin, gpio_flags_t flags, std::source_location sl) { - if (gpio_pin_configure_dt(&pin, flags) != 0) { + auto status = gpio_pin_configure_dt(&pin, flags); + if (status != 0) { rims::Log{sl}.error("%s for %s:%d failed","gpio_pin_configure_dt", pin.port->name, pin.pin); throw gpio_pin_configure_error{}; } else { diff --git a/rims_app/src/zephyr.hpp b/rims_app/src/zephyr.hpp index 9d4fda74eb9..fb495b28086 100644 --- a/rims_app/src/zephyr.hpp +++ b/rims_app/src/zephyr.hpp @@ -24,27 +24,37 @@ constexpr static k_timeout_t chronoToKTimeout(std::chrono::nanoseconds duration) return K_NSEC(duration.count()); } -namespace zephyr::gpio{ -struct error : public std::exception{ -}; +namespace zephyr::gpio { +struct error : public std::exception {}; -struct io_error : public error{ -}; +struct io_error : public error {}; -inline void pin_set_dt(const gpio_dt_spec &spec, int value){ - if(auto ret = gpio_pin_set_dt(&spec, value); ret!=0){ +inline void pin_set_dt(const gpio_dt_spec &spec, int value) { + if (auto ret = gpio_pin_set_dt(&spec, value); ret != 0) { throw io_error{}; } } -inline void pin_toggle_dt(const gpio_dt_spec &spec){ - if(auto ret = gpio_pin_toggle_dt(&spec); ret!=0){ +inline void pin_toggle_dt(const gpio_dt_spec &spec) { + if (auto ret = gpio_pin_toggle_dt(&spec); ret != 0) { throw io_error{}; } } +inline void pin_set_low(const gpio_dt_spec &spec) { + if (auto ret = gpio_pin_set_dt(&spec, 0); ret != 0) { + throw io_error{}; + } } +inline void pin_set_high(const gpio_dt_spec &spec) { + if (auto ret = gpio_pin_set_dt(&spec, 1); ret != 0) { + throw io_error{}; + } +} + +} // namespace zephyr::gpio + namespace zephyr::crc { inline uint32_t crc32_ieee(std::span data) { @@ -69,7 +79,6 @@ inline void k_init(k_poll_event &event, k_fifo &obj) { ::k_poll_event_init(&event, K_POLL_TYPE_FIFO_DATA_AVAILABLE, K_POLL_MODE_NOTIFY_ONLY, as_voidptr(obj)); } - inline int k_poll(std::span events, std::chrono::nanoseconds timeout = std::chrono::nanoseconds{0}) { return ::k_poll(events.data(), events.size(), chronoToKTimeout(timeout)); } @@ -102,7 +111,7 @@ template auto k_poll_handle(k_poll_event &event, Fn &&fn) { } // namespace zephyr::event_pool -namespace zephyr::semaphore{ +namespace zephyr::semaphore { struct sem : public ::k_sem { sem(unsigned int initial_count, unsigned int limit) { @@ -110,11 +119,11 @@ struct sem : public ::k_sem { } }; -inline int k_sem_take_now(sem &sem){ +inline int k_sem_take_now(sem &sem) { return ::k_sem_take(&sem, K_NO_WAIT); } -} +} // namespace zephyr::semaphore namespace zephyr::gpio { diff --git a/rims_app/src/zero_cross_detection.cpp b/rims_app/src/zero_cross_detection.cpp index d47838e66bf..e7d9329da32 100644 --- a/rims_app/src/zero_cross_detection.cpp +++ b/rims_app/src/zero_cross_detection.cpp @@ -13,8 +13,8 @@ #include namespace rims { -gpio_dt_spec ch1ZCD_pin_spec = GPIO_DT_SPEC_GET(DT_NODELABEL(zcd_state_1), gpios); -gpio_dt_spec ch2ZCD_pin_spec = GPIO_DT_SPEC_GET(DT_NODELABEL(zcd_state_2), gpios); +gpio_dt_spec ch1ZCD_pin_spec = GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_zcd), gpios); +gpio_dt_spec ch2ZCD_pin_spec = GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_zcd), gpios); static int gpio_pin_mask_to_index(uint32_t pin_mask) { std::bitset<32> bits{pin_mask}; @@ -43,7 +43,7 @@ void ZeroCrossDetection::interrupt_handler(const device *dev, struct gpio_callba ZeroCrossDetection::ZeroCrossDetection(gpio_dt_spec *gpio, ZCDFifo_t *queue, uint8_t channel) : _gpio{gpio}, _channel{channel}, _msgQueue{queue}, _intCheckSem{0, 1}, _intCheckTimer{_intCheckSem, std::chrono::seconds{1}} { ULOG_INFO("zero_cross_detection_entrypoint"); - + _intCheckTimer.start(); if (gpio_pin_interrupt_configure_dt(_gpio, GPIO_INT_EDGE_BOTH) != 0) { @@ -59,7 +59,7 @@ ZeroCrossDetection::ZeroCrossDetection(gpio_dt_spec *gpio, ZCDFifo_t *queue, uin } else { ULOG_INFO("%s pin %d@%s ok", "gpio_add_callback", _gpio->pin, gpio->port->name); } - + ULOG_INFO("zero_cross_detection_entrypoint DONE"); } @@ -79,8 +79,8 @@ void ZeroCrossDetectionOrchestrator::loop() { // std::this_thread::sleep_for(std::chrono::seconds{1}); auto ret = zephyr::event_pool::k_poll_forever(_events); if (ret == 0) { - zephyr::event_pool::k_poll_handle(_events.at(0), [&](){ event_zcdCheck(_zcd.at(0));} ); - zephyr::event_pool::k_poll_handle(_events.at(1), [&](){ event_zcdCheck(_zcd.at(1));} ); + zephyr::event_pool::k_poll_handle(_events.at(0), [&]() { event_zcdCheck(_zcd.at(0)); }); + zephyr::event_pool::k_poll_handle(_events.at(1), [&]() { event_zcdCheck(_zcd.at(1)); }); } } } -- 2.45.2 From 31ecbc1bda17dc0da89edb595327b1f714f5ce94 Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Tue, 22 Apr 2025 11:25:15 +0200 Subject: [PATCH 06/18] Fix uart logging issues --- rims_app/prj.conf | 2 +- rims_app/src/circular_buffer.hpp | 585 +++++++++++++--------- rims_app/src/main.cpp | 4 +- rims_app/src/temperature_measurements.cpp | 16 +- rims_app/src/temperature_measurements.hpp | 4 +- rims_app/src/uart.cpp | 84 ++-- rims_app/src/uart.hpp | 48 +- 7 files changed, 443 insertions(+), 300 deletions(-) diff --git a/rims_app/prj.conf b/rims_app/prj.conf index dce83b24e2c..18e701bf896 100644 --- a/rims_app/prj.conf +++ b/rims_app/prj.conf @@ -50,7 +50,7 @@ CONFIG_LOG_BACKEND_UART=n # Use UART for log output CONFIG_MAIN_STACK_SIZE=1500 # CONFIG_CRC=y -CONFIG_ASSERT=n +CONFIG_ASSERT=y #CONFIG_NUM_PREEMPT_PRIORITIES=0 diff --git a/rims_app/src/circular_buffer.hpp b/rims_app/src/circular_buffer.hpp index d3a9d8af05e..9b99345a7d8 100644 --- a/rims_app/src/circular_buffer.hpp +++ b/rims_app/src/circular_buffer.hpp @@ -1,8 +1,8 @@ #pragma once #include "zephyr.hpp" -#include "zephyr/sys/__assert.h" #include +#include #include #include @@ -11,17 +11,6 @@ namespace rims { -// template class circular_buffer_base { -// public: -// virtual ~circular_buffer_base() = default; - -// virtual const T &back() const = 0; -// virtual T &back() = 0; - -// virtual T &emplace_back(T &&item) = 0; -// virtual void pop_back() = 0; -// }; - template class static_vector { private: std::array buffer; @@ -32,7 +21,7 @@ template class static_vector { return count; } - constexpr void clean() noexcept { + constexpr void clean() noexcept { /// TODO call dtors count = 0; } @@ -60,308 +49,432 @@ template class static_vector { } }; -template class circular_buffer { +struct RingIndex { + using index_t = uint16_t; + + constexpr RingIndex(index_t n) : N{n} { + } + + constexpr void reset() { + _head = 0; + _tail = 0; + _full = false; + } + + constexpr bool empty() const { + return (!_full && _head == _tail); + } + + constexpr bool full() const { + return _full; + } + + constexpr std::size_t size() const { + if (full()) { + return N; + } + if (_head >= _tail) { + return _head - _tail; + } else { + return N + _head - _tail; + } + } + + constexpr std::size_t capacity() const { + return N; + } + + constexpr std::size_t free() const { + return capacity() - size(); + } + + constexpr index_t increment_head() { + __ASSERT(not full(), "Cannot increment head on full buffer"); + return increment_head(1); + } + + constexpr index_t increment_head(index_t n) { + __ASSERT(free() >= n, "Cannot increment head more than free space"); + _head = (_head + n) % N; + _full = _head == _tail; + return prev_head(); + } + + constexpr index_t decrement_head() { + __ASSERT(not empty(), "Cannot decrement head on empty buffer"); + _head = (_head + N - 1) % N; + _full = false; + return _head; + } + + constexpr index_t increment_tail() { + __ASSERT(!empty(), "Cannot increment tail on empty buffer"); + return increment_tail(1); + } + + constexpr index_t increment_tail(index_t n) { + __ASSERT(size() >= n, "Tail cannot pass head"); + _tail = (_tail + n) % N; + _full = false; + return prev_tail(); + } + + constexpr index_t head() const { + return _head; + } + + constexpr index_t tail() const { + return _tail; + } + + constexpr index_t firstFree() const { + __ASSERT(!full(), "Buffer is full"); + return head(); + } + + constexpr bool headAfterTail() const { + return head() >= tail(); + } + constexpr std::size_t spaceBack() const { + if (headAfterTail()) { + return capacity() - head(); + } else { + if (spaceMid() == 0) { + return capacity() - tail() - 1; + } + } + return 0; + } + + constexpr std::size_t spaceFront() const { + if (headAfterTail()) { + return tail(); + } else { + if (spaceMid() == 0) { + return head(); + } + } + return 0; + } + + constexpr std::size_t spaceMid() const { + const auto head2Tail = tail() - head(); + return head2Tail > 0 ? head2Tail : 0; + } + + using area = std::pair; + constexpr std::pair getFreeAreas() const { + __ASSERT((spaceFront() + spaceBack() + spaceMid()) == free(), "check that capacity always matches"); + + const std::size_t capacityMid = spaceMid(); + if (capacityMid) { + return {{head(), capacityMid}, {0, 0}}; + } else { + return {{head(), spaceBack()}, {0, spaceFront()}}; + } + } + + private: + constexpr index_t prev_tail() const { + return _tail == 0 ? N - 1 : _tail - 1; + } + constexpr index_t prev_head() const { + return _head == 0 ? N - 1 : _head - 1; + } + + index_t N{0}; + index_t _head{0}; + index_t _tail{0}; + bool _full{false}; +}; + +template class ring_buffer { public: - explicit circular_buffer() = default; + using value_type = T; + explicit ring_buffer() : _index{N} { + } - T &emplace_front(const T &item) { - if (size() == capacity()) { - std::destroy_at(std::addressof(buf_[head_])); - std::memset(std::addressof(buf_[head_]), 0, sizeof(T)); - } - auto at = head_; - std::construct_at(reinterpret_cast(std::addressof(buf_[head_])), item); - - if (full_) { - tail_ = (tail_ + 1) % N; + T &push_back(const T &item) { + if (full()) { + pop_front(); } - head_ = (head_ + 1) % N; - full_ = head_ == tail_; + auto at = _index.head(); + std::construct_at(reinterpret_cast(std::addressof(buf_[_index.increment_head()])), item); + return directy_at(at); } + void put_n(const T *items, std::size_t n) { + static_assert(std::is_trivially_constructible_v); + __ASSERT(_index.free() >= n, "need to have space for all items"); + + auto areas = _index.getFreeAreas(); + + __ASSERT((areas.first.second + areas.second.second) >= n, "areas should contains enaough free space to fit all items"); + + std::memcpy(buf_[areas.first.first], items, areas.first.second * sizeof(T)); + if (areas.first.second <= n) std::memcpy(buf_[areas.second.first], items + areas.first.second, areas.second.second * sizeof(T)); + + _index.increment_head(n); + } + T get() { __ASSERT_NO_MSG(not empty()); - // Read data and advance the tail (we now have a free space) - T val = std::move(*reinterpret_cast(std::addressof(buf_[tail_]))); - std::destroy_at(std::addressof(buf_[tail_])); - std::memset(std::addressof(buf_[tail_]), 0, sizeof(T)); - tail_ = (tail_ + 1) % N; - full_ = false; + // Read data and advance the tail (we now have a free space) + T val = std::move(*reinterpret_cast(std::addressof(buf_[_index.tail()]))); + destroy_at(_index.increment_tail()); + return val; } + // removes the last (youngest) element void pop_back() { - std::destroy_at(std::addressof(buf_[tail_])); - std::memset(std::addressof(buf_[tail_]), 0, sizeof(T)); - tail_ = (tail_ + 1) % N; - full_ = false; + __ASSERT_NO_MSG(not empty()); + + destroy_at(_index.decrement_head()); } + // removes the first (oldest) element void pop_front() { - std::destroy_at(std::addressof(buf_[head_])); - std::memset(std::addressof(buf_[head_]), 0, sizeof(T)); - head_ = (head_ - 1) % N; - full_ = false; + __ASSERT_NO_MSG(not empty()); + + destroy_at(_index.increment_tail()); } + // oldest element const T &front() const { - assert(!empty()); - return *begin(); + __ASSERT_NO_MSG(not empty()); + + return *cbegin(); } T &front() { - int i = head_ - 1; - if (i == -1) i = N - 1; + __ASSERT_NO_MSG(not empty()); + return *begin(); } const T &back() const { - int i = head_ - 1; - if (i == -1) i = N - 1; - return directy_at(i); + __ASSERT_NO_MSG(not empty()); + + return *std::prev(cend()); } T &back() { - int i = head_ - 1; - if (i == -1) i = N - 1; - return directy_at(i); + __ASSERT_NO_MSG(not empty()); + + return *(std::prev(end())); } constexpr void reset() { - head_ = tail_; - full_ = false; + // destroy elements? :| + _index.reset(); } - bool empty() const { - return (!full() && (head_ == tail_)); + constexpr bool empty() const { + return _index.empty(); } - bool full() const { - return full_; + constexpr bool full() const { + return _index.full(); } - std::size_t capacity() const { - return N; + constexpr std::size_t capacity() const { + return _index.capacity(); } - std::size_t size() const { - std::size_t size = N; - - if (!full()) { - if (head_ >= tail_) { - size = head_ - tail_; - } else { - size = N + head_ - tail_; - } - } - - return size; + constexpr std::size_t size() const { + return _index.size(); } - // Iterator class for circular_buffer - class iterator { + constexpr std::size_t free() const { + return _index.free(); + } + + // Iterator class for ring_buffer + template class iterator_base { public: - using iterator_category = std::forward_iterator_tag; + using iterator_category = std::random_access_iterator_tag; using value_type = T; using difference_type = std::ptrdiff_t; - using pointer = T *; - using reference = T &; + using pointer = std::conditional_t; + using reference = std::conditional_t; - iterator(std::array &buf, std::size_t index, std::size_t tail, std::size_t head) - : buf_(buf), index_(index), tail_(tail), head_(head), looped_(false) { + using iterator_t = iterator_base; + using buf_t = std::conditional_t, std::array>; + + constexpr iterator_base() = default; + constexpr iterator_base(buf_t &buf, std::size_t index, std::size_t tail, std::size_t head) + : buf_(&buf), current_(index), tail_(tail), head_(head), looped_(false) { } - reference operator*() { - return *reinterpret_cast(std::addressof(buf_[index_])); - } - - pointer operator->() { - return reinterpret_cast(std::addressof(buf_[index_])); - } - - iterator &operator++() { - index_ = (index_ + 1) % N; - // Detect if we have looped over the circular buffer - if (index_ == tail_ && looped_) { - index_ = head_; // Move iterator to end - } - if (index_ == head_) { - looped_ = true; - } - return *this; - } - - iterator operator++(int) { - iterator tmp = *this; - ++(*this); - return tmp; - } - - iterator operator+(difference_type n) const { - std::size_t new_index = (index_ + n) % N; - return iterator(buf_, new_index, tail_, head_); - } - - iterator operator-(difference_type n) const { - std::size_t new_index = (index_ + N - (n % N)) % N; - return iterator(buf_, new_index, tail_, head_); - } - - iterator& operator+=(difference_type n) { - index_ = (index_ + n) % N; - return *this; - } - - iterator& operator-=(difference_type n) { - index_ = (index_ + N - (n % N)) % N; - return *this; - } - - reference operator[](difference_type n) { - std::size_t idx = (index_ + n) % N; - return *reinterpret_cast(std::addressof(buf_[idx])); - } - - bool operator<(const iterator& other) const { - return (*this - other) < 0; - } - bool operator>(const iterator& other) const { - return other < *this; - } - bool operator<=(const iterator& other) const { - return !(*this > other); - } - bool operator>=(const iterator& other) const { - return !(*this < other); - } - - bool operator==(const iterator &other) const { - return index_ == other.index_ && looped_; - } - - bool operator!=(const iterator &other) const { - return !(*this == other); - } - - private: - std::array &buf_; - std::size_t index_; - const std::size_t tail_; - const std::size_t head_; - bool looped_; - }; - - class const_iterator { - public: - using iterator_category = std::forward_iterator_tag; - using value_type = T; - using difference_type = std::ptrdiff_t; - using pointer = const T *; - using reference = const T &; - - const_iterator(const std::array &buf, std::size_t index, std::size_t tail, std::size_t head) - : buf_(buf), index_(index), tail_(tail), head_(head), looped_(false) { - } + constexpr iterator_base(const iterator_base &rhs) = default; + constexpr iterator_base(iterator_base &&rhs) = default; + constexpr iterator_base &operator=(const iterator_base &rhs) = default; + constexpr iterator_base &operator=(iterator_base &&rhs) = default; reference operator*() const { - return *reinterpret_cast(std::addressof(buf_[index_])); + return *reinterpret_cast(std::addressof((*buf_)[current_])); } pointer operator->() const { - return reinterpret_cast(std::addressof(buf_[index_])); + return std::addressof(operator*()); } - const_iterator &operator++() { - index_ = (index_ + 1) % N; + constexpr iterator_t &operator++() { + current_ = (current_ + 1) % N; // Detect if we have looped over the circular buffer - if (index_ == tail_ && looped_) { - index_ = head_; // Move iterator to end + if (current_ == tail_ && looped_) { + current_ = head_; // Move iterator to end } - if (index_ == head_) { + if (current_ == head_) { looped_ = true; } return *this; } - const_iterator operator++(int) { - iterator tmp = *this; + constexpr iterator_t operator++(int) { + iterator_t tmp = *this; ++(*this); return tmp; } - - const_iterator operator+(difference_type n) const { - std::size_t new_index = (index_ + n) % N; - return {buf_, new_index, tail_, head_}; - } - - const_iterator operator-(difference_type n) const { - std::size_t new_index = (index_ + N - (n % N)) % N; - return {buf_, new_index, tail_, head_}; - } - - const_iterator& operator+=(difference_type n) { - index_ = (index_ + n) % N; + + constexpr iterator_t &operator--() { + if (current_ == 0) current_ = N - 1; + else current_--; + + // Detect if we have looped over the circular buffer + if (current_ == head_ && looped_) { + current_ = tail_; // Move iterator to end + } + if (current_ == tail_) { + looped_ = true; + } return *this; } - - const_iterator& operator-=(difference_type n) { - index_ = (index_ + N - (n % N)) % N; + + constexpr iterator_t operator--(int) { + iterator_t tmp = *this; + --(*this); + return tmp; + } + + constexpr bool operator==(const iterator_t &other) const { + return current_ == other.current_ && looped_; + } + + constexpr iterator_t &operator+=(difference_type n) { + current_ = (current_ + n) % N; return *this; } - - reference operator[](difference_type n) { - std::size_t idx = (index_ + n) % N; - return *reinterpret_cast(std::addressof(buf_[idx])); - } - - bool operator<(const iterator& other) const { - return (*this - other) < 0; - } - bool operator>(const iterator& other) const { - return other < *this; - } - bool operator<=(const iterator& other) const { - return !(*this > other); - } - bool operator>=(const iterator& other) const { - return !(*this < other); + + constexpr iterator_t &operator+=(const iterator_t &n) { + current_ = (current_ + n.current_) % N; + return *this; } - bool operator==(const const_iterator &other) const { - return index_ == other.index_ && looped_; + constexpr iterator_t operator+(difference_type n) const { + std::size_t new_index = (current_ + n) % N; + return iterator_t(*buf_, new_index, tail_, head_); } - bool operator!=(const const_iterator &other) const { - return !(*this == other); + constexpr friend iterator_t operator+(iterator_t it, const iterator_t &n) { + it += n; + return it; } + constexpr friend iterator_t operator+(difference_type it, const iterator_t &n) { + iterator_t tmp = n; + tmp += it; + return tmp; + } + + constexpr iterator_t &operator-=(difference_type n) { + current_ = (current_ + N - (n % N)) % N; + return *this; + } + + constexpr friend iterator_t operator-(const iterator_t &lhs, difference_type n) { + iterator_t tmp = lhs; + tmp -= n; + return tmp; + } + + constexpr friend difference_type operator-(const iterator_t &lhs, const iterator_t &rhs) { + if (lhs.current_ >= rhs.current_) { + return static_cast(lhs.current_ - rhs.current_); + } else { + return static_cast(N + lhs.current_ - rhs.current_); + } + } + + constexpr reference operator[](difference_type n) const { + return *(*this + n); + } + + // constexpr bool operator<(const iterator_t & other) const { + + // return (*this - other) < 0; + // } + // constexpr bool operator>(const iterator_t & other) const { + // return other < *this; + // } + // constexpr bool operator<=(const iterator_t & other) const { + // return !(*this > other); + // } + // constexpr bool operator>=(const iterator_t & other) const { + // return !(*this < other); + // } + // constexpr bool operator!=(const iterator_t & other) const { + // return !(*this == other); + // } + private: - const std::array &buf_; - std::size_t index_; - const std::size_t tail_; - const std::size_t head_; - bool looped_; + buf_t *buf_{nullptr}; + std::size_t current_{0}; + + std::size_t tail_{0}; + std::size_t head_{0}; + + bool looped_{false}; }; + using iterator = iterator_base; + using const_iterator = iterator_base; + + static_assert(std::bidirectional_iterator); + // static_assert(std::random_access_iterator< iterator >); + static_assert(std::bidirectional_iterator); + // static_assert(std::random_access_iterator< const_iterator >); + // Begin and end functions to return iterator iterator begin() { - return iterator(buf_, tail_, tail_, head_); + return {buf_, _index.tail(), _index.tail(), _index.head()}; } - - iterator end() { - return iterator(buf_, head_, tail_, head_); + const_iterator begin() const { + return {buf_, _index.tail(), _index.tail(), _index.head()}; } const_iterator cbegin() const { - return const_iterator(buf_, tail_, tail_, head_); + return {buf_, _index.tail(), _index.tail(), _index.head()}; } + iterator end() { + return {buf_, _index.head(), _index.tail(), _index.head()}; + } + const_iterator end() const { + return {buf_, _index.head(), _index.tail(), _index.head()}; + } const_iterator cend() const { - return const_iterator(buf_, head_, tail_, head_); + return {buf_, _index.head(), _index.tail(), _index.head()}; } private: + void destroy_at(int index) { + std::destroy_at(std::addressof(buf_[index])); + std::memset(std::addressof(buf_[index]), 0, sizeof(T)); + } + const T &directy_at(int index) const { return *reinterpret_cast(std::addressof(buf_[index])); } @@ -369,13 +482,9 @@ template class circular_buffer { return *reinterpret_cast(std::addressof(buf_[index])); } - // std::mutex mutex_; std::array buf_; - std::size_t head_ = 0; - std::size_t tail_ = 0; - bool full_{false}; + RingIndex _index; }; - class ZephyrMutex { public: ZephyrMutex() { @@ -402,8 +511,8 @@ class ZephyrMutex { ZephyrMutex &operator=(const ZephyrMutex &) = delete; }; -template class thread_safe_circular_buffer : public circular_buffer { - using base = circular_buffer; +template class thread_safe_circular_buffer : public ring_buffer { + using base = ring_buffer; public: const T &back() const { @@ -455,9 +564,9 @@ template class zephyr_fifo_buffer { try { if (not el) return false; // should be a assert fn(el->item); // consume item, fn can throw - _elements.pop_back(); // clear item from queue + _elements.pop_front(); // clear first item from queue } catch (...) { - _elements.pop_back(); + _elements.pop_front(); throw; } @@ -472,7 +581,7 @@ template class zephyr_fifo_buffer { auto tmp = ZephyrFifoElement{}; if (fn(tmp.item)) // fill new data { - auto &el = _elements.emplace_front(std::move(tmp)); + auto &el = _elements.push_back(std::move(tmp)); k_fifo_put(&_fifo, &el); // put data into a queue } @@ -481,8 +590,8 @@ template class zephyr_fifo_buffer { private: // ZephyrMutex _mutex{}; - circular_buffer, N> _elements{}; - k_fifo &_fifo; + ring_buffer, N> _elements{}; + k_fifo &_fifo; }; } // namespace rims diff --git a/rims_app/src/main.cpp b/rims_app/src/main.cpp index 6b785b435a0..657d2d38b48 100644 --- a/rims_app/src/main.cpp +++ b/rims_app/src/main.cpp @@ -29,7 +29,7 @@ TStack messengerStack{k_messengerStack, K_THREAD_STACK_SIZEOF(k_messengerStack)} static K_THREAD_STACK_DEFINE(k_uartStack, 2048); TStack uartStack{k_uartStack, K_THREAD_STACK_SIZEOF(k_uartStack)}; -static K_THREAD_STACK_DEFINE(k_temperatureSamplerStack, 4096); +static K_THREAD_STACK_DEFINE(k_temperatureSamplerStack, 2048); TStack temperatureSamplerStack{k_temperatureSamplerStack, K_THREAD_STACK_SIZEOF(k_temperatureSamplerStack)}; static K_THREAD_STACK_DEFINE(k_zeroCrossDetectionStack, 2048); @@ -105,7 +105,7 @@ int main() { zephyr::gpio::pin_configure(gprelay_2_en, GPIO_OUTPUT_INACTIVE); while (1) { - std::this_thread::sleep_for(std::chrono::seconds{5}); + std::this_thread::sleep_for(std::chrono::seconds{1}); // zephyr::gpio::pin_toggle_dt(gprelay_2_en); // const unsigned char data[] = {'b', 0x55}; diff --git a/rims_app/src/temperature_measurements.cpp b/rims_app/src/temperature_measurements.cpp index b645701a8ae..09d8513854b 100644 --- a/rims_app/src/temperature_measurements.cpp +++ b/rims_app/src/temperature_measurements.cpp @@ -20,6 +20,8 @@ #include #include "zephyr.hpp" +#include "zephyr/device.h" +#include "zephyr/sys/__assert.h" #define ADC_NODE DT_NODELABEL(adc1) #define DT_ADC_TEMP_NODELABEL DT_NODELABEL(adc_temp) @@ -94,7 +96,7 @@ template void PB_encode_egress(const T &tempresp) { void TemperatureSampler::take_sample() { ULOG_DEBUG("Samples on ch %d, size %d", this->_channel, this->_samples.size()); - _samples.emplace_front(adc_take_sample()); + _samples.push_back(adc_take_sample()); } TemperatureSampler::TemperatureSampler(uint8_t channel) @@ -424,7 +426,9 @@ void TemperatureSamplerOrchestrator::action_sendTemperature(uint8_t ch) { int TemperatureSamplerThread::do_hardwarenInit() { const device *adc = DEVICE_DT_GET(ADC_NODE); const gpio_dt_spec chsel_pin_dev = GPIO_DT_SPEC_GET(DT_NODELABEL(temp_channel_sel), gpios); - + + __ASSERT(device_is_ready(adc), "adc needs to work"); + zephyr::gpio::pin_configure(chsel_pin_dev, GPIO_OUTPUT_INACTIVE); if (!device_is_ready(adc)) { @@ -451,12 +455,12 @@ int TemperatureSamplerThread::do_hardwarenInit() { }; const adc_channel_cfg adc_temp = ADC_CHANNEL_CFG_DT(DT_NODELABEL(adc_temp)); - // adc_channel_cfg adc_current_ch1 = ADC_CHANNEL_CFG_DT(DT_NODELABEL(adc_current_ch1)); - // adc_channel_cfg adc_current_ch2 = ADC_CHANNEL_CFG_DT(DT_NODELABEL(adc_current_ch2)); + adc_channel_cfg adc_current_ch1 = ADC_CHANNEL_CFG_DT(DT_NODELABEL(adc_current_ch1)); + adc_channel_cfg adc_current_ch2 = ADC_CHANNEL_CFG_DT(DT_NODELABEL(adc_current_ch2)); auto ret = configurechannel(adc_temp); - // configurechannel(adc_current_ch1); - // configurechannel(adc_current_ch2); + configurechannel(adc_current_ch1); + configurechannel(adc_current_ch2); zephyr::gpio::pin_configure(rmax_en_gpio_dt, GPIO_OUTPUT_INACTIVE); zephyr::gpio::pin_configure(rmin_en_gpio_dt, GPIO_OUTPUT_INACTIVE); diff --git a/rims_app/src/temperature_measurements.hpp b/rims_app/src/temperature_measurements.hpp index 2c2dde405a0..29410feaf80 100644 --- a/rims_app/src/temperature_measurements.hpp +++ b/rims_app/src/temperature_measurements.hpp @@ -70,7 +70,7 @@ class TemperatureSampler { // zephyr::semaphore::sem _samplerSem{0, 1}; // RecurringSemaphoreTimer _samplerTimer; - circular_buffer _samples; + ring_buffer _samples; std::size_t _samplesNumber{MaxSampleSize}; const std::uint8_t _channel{}; @@ -109,7 +109,7 @@ class TemperatureSamplerOrchestrator { class TemperatureSamplerThread : public ZephyrThread { public: - TemperatureSamplerThread(TStackBase &stack) : ZephyrThread(stack, 15, 0, "TemperatureSampler"){}; + TemperatureSamplerThread(TStackBase &stack) : ZephyrThread(stack, 14, 0, "TemperatureSampler"){}; int do_hardwarenInit() override; void threadMain() override; }; diff --git a/rims_app/src/uart.cpp b/rims_app/src/uart.cpp index a8fbf570f60..f5650bac90d 100644 --- a/rims_app/src/uart.cpp +++ b/rims_app/src/uart.cpp @@ -3,6 +3,10 @@ #include "log.hpp" #include "messenger.hpp" #include "syscalls/uart.h" +#include "zephyr/device.h" +#include "zephyr/irq.h" +#include "zephyr/spinlock.h" +#include "zephyr/sys/__assert.h" #include #include @@ -79,26 +83,27 @@ void AsyncUART::loop() { } } +static std::size_t buffer_copy_wait {}; + // free function, only need to copy data to uart's TX_BUFFER and that's it void AsyncUART::transmit(AsyncUART *dev, std::span bytes) { if (bytes.empty()) return; + __ASSERT(bytes.size_bytes() <= dev->tx_buffer.capacity(), "for now, all bytes needs to fir in tx buffer"); - bool first = true; - for (auto byte : bytes) { - if (first) { - if (dev->tx_buffer.empty()) { - dev->tx_buffer.emplace_front(byte); - uart_irq_tx_enable(dev->_dev); // enable interrupt - continue; - } - first = false; - } - - while (dev->tx_buffer.full()) { - std::this_thread::sleep_for(std::chrono::microseconds{12}); - }; - dev->tx_buffer.emplace_front(byte); + while (dev->tx_buffer.free() < bytes.size_bytes()) { + buffer_copy_wait++; + std::this_thread::sleep_for(std::chrono::microseconds{20}); } + + k_spinlock_key_t key = k_spin_lock(&dev->tx_lock); + + __ASSERT(dev->no_copyInProgress(), "multiple copies at the same time are not allowed"); + // copy all data to TX + dev->tx_buffer.put_n(bytes.data(), bytes.size()); + // if disabled, enable tx interrupts + if (not dev->tx_irq_enabled()) dev->tx_irq_enable(); + + k_spin_unlock(&dev->tx_lock, key); } void AsyncUART::uartCallback(const device *dev, void *user_data) { @@ -107,13 +112,14 @@ void AsyncUART::uartCallback(const device *dev, void *user_data) { } void AsyncUART::uartISR() { + __ASSERT(device_is_ready(_dev), "device needs to work"); try { while (uart_irq_update(_dev) && uart_irq_is_pending(_dev)) { - if (rxHasByte()) { + if (uart_irq_rx_ready(_dev)) { readByteUart(); } if (uart_irq_tx_ready(_dev)) { - putByteUart(); + writeByteUart(); } } @@ -128,21 +134,21 @@ void AsyncUART::uartISR() { } } -bool AsyncUART::rxHasByte() const { - return uart_irq_rx_ready(_dev); -} - void AsyncUART::readByteUart() { - if (faultFlag) { - if (rxByte() == 0) faultFlag = false; + if (_faultFlag) { + if (rxByte() == 0) _faultFlag = false; } // throw on buffer overflow - else if (tx_buffer.full()) { + else if (rxBuffer().full()) { throw uart_rx_buffer_overflow{}; } // push_back returns last placed byte, if the byte is 0x00 we got end of frame else if (rxBuffer().push_back(rxByte()) == 0) { - processMessage(); + if(rxBuffer().size()>1){ + processMessage(); + }else{ + rxBuffer().clean(); + } } } @@ -175,24 +181,20 @@ void AsyncUART::switchRxBuffer() { } bool AsyncUART::txHasByte() const { - return tx_buffer.size(); + return not tx_buffer.empty(); } -bool AsyncUART::txByte(uint8_t byte) { - auto send_len = uart_fifo_fill(_dev, &byte, 1); - if (send_len != 1) { - // LOG_ERR("Drop %d bytes", rb_len - send_len); - while (1) { - /// just loop here - } +void AsyncUART::txByte(uint8_t byte) { + [[maybe_unused]] auto send_len = uart_fifo_fill(_dev, &byte, 1); + __ASSERT(send_len == 1, "fifo fill has to work as expected"); +} + +void AsyncUART::writeByteUart() { + if (txHasByte()) { + txByte(tx_buffer.get()); + } else { + tx_irq_disable(); } - return false; -} - -void AsyncUART::putByteUart() { - bool hasData = txHasByte(); - if (hasData) txByte(tx_buffer.get()); - else uart_irq_tx_disable(_dev); } void AsyncUART::handleRxBufferOverflowError(const uart_rx_buffer_overflow &e) { @@ -205,7 +207,7 @@ void AsyncUART::handleRxBufferOverflowError(const uart_rx_buffer_overflow &e) { rxBuffer().clean(); // indicate the driver to skip bytes until end of frame - faultFlag = true; + _faultFlag = true; } void AsyncUART::handleRxNotReadyError(const uart_rx_not_ready_error &e) { diff --git a/rims_app/src/uart.hpp b/rims_app/src/uart.hpp index 7fa32c04cfb..7559f80c461 100644 --- a/rims_app/src/uart.hpp +++ b/rims_app/src/uart.hpp @@ -19,27 +19,55 @@ class uart_rx_not_ready_error; class AsyncUART { using rx_buffer_t = static_vector; - using tx_buffer_t = circular_buffer; + using tx_buffer_t = ring_buffer; public: AsyncUART(); void loop(); - + static void transmit(AsyncUART *dev, std::span bytes); static void workHandler(k_work *work); static void uartCallback(const struct device *dev, void *user_data); - - void uartISR(); - const device *_dev; + void uartISR(); + + bool tx_irq_enabled() const { + return _txIrqEnabled; + } + void tx_irq_enable() { + _txIrqEnabled = true; + uart_irq_tx_enable(_dev); + } + void tx_irq_disable() { + _txIrqEnabled = false; + uart_irq_tx_disable(_dev); + } + + void beginCopy() { + _copyInProgress = true; + } + void endCopy() { + _copyInProgress = false; + } + constexpr bool copyInProgress() const { + return _copyInProgress; + } + constexpr bool no_copyInProgress() const { + return not _copyInProgress; + } + + const device *_dev; private: std::array rx_buffers; uint8_t _currentBufferIndex{}; - bool faultFlag = false; - tx_buffer_t tx_buffer; + bool _faultFlag = false; + bool _copyInProgress = false; + bool _txIrqEnabled = false; + + k_spinlock tx_lock; + tx_buffer_t tx_buffer; // ISR RX part - inline bool rxHasByte() const; inline void readByteUart(); // process byte inline uint8_t rxByte(); // low level read byte from device inline void processMessage(); @@ -47,9 +75,9 @@ class AsyncUART { inline void switchRxBuffer(); // ISR TX part - inline void putByteUart(); + inline void writeByteUart(); inline bool txHasByte() const; - inline bool txByte(uint8_t byte); // low level write byte to device + inline void txByte(uint8_t byte); // low level write byte to device // exception handlers inline void handleRxNotReadyError(const uart_rx_not_ready_error &e); -- 2.45.2 From 76bdb965b47a60b01abfd3385490b45682f10165 Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Tue, 22 Apr 2025 12:09:17 +0200 Subject: [PATCH 07/18] refactor --- rims_app/prj.conf | 2 +- rims_app/src/temperature_measurements.cpp | 2 +- rims_app/src/uart.cpp | 61 ++++++++++++++++------- rims_app/src/uart.hpp | 53 ++++++++++---------- 4 files changed, 70 insertions(+), 48 deletions(-) diff --git a/rims_app/prj.conf b/rims_app/prj.conf index 18e701bf896..dce83b24e2c 100644 --- a/rims_app/prj.conf +++ b/rims_app/prj.conf @@ -50,7 +50,7 @@ CONFIG_LOG_BACKEND_UART=n # Use UART for log output CONFIG_MAIN_STACK_SIZE=1500 # CONFIG_CRC=y -CONFIG_ASSERT=y +CONFIG_ASSERT=n #CONFIG_NUM_PREEMPT_PRIORITIES=0 diff --git a/rims_app/src/temperature_measurements.cpp b/rims_app/src/temperature_measurements.cpp index 09d8513854b..d0c1c379244 100644 --- a/rims_app/src/temperature_measurements.cpp +++ b/rims_app/src/temperature_measurements.cpp @@ -95,7 +95,7 @@ template void PB_encode_egress(const T &tempresp) { } void TemperatureSampler::take_sample() { - ULOG_DEBUG("Samples on ch %d, size %d", this->_channel, this->_samples.size()); + // ULOG_DEBUG("Samples on ch %d, size %d", this->_channel, this->_samples.size()); _samples.push_back(adc_take_sample()); } diff --git a/rims_app/src/uart.cpp b/rims_app/src/uart.cpp index f5650bac90d..ef7e4f3f962 100644 --- a/rims_app/src/uart.cpp +++ b/rims_app/src/uart.cpp @@ -60,50 +60,73 @@ void log_worker(struct k_work *work) { } // Global log work item + static struct k_work_log_data uart_log_work; +#ifdef RIMS_UART_PERF_COUNTERS +static std::size_t buffer_copy_wait{}; +static std::size_t isr_execution{}; +static std::chrono::nanoseconds tx_total{}; +static std::chrono::nanoseconds tx_crit{}; +#endif + AsyncUART::AsyncUART() { _dev = DEVICE_DT_GET(DT_NODELABEL(usart1)); + __ASSERT(device_is_ready(_dev), "device needs to be ready"); - auto ret = uart_irq_callback_user_data_set(_dev, AsyncUART::uartCallback, this); - if (ret < 0) { - throw uart_not_ready_error{}; // catch this somewhere?? - } + [[maybe_unused]] auto ret = uart_irq_callback_user_data_set(_dev, AsyncUART::uartCallback, this); + __ASSERT(ret >= 0, ""); uart_irq_rx_enable(_dev); } void AsyncUART::loop() { - if (!device_is_ready(_dev)) { - /// TODO throw? - return; // Exit if the UART device is not ready - } + __ASSERT(device_is_ready(_dev), "device needs to be ready"); while (1) { - std::this_thread::sleep_for(std::chrono::seconds{2}); + std::this_thread::sleep_for(std::chrono::seconds{10}); +#ifdef RIMS_UART_PERF_COUNTERS + ULOG_DEBUG(R"log(buffer_copy_wait : %d)log", buffer_copy_wait); + ULOG_DEBUG(R"log(tx_total : %lldus)log", tx_total.count() / 1000); + ULOG_DEBUG(R"log(tx_critical : %lldus)log", tx_crit.count() / 1000); + ULOG_DEBUG(R"log(isr number : %zu)log", isr_execution); +#endif } } -static std::size_t buffer_copy_wait {}; - // free function, only need to copy data to uart's TX_BUFFER and that's it void AsyncUART::transmit(AsyncUART *dev, std::span bytes) { +#ifdef RIMS_UART_PERF_COUNTERS + auto isr_start = std::chrono::high_resolution_clock::now(); + isr_execution++; +#endif if (bytes.empty()) return; __ASSERT(bytes.size_bytes() <= dev->tx_buffer.capacity(), "for now, all bytes needs to fir in tx buffer"); while (dev->tx_buffer.free() < bytes.size_bytes()) { +#ifdef RIMS_UART_PERF_COUNTERS buffer_copy_wait++; +#endif std::this_thread::sleep_for(std::chrono::microseconds{20}); } - + +#ifdef RIMS_UART_PERF_COUNTERS + auto crit_start = std::chrono::high_resolution_clock::now(); +#endif k_spinlock_key_t key = k_spin_lock(&dev->tx_lock); - + __ASSERT(dev->no_copyInProgress(), "multiple copies at the same time are not allowed"); // copy all data to TX dev->tx_buffer.put_n(bytes.data(), bytes.size()); // if disabled, enable tx interrupts if (not dev->tx_irq_enabled()) dev->tx_irq_enable(); - + k_spin_unlock(&dev->tx_lock, key); +#ifdef RIMS_UART_PERF_COUNTERS + auto crit_stop = std::chrono::high_resolution_clock::now(); + auto isr_stop = std::chrono::high_resolution_clock::now(); + tx_total += isr_stop - isr_start; + tx_crit += crit_stop - crit_start; +#endif } void AsyncUART::uartCallback(const device *dev, void *user_data) { @@ -144,11 +167,11 @@ void AsyncUART::readByteUart() { } // push_back returns last placed byte, if the byte is 0x00 we got end of frame else if (rxBuffer().push_back(rxByte()) == 0) { - if(rxBuffer().size()>1){ - processMessage(); - }else{ - rxBuffer().clean(); - } + if (rxBuffer().size() > 1) { + processMessage(); + } else { + rxBuffer().clean(); + } } } diff --git a/rims_app/src/uart.hpp b/rims_app/src/uart.hpp index 7559f80c461..54969ad1184 100644 --- a/rims_app/src/uart.hpp +++ b/rims_app/src/uart.hpp @@ -26,38 +26,12 @@ class AsyncUART { void loop(); static void transmit(AsyncUART *dev, std::span bytes); - static void workHandler(k_work *work); static void uartCallback(const struct device *dev, void *user_data); void uartISR(); - bool tx_irq_enabled() const { - return _txIrqEnabled; - } - void tx_irq_enable() { - _txIrqEnabled = true; - uart_irq_tx_enable(_dev); - } - void tx_irq_disable() { - _txIrqEnabled = false; - uart_irq_tx_disable(_dev); - } - - void beginCopy() { - _copyInProgress = true; - } - void endCopy() { - _copyInProgress = false; - } - constexpr bool copyInProgress() const { - return _copyInProgress; - } - constexpr bool no_copyInProgress() const { - return not _copyInProgress; - } - - const device *_dev; private: + const device *_dev; std::array rx_buffers; uint8_t _currentBufferIndex{}; bool _faultFlag = false; @@ -79,6 +53,31 @@ class AsyncUART { inline bool txHasByte() const; inline void txByte(uint8_t byte); // low level write byte to device + constexpr bool tx_irq_enabled() const { + return _txIrqEnabled; + } + void tx_irq_enable() { + _txIrqEnabled = true; + uart_irq_tx_enable(_dev); + } + void tx_irq_disable() { + uart_irq_tx_disable(_dev); + _txIrqEnabled = false; + } + + void beginCopy() { + _copyInProgress = true; + } + void endCopy() { + _copyInProgress = false; + } + constexpr bool copyInProgress() const { + return _copyInProgress; + } + constexpr bool no_copyInProgress() const { + return not _copyInProgress; + } + // exception handlers inline void handleRxNotReadyError(const uart_rx_not_ready_error &e); inline void handleRxBufferOverflowError(const uart_rx_buffer_overflow &e); -- 2.45.2 From e637a8a272a032657ad06c47f71e92378ff8cbea Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Wed, 23 Apr 2025 12:28:47 +0200 Subject: [PATCH 08/18] refactor --- rims_app/CMakeLists.txt.user | 2 +- rims_app/prj.conf | 2 +- rims_app/src/common.hpp | 5 +- rims_app/src/main.cpp | 7 +- rims_app/src/power_measurements.cpp | 8 +- rims_app/src/temperature_measurements.cpp | 204 ++++++++++++---------- rims_app/src/temperature_measurements.hpp | 61 ++++--- rims_app/src/uart.hpp | 1 - rims_app/src/zephyr.cpp | 43 ++++- rims_app/src/zephyr.hpp | 26 ++- rims_app/src/zero_cross_detection.cpp | 100 ++++++----- rims_app/src/zero_cross_detection.hpp | 25 +-- 12 files changed, 291 insertions(+), 193 deletions(-) diff --git a/rims_app/CMakeLists.txt.user b/rims_app/CMakeLists.txt.user index f68b96de663..a558ff55784 100644 --- a/rims_app/CMakeLists.txt.user +++ b/rims_app/CMakeLists.txt.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/rims_app/prj.conf b/rims_app/prj.conf index dce83b24e2c..18e701bf896 100644 --- a/rims_app/prj.conf +++ b/rims_app/prj.conf @@ -50,7 +50,7 @@ CONFIG_LOG_BACKEND_UART=n # Use UART for log output CONFIG_MAIN_STACK_SIZE=1500 # CONFIG_CRC=y -CONFIG_ASSERT=n +CONFIG_ASSERT=y #CONFIG_NUM_PREEMPT_PRIORITIES=0 diff --git a/rims_app/src/common.hpp b/rims_app/src/common.hpp index 20c3dcab401..b32eab57b05 100644 --- a/rims_app/src/common.hpp +++ b/rims_app/src/common.hpp @@ -238,7 +238,10 @@ constexpr int GPIO_PIN_PB6 = 6; using clock = std::chrono::steady_clock; using time_point = clock::time_point; -using short_microseconds = std::chrono::duration; // max ~65ms + +using microseconds_u16_t = std::chrono::duration; // max ~65ms +using microseconds_u32_t = std::chrono::duration; // max ~4294s, ~71 min + using adc_sample = uint16_t; } // namespace rims diff --git a/rims_app/src/main.cpp b/rims_app/src/main.cpp index 657d2d38b48..7c51a7636ec 100644 --- a/rims_app/src/main.cpp +++ b/rims_app/src/main.cpp @@ -68,6 +68,8 @@ LazyInit phaseModulation; gpio_dt_spec status = GPIO_DT_SPEC_GET(DT_NODELABEL(status_led), gpios); gpio_dt_spec gprelay_1_en = GPIO_DT_SPEC_GET(DT_NODELABEL(gprelay_1_en), gpios); gpio_dt_spec gprelay_2_en = GPIO_DT_SPEC_GET(DT_NODELABEL(gprelay_2_en), gpios); // pompka +gpio_dt_spec ch_1_zcd = GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_zcd), gpios); +gpio_dt_spec ch_2_zcd = GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_zcd), gpios); // pompka int main() { using namespace rims; @@ -104,9 +106,12 @@ int main() { zephyr::gpio::pin_configure(gprelay_1_en, GPIO_OUTPUT_INACTIVE); zephyr::gpio::pin_configure(gprelay_2_en, GPIO_OUTPUT_INACTIVE); + zephyr::gpio::pin_configure(ch_1_zcd, GPIO_INPUT | GPIO_PULL_UP); + zephyr::gpio::pin_configure(ch_2_zcd, GPIO_INPUT | GPIO_PULL_UP); + + // zephyr::gpio::pin_toggle_dt(gprelay_2_en); while (1) { std::this_thread::sleep_for(std::chrono::seconds{1}); - // zephyr::gpio::pin_toggle_dt(gprelay_2_en); // const unsigned char data[] = {'b', 0x55}; // uart_fifo_fill(defaultUart()->_dev, data, 1); diff --git a/rims_app/src/power_measurements.cpp b/rims_app/src/power_measurements.cpp index 36eb5c5a387..5f62a3bfb12 100644 --- a/rims_app/src/power_measurements.cpp +++ b/rims_app/src/power_measurements.cpp @@ -27,7 +27,7 @@ namespace rims { // Function to fit the sine wave's amplitude (no phase estimation needed) -float fit_sine_wave_amplitude(std::span current_measurements, std::span offsets) { +float fit_sine_wave_amplitude(std::span current_measurements, std::span offsets) { const int N = current_measurements.size(); float sum_sin_current = 0.0; float sum_sin_sin = 0.0; @@ -50,7 +50,7 @@ float fit_sine_wave_amplitude(std::span current_measurements, std::s } // Function to calculate power based on current measurements, resistance, and zero-crossing information -float calculate_power(std::span current_measurements, std::span time_intervals, float resistance) { +float calculate_power(std::span current_measurements, std::span time_intervals, float resistance) { // Step 1: Fit sine wave amplitude auto amplitude = fit_sine_wave_amplitude(current_measurements, time_intervals); @@ -69,13 +69,13 @@ void power_measurements_entrypoint(void *p1, void *p2, void *p3) { static std::pmr::monotonic_buffer_resource br{buf, 256, std::pmr::null_memory_resource()}; std::pmr::vector measurements{&br}; - std::pmr::vector time_points{&br}; + std::pmr::vector time_points{&br}; measurements.reserve(100); time_points.reserve(100); measurements.emplace_back(32.0f); - time_points.emplace_back(std::chrono::duration_cast(clock::now() - clock::now())); + time_points.emplace_back(std::chrono::duration_cast(clock::now() - clock::now())); while (1) { ///TODO fix diff --git a/rims_app/src/temperature_measurements.cpp b/rims_app/src/temperature_measurements.cpp index d0c1c379244..9c816b97b92 100644 --- a/rims_app/src/temperature_measurements.cpp +++ b/rims_app/src/temperature_measurements.cpp @@ -20,11 +20,6 @@ #include #include "zephyr.hpp" -#include "zephyr/device.h" -#include "zephyr/sys/__assert.h" - -#define ADC_NODE DT_NODELABEL(adc1) -#define DT_ADC_TEMP_NODELABEL DT_NODELABEL(adc_temp) namespace rims { @@ -51,29 +46,42 @@ K_FIFO_DEFINE(temperatureEggress); zephyr_fifo_buffer temperatureIngressQueue{temperatureIngress}; zephyr_fifo_buffer temperatureEgressQueue{temperatureEggress}; -constexpr gpio_dt_spec rmin_en_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(rmin_en), gpios); -constexpr gpio_dt_spec rmax_en_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(rmax_en), gpios); -constexpr gpio_dt_spec rpt_en_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(rpt_en), gpios); +namespace { +constexpr gpio_dt_spec rmin_en_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(rmin_en), gpios); +constexpr gpio_dt_spec rmax_en_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(rmax_en), gpios); +constexpr gpio_dt_spec rpt_en_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(rpt_en), gpios); constexpr gpio_dt_spec temp_channel_sel_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(temp_channel_sel), gpios); -void select_rmin(){ +constexpr adc_channel_cfg adc_temp = ADC_CHANNEL_CFG_DT(DT_NODELABEL(adc_temp)); + +const device *adc = DEVICE_DT_GET(DT_NODELABEL(adc1)); + +void select_rmin() { zephyr::gpio::pin_set_low(rmax_en_gpio_dt); zephyr::gpio::pin_set_high(rmin_en_gpio_dt); zephyr::gpio::pin_set_low(rpt_en_gpio_dt); } -void select_rmax(){ +void select_rmax() { zephyr::gpio::pin_set_high(rmax_en_gpio_dt); zephyr::gpio::pin_set_low(rmin_en_gpio_dt); zephyr::gpio::pin_set_low(rpt_en_gpio_dt); } -void select_rpt(){ +void select_rpt() { zephyr::gpio::pin_set_low(rmax_en_gpio_dt); zephyr::gpio::pin_set_low(rmin_en_gpio_dt); zephyr::gpio::pin_set_high(rpt_en_gpio_dt); } +void select_channel(int channel) { // TODO check channels + if (channel == 1) { + zephyr::gpio::pin_set_low(temp_channel_sel_gpio_dt); + } else { + zephyr::gpio::pin_set_high(temp_channel_sel_gpio_dt); + } +} + template void PB_encode_egress(const T &tempresp) { /// TODO repeat on fail temperatureEgressQueue.try_produce([&](temperature_EgressMessages &egress) { @@ -93,34 +101,79 @@ template void PB_encode_egress(const T &tempresp) { return true; }); } +} // namespace +TemperatureSampler::TemperatureSampler(uint8_t channel) + : _samplerSem{0, 1}, _samplerTimer{_samplerSem, std::chrono::milliseconds{125}}, // + _broadcastSem{0, 1}, _broadcastTimer{_broadcastSem, std::chrono::seconds{10}}, // + _channel{channel} { + + calibration(); + _broadcastTimer.start(); +} + +void TemperatureSampler::activate() { + // TODO check when last time calibrated and recalibrate if needed + select_channel(_channel); + std::this_thread::sleep_for(std::chrono::milliseconds{1}); // TODO check how long the signal needs to stabilize, and set this value here + _samplerTimer.start(); + // ULOG_INFO("Channel %d activated", _channel); +} + +void TemperatureSampler::deactivate() { + _samplerTimer.stop(); +} void TemperatureSampler::take_sample() { - // ULOG_DEBUG("Samples on ch %d, size %d", this->_channel, this->_samples.size()); _samples.push_back(adc_take_sample()); } -TemperatureSampler::TemperatureSampler(uint8_t channel) - : /*_samplerSem{0, 1}, _samplerTimer{_samplerSem, std::chrono::milliseconds{125}}, */ _channel{channel} { - calibration(); +void TemperatureSampler::broadcastTemperature() const { + PB_encode_egress(temperature_statistics()); } void TemperatureSampler::calibration() { /// TODO Change to current channal + select_channel(_channel); ULOG_INFO("Starting temp adc calibration"); select_rmin(); this->_adc_Tmin = takeStableRead(); - + select_rmax(); this->_adc_Tmax = takeStableRead(); ULOG_INFO("Calibration done, min{%d}/max{%d}", _adc_Tmin, _adc_Tmax); - + select_rpt(); } +// asserts function takes less then +// TODO needs to assert execution on this thread only, right? +class BlockExecutionDurationAssert { +#if __ASSERT_ON + const std::chrono::steady_clock::time_point _start; + const std::chrono::steady_clock::duration _maxTime; + + public: + BlockExecutionDurationAssert(std::chrono::nanoseconds maxTime) : _start{std::chrono::steady_clock::now()}, _maxTime{maxTime} { + } + ~BlockExecutionDurationAssert() { + __ASSERT((std::chrono::steady_clock::now() - _start) <= _maxTime, "execution taked too long"); + } +#else + TimeCheck(std::chrono::nanoseconds) { + } +#endif +}; + void TemperatureSampler::tick() { - // if (zephyr::semaphore::k_sem_take_now(_samplerSem) == 0) { - // take_sample(); - // } + // executed every 1ms by the orchestrator + // BlockExecutionDurationAssert _{std::chrono::microseconds{900}}; + + if (zephyr::semaphore::k_sem_take_now(_samplerSem) == 0) { + take_sample(); + } + if (zephyr::semaphore::k_sem_take_now(_broadcastSem) == 0) { + broadcastTemperature(); + } } BroadcastTemperatureStatistics TemperatureSampler::temperature_statistics() const { @@ -130,7 +183,7 @@ BroadcastTemperatureStatistics TemperatureSampler::temperature_statistics() cons // Use only the last N samples or all if fewer std::size_t count = std::max(std::min(this->_samplesNumber, total_samples), 1U); auto start_it = samples.cend() - count; - + auto sum = std::accumulate(start_it, samples.cend(), Sample{}); auto mean = sum / count; @@ -185,19 +238,18 @@ TemperatureSampler::Sample TemperatureSampler::to_temperature(uint32_t adcReadin return temperature; } -uint32_t TemperatureSampler::adc() const -{ +uint32_t TemperatureSampler::adc() const { std::array samples{}; // TODO add emssage for samples fer measurement constexpr auto ADC_RESOLUTION = 12; const adc_dt_spec adc_spec = ADC_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 0); - + const adc_sequence_options options = { .interval_us = 0, .callback = nullptr, .user_data = nullptr, .extra_samplings = 3, }; - + adc_sequence sequence = { .options = &options, .channels = BIT(15), @@ -206,16 +258,16 @@ uint32_t TemperatureSampler::adc() const .resolution = ADC_RESOLUTION, .oversampling = 0, }; - + auto err = adc_read_dt(&adc_spec, &sequence); if (err != 0) { ULOG_ERROR("ADC got error %d at sequence.cahnnelid %d", err, sequence.channels); return 0; } - + auto sum = std::accumulate(samples.begin(), samples.end(), 0); auto avg = sum / samples.size(); - + return avg; } @@ -233,41 +285,33 @@ template constexpr bool unknownChannelError(const } TemperatureSamplerOrchestrator::TemperatureSamplerOrchestrator() - : // - _temperatureSamplerChannel{0}, // - _temperatureSamplerChannels{TemperatureSampler{1}, TemperatureSampler{2}}, // - _samplerSem{0, 1}, // - _samplerTimer{_samplerSem, std::chrono::milliseconds{500}}, // - _broadcastSem{zephyr::semaphore::sem{0, 1}, zephyr::semaphore::sem{0, 1}}, // - _broadcastTimer{ // - RecurringSemaphoreTimer{_broadcastSem.at(0), std::chrono::seconds{4}}, - RecurringSemaphoreTimer{_broadcastSem.at(1), std::chrono::seconds{3}} - } { + : // + _channel{0}, // + _temperatureSamplerChannels{TemperatureSampler{1}, TemperatureSampler{2}}, // + _samplerTickSem{0, 1}, _samplerTick{_samplerTickSem, std::chrono::milliseconds{1}}, // + _channelChangeSem{0, 1}, _channelChange{_channelChangeSem, std::chrono::seconds{1}} // +{ ULOG_INFO("Initialize message events"); temperatureIngressQueue.k_event_init(_events.at(0)); - - ULOG_INFO("Initialize timer events"); - zephyr::event_pool::k_init(_events.at(1), _samplerSem); - zephyr::event_pool::k_init(_events.at(2), _broadcastSem[0]); - zephyr::event_pool::k_init(_events.at(3), _broadcastSem[1]); + zephyr::event_pool::k_init(_events.at(1), _samplerTickSem); + zephyr::event_pool::k_init(_events.at(2), _channelChangeSem); }; void TemperatureSamplerOrchestrator::loop() { ULOG_INFO("Starting TemperatureSampler loop"); + _samplerTick.start(); + _channelChange.start(); - _samplerTimer.start(); - _broadcastTimer[0].start(); - _broadcastTimer[1].start(); + action_changeChannel(); while (1) { try { auto ret = zephyr::event_pool::k_poll_forever(_events); if (ret == 0) { zephyr::event_pool::k_poll_handle(_events[0], [&]() { event_messageArrived(); }); - zephyr::event_pool::k_poll_handle(_events[1], [&]() { action_takeSample(); }); - zephyr::event_pool::k_poll_handle(_events[2], [&]() { action_sendTemperature(0); }); - zephyr::event_pool::k_poll_handle(_events[3], [&]() { action_sendTemperature(1); }); + zephyr::event_pool::k_poll_handle(_events[1], [&]() { action_samplersTick(); }); + zephyr::event_pool::k_poll_handle(_events[2], [&]() { action_changeChannel(); }); } } catch (const std::exception &e) { ULOG_ERROR("EXCEPTION %s", e.what()); @@ -396,12 +440,15 @@ void TemperatureSamplerOrchestrator::handle_filterConfigRequest(const FilterConf if (unknownChannelError(req, resp)) return; if (req.has_emaFilterParams) { + /// TODO set } if (req.has_kalmanFilterParams) { + /// TODO set } if (req.has_averageFilterParams) { + /// TODO set } if (not req.skipFullResponse) { @@ -412,62 +459,33 @@ void TemperatureSamplerOrchestrator::handle_filterRequest(const FilterRequest &r if (unknownChannelError(req, resp)) return; } -void TemperatureSamplerOrchestrator::action_takeSample() { - zephyr::semaphore::k_sem_take_now(_samplerSem); - _temperatureSamplerChannels[_temperatureSamplerChannel].take_sample(); - _temperatureSamplerChannel = (_temperatureSamplerChannel + 1) % channelNumber; +void TemperatureSamplerOrchestrator::action_samplersTick() { + zephyr::semaphore::k_sem_take_now(_samplerTickSem); + for (auto &sampler : _temperatureSamplerChannels) { + sampler.tick(); + } } -void TemperatureSamplerOrchestrator::action_sendTemperature(uint8_t ch) { - zephyr::semaphore::k_sem_take_now(_broadcastSem[ch]); - PB_encode_egress(_temperatureSamplerChannels[ch].temperature_statistics()); +void TemperatureSamplerOrchestrator::action_changeChannel() { + zephyr::semaphore::k_sem_take_now(_channelChangeSem); + for (auto &sampler : _temperatureSamplerChannels) { + sampler.deactivate(); + } + + _temperatureSamplerChannels.at(_channel).activate(); + _channel = (_channel + 1) % temperatureChannelsNr; } int TemperatureSamplerThread::do_hardwarenInit() { - const device *adc = DEVICE_DT_GET(ADC_NODE); - const gpio_dt_spec chsel_pin_dev = GPIO_DT_SPEC_GET(DT_NODELABEL(temp_channel_sel), gpios); - __ASSERT(device_is_ready(adc), "adc needs to work"); - - zephyr::gpio::pin_configure(chsel_pin_dev, GPIO_OUTPUT_INACTIVE); - if (!device_is_ready(adc)) { - ULOG_ERROR("ADC controller device %s not ready\n", adc->name); - return -1; - } else { - ULOG_INFO("ADC controller device %s checked and ready", adc->name); - } - - auto configurechannel = [&](const auto &adcspec) { - if (!device_is_ready(adc)) { - ULOG_ERROR("ADC channel device %s not ready", adc->name); - return -1; - } else { - ULOG_INFO("ADC channel device %s checked and ready", adc->name); - } - - if (adc_channel_setup(adc, &adcspec)) { - ULOG_ERROR("Failed to configure ADC"); - return -1; - } - ULOG_INFO("Channel onfiguration done"); - return 0; - }; - - const adc_channel_cfg adc_temp = ADC_CHANNEL_CFG_DT(DT_NODELABEL(adc_temp)); - adc_channel_cfg adc_current_ch1 = ADC_CHANNEL_CFG_DT(DT_NODELABEL(adc_current_ch1)); - adc_channel_cfg adc_current_ch2 = ADC_CHANNEL_CFG_DT(DT_NODELABEL(adc_current_ch2)); - - auto ret = configurechannel(adc_temp); - configurechannel(adc_current_ch1); - configurechannel(adc_current_ch2); - zephyr::gpio::pin_configure(rmax_en_gpio_dt, GPIO_OUTPUT_INACTIVE); zephyr::gpio::pin_configure(rmin_en_gpio_dt, GPIO_OUTPUT_INACTIVE); zephyr::gpio::pin_configure(rpt_en_gpio_dt, GPIO_OUTPUT_INACTIVE); zephyr::gpio::pin_configure(temp_channel_sel_gpio_dt, GPIO_OUTPUT_INACTIVE); - return ret; + zephyr::adc::channel_setup(adc, adc_temp); + return 0; } void TemperatureSamplerThread::threadMain() { diff --git a/rims_app/src/temperature_measurements.hpp b/rims_app/src/temperature_measurements.hpp index 29410feaf80..3480dc21b7a 100644 --- a/rims_app/src/temperature_measurements.hpp +++ b/rims_app/src/temperature_measurements.hpp @@ -45,33 +45,42 @@ class TemperatureSampler { TemperatureSampler(TemperatureSampler &&) = delete; TemperatureSampler &operator=(TemperatureSampler &&) = delete; - - void calibration(); + void tick(); + + void setSamplerConfig(const temperature_SamplerConfig &config) noexcept(false); + + void activate(); + void deactivate(); BroadcastTemperatureStatistics temperature_statistics() const; - TemperatureCurrent temperature() const; - - void take_sample(); - void setSamplerConfig(const temperature_SamplerConfig &config) noexcept(false); - + TemperatureCurrent temperature() const; protected: + void calibration(); + + bool _running{false}; + + void take_sample(); + void broadcastTemperature() const; + uint32_t takeStableRead(); - - Sample to_temperature(uint32_t adc_value) const; + + Sample to_temperature(uint32_t adc_value) const; uint32_t adc() const; - Sample adc_take_sample() const; - + Sample adc_take_sample() const; + uint32_t _adc_Tmin{}; uint32_t _adc_Tmax{}; - float _Tmin{-25.0f}; - float _Tmax{103.0f}; - - // zephyr::semaphore::sem _samplerSem{0, 1}; - // RecurringSemaphoreTimer _samplerTimer; - + float _Tmin{-25.0f}; + float _Tmax{103.0f}; + + zephyr::semaphore::sem _samplerSem{0, 1}; + RecurringSemaphoreTimer _samplerTimer; + zephyr::semaphore::sem _broadcastSem{0, 1}; + RecurringSemaphoreTimer _broadcastTimer; + ring_buffer _samples; - std::size_t _samplesNumber{MaxSampleSize}; + std::size_t _samplesNumber{MaxSampleSize}; const std::uint8_t _channel{}; }; @@ -92,19 +101,19 @@ class TemperatureSamplerOrchestrator { void handle_filterConfigRequest(const FilterConfigRequest &req, FilterConfigResponse &resp) const; void handle_filterRequest(const FilterRequest &req, FilterResponse &resp) const; - void action_takeSample(); - void action_sendTemperature(uint8_t ch); + void action_samplersTick(); + void action_changeChannel(); - uint8_t _temperatureSamplerChannel{0}; + uint8_t _channel{0}; std::array _temperatureSamplerChannels; - zephyr::semaphore::sem _samplerSem{0, 1}; - RecurringSemaphoreTimer _samplerTimer; + zephyr::semaphore::sem _samplerTickSem{0, 1}; + RecurringSemaphoreTimer _samplerTick; - std::array _broadcastSem; - std::array _broadcastTimer; + zephyr::semaphore::sem _channelChangeSem{0, 1}; + RecurringSemaphoreTimer _channelChange; - std::array _events; // event from timer and from Messenger + std::array _events; // event from timer and from Messenger }; class TemperatureSamplerThread : public ZephyrThread { diff --git a/rims_app/src/uart.hpp b/rims_app/src/uart.hpp index 54969ad1184..d70f7436cb5 100644 --- a/rims_app/src/uart.hpp +++ b/rims_app/src/uart.hpp @@ -3,7 +3,6 @@ #include "circular_buffer.hpp" #include "common.hpp" -#include #include #include #include diff --git a/rims_app/src/zephyr.cpp b/rims_app/src/zephyr.cpp index 4447e5dfb81..80a22be1f35 100644 --- a/rims_app/src/zephyr.cpp +++ b/rims_app/src/zephyr.cpp @@ -1,14 +1,45 @@ #include "zephyr.hpp" #include "log.hpp" +#include "zephyr/sys/__assert.h" #include -void zephyr::gpio::pin_configure(const gpio_dt_spec &pin, gpio_flags_t flags, std::source_location sl) { - auto status = gpio_pin_configure_dt(&pin, flags); - if (status != 0) { - rims::Log{sl}.error("%s for %s:%d failed","gpio_pin_configure_dt", pin.port->name, pin.pin); - throw gpio_pin_configure_error{}; +void zephyr::adc::channel_setup(const device *adc, const adc_channel_cfg &channel_cfg, std::source_location sl) { + __ASSERT(adc, "adc needs to work"); + auto status = adc_channel_setup(adc, &channel_cfg); + if (status < 0) { + rims::Log{sl}.error("%s for %s:%d failed with status %d", "adc_channel_setup", adc->name, channel_cfg.channel_id, status); + throw adc_configure_error{}; + } + rims::Log{sl}.info("Channel onfiguration done"); +} + +void zephyr::gpio::pin_configure(const gpio_dt_spec &spec, gpio_flags_t flags, std::source_location sl) { + auto status = gpio_pin_configure_dt(&spec, flags); + if (status < 0) { + rims::Log{sl}.error("%s for %s:%d failed with status %d", "gpio_pin_configure_dt", spec.port->name, spec.pin, status); + throw pin_configure_error{}; } else { - rims::Log{sl}.info("%s for %s:%d checked and ready", "gpio_pin_configure_dt", pin.port->name, pin.pin); + rims::Log{sl}.info("%s for %s:%d checked and ready", "gpio_pin_configure_dt", spec.port->name, spec.pin); + } +} + +void zephyr::gpio::pin_interrupt_configure(const gpio_dt_spec &spec, gpio_flags_t flags, std::source_location sl) { + auto status = gpio_pin_interrupt_configure_dt(&spec, GPIO_INT_EDGE_BOTH); + if (status < 0) { + rims::Log{sl}.error("%s pin %d@%s failed with status %d", "gpio_pin_interrupt_configure_dt", spec.pin, spec.port->name, status); + throw pin_configure_error{}; + } else { + rims::Log{sl}.info("%s pin %d@%s ok", "gpio_pin_interrupt_configure_dt", spec.pin, spec.port->name); + } +} + +void zephyr::gpio::add_callback(const gpio_dt_spec &spec, callback &callback, std::source_location sl) { + auto status = gpio_add_callback(spec.port, &callback._cb); + if (status < 0) { + rims::Log{sl}.error("%s pin %d@%s failed with status %d", "gpio_add_callback", spec.pin, spec.port->name, status); + throw pin_configure_error{}; + } else { + rims::Log{sl}.info("%s pin %d@%s ok", "gpio_add_callback", spec.pin, spec.port->name); } } diff --git a/rims_app/src/zephyr.hpp b/rims_app/src/zephyr.hpp index fb495b28086..92b2648b361 100644 --- a/rims_app/src/zephyr.hpp +++ b/rims_app/src/zephyr.hpp @@ -1,5 +1,6 @@ #pragma once +#include "zephyr/drivers/adc.h" #include #include #include @@ -125,9 +126,21 @@ inline int k_sem_take_now(sem &sem) { } // namespace zephyr::semaphore +namespace zephyr::adc{ +class adc_configure_error : public std::exception { + // exception interface + public: + const char *what() const noexcept override { + return "adc_channel_setup error"; + } +}; + +void channel_setup(const struct device * adc, const adc_channel_cfg &channel_cfg, std::source_location sl = std::source_location::current()); +} + namespace zephyr::gpio { -class gpio_pin_configure_error : public std::exception { +class pin_configure_error : public std::exception { // exception interface public: const char *what() const noexcept override { @@ -135,5 +148,14 @@ class gpio_pin_configure_error : public std::exception { } }; -void pin_configure(const gpio_dt_spec &pin, gpio_flags_t flags, std::source_location sl = std::source_location::current()); +struct callback { + gpio_callback _cb; + callback(gpio_callback_handler_t handler, gpio_port_pins_t pin_mask) { + gpio_init_callback(&_cb, handler, pin_mask); + } +}; + +void pin_configure(const gpio_dt_spec &spec, gpio_flags_t flags, std::source_location sl = std::source_location::current()); +void pin_interrupt_configure(const struct gpio_dt_spec &spec, gpio_flags_t flags, std::source_location sl = std::source_location::current()); +void add_callback(const gpio_dt_spec &spec, callback &callback, std::source_location sl = std::source_location::current()); } // namespace zephyr::gpio diff --git a/rims_app/src/zero_cross_detection.cpp b/rims_app/src/zero_cross_detection.cpp index e7d9329da32..299e2cbdcd3 100644 --- a/rims_app/src/zero_cross_detection.cpp +++ b/rims_app/src/zero_cross_detection.cpp @@ -1,72 +1,65 @@ #include "zero_cross_detection.hpp" +#include "common.hpp" #include "log.hpp" -#include "syscalls/kernel.h" #include "zephyr.hpp" -#include "zephyr/sys/__assert.h" #include #include +#include #include #include #include #include -namespace rims { -gpio_dt_spec ch1ZCD_pin_spec = GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_zcd), gpios); -gpio_dt_spec ch2ZCD_pin_spec = GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_zcd), gpios); +namespace rims { +namespace { +constexpr gpio_dt_spec ch1ZCD_pin_spec = GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_zcd), gpios); +constexpr gpio_dt_spec ch2ZCD_pin_spec = GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_zcd), gpios); static int gpio_pin_mask_to_index(uint32_t pin_mask) { std::bitset<32> bits{pin_mask}; if (bits.none()) return -1; return bits._Find_first(); } - +} // namespace K_FIFO_DEFINE(zcdFifo); zephyr_fifo_buffer ZeroCrossDetectionEventQueue{zcdFifo}; void ZeroCrossDetection::interrupt_handler(const device *dev, struct gpio_callback *cb, uint32_t pins) { - ULOG_INFO("zcd"); auto _this = CONTAINER_OF(cb, ZeroCrossDetection, cb); int pin_state = gpio_pin_get(dev, gpio_pin_mask_to_index(pins)); - _this->_lastInt = std::chrono::steady_clock::now(); - if (not _this->_msgQueue->try_produce([&](ZeroCrossDetectionEvent &event) { event.state = static_cast(pin_state); event.channel = _this->_channel; return true; - })) { + }) + ) { + } + + if(pin_state==0){ + _this->_cyclesWidths.push_back(std::chrono::duration_cast(clock::now() - _this->_pulseDown)); + _this->_pulseDown = clock::now(); + } else { + _this->_pulsWidths.push_back(std::chrono::duration_cast(clock::now() - _this->_pulseDown)); } } -ZeroCrossDetection::ZeroCrossDetection(gpio_dt_spec *gpio, ZCDFifo_t *queue, uint8_t channel) - : _gpio{gpio}, _channel{channel}, _msgQueue{queue}, _intCheckSem{0, 1}, _intCheckTimer{_intCheckSem, std::chrono::seconds{1}} { - ULOG_INFO("zero_cross_detection_entrypoint"); - +ZeroCrossDetection::ZeroCrossDetection(const gpio_dt_spec &gpio, ZCDFifo_t *queue, uint8_t channel) + : _channel{channel}, _msgQueue{queue}, cb{ZeroCrossDetection::interrupt_handler, BIT(gpio.pin)}, // + _intCheckSem{0, 1}, _intCheckTimer{_intCheckSem, std::chrono::seconds{30}} { _intCheckTimer.start(); - - if (gpio_pin_interrupt_configure_dt(_gpio, GPIO_INT_EDGE_BOTH) != 0) { - ULOG_CRITICAL("%s pin %d@%s failed", "gpio_pin_interrupt_configure_dt", _gpio->pin, gpio->port->name); - } else { - ULOG_INFO("%s pin %d@%s ok", "gpio_pin_interrupt_configure_dt", _gpio->pin, gpio->port->name); - } - - gpio_init_callback(&cb, ZeroCrossDetection::interrupt_handler, BIT(_gpio->pin)); - - if (gpio_add_callback(_gpio->port, &cb) < 0) { - ULOG_CRITICAL("%s pin %d@%s ok", "gpio_add_callback", _gpio->pin, gpio->port->name); - } else { - ULOG_INFO("%s pin %d@%s ok", "gpio_add_callback", _gpio->pin, gpio->port->name); - } - - ULOG_INFO("zero_cross_detection_entrypoint DONE"); + _pulseDown = clock::now(); + + zephyr::gpio::pin_interrupt_configure(gpio, GPIO_INT_EDGE_BOTH); + zephyr::gpio::add_callback(gpio, cb); } ZeroCrossDetectionOrchestrator::ZeroCrossDetectionOrchestrator() : _zcd{ - ZeroCrossDetection{&ch1ZCD_pin_spec, &ZeroCrossDetectionEventQueue, 0}, - ZeroCrossDetection{&ch2ZCD_pin_spec, &ZeroCrossDetectionEventQueue, 1} + ZeroCrossDetection{ch1ZCD_pin_spec, &ZeroCrossDetectionEventQueue, 0}, + ZeroCrossDetection{ch2ZCD_pin_spec, &ZeroCrossDetectionEventQueue, 1} } { ULOG_INFO("Events configuration"); zephyr::event_pool::k_init(_events.at(0), _zcd.at(0)._intCheckSem); @@ -76,7 +69,6 @@ ZeroCrossDetectionOrchestrator::ZeroCrossDetectionOrchestrator() void ZeroCrossDetectionOrchestrator::loop() { while (1) { - // std::this_thread::sleep_for(std::chrono::seconds{1}); auto ret = zephyr::event_pool::k_poll_forever(_events); if (ret == 0) { zephyr::event_pool::k_poll_handle(_events.at(0), [&]() { event_zcdCheck(_zcd.at(0)); }); @@ -90,14 +82,16 @@ void ZeroCrossDetectionOrchestrator::event_zcdCheck(ZeroCrossDetection &channel) if (ret != 0) { ULOG_INFO("B %d", ret); } else { - const auto toLast = std::chrono::steady_clock::now() - channel._lastInt; - if (toLast > std::chrono::seconds{2}) { - ULOG_INFO( - "ZCD detected no signal on %d for more than %d s", - channel._channel, - std::chrono::duration_cast>(toLast).count() - ); - } + ULOG_INFO("ZCD pulse width for channel %d = %dus", channel._channel, channel.pulsWidth().count()); + ULOG_INFO("ZCD cycle width for channel %d = %dus", channel._channel, channel.cycleWidth().count()); + // const auto toLast = clock::now() - channel._lastInt; + // if (toLast > std::chrono::seconds{30}) { + // ULOG_INFO( + // "ZCD detected no signal on %d for more than %d s", + // channel._channel, + // std::chrono::duration_cast>(toLast).count() + // ); + // } } } @@ -107,13 +101,27 @@ void ZeroCrossDetectionThread::threadMain() { } int ZeroCrossDetectionThread::do_hardwarenInit() { - /// can throw - auto configure = [](const auto &dt_spec) { zephyr::gpio::pin_configure(dt_spec, GPIO_OUTPUT_INACTIVE); }; - - configure(ch1ZCD_pin_spec); - configure(ch2ZCD_pin_spec); + zephyr::gpio::pin_configure(ch1ZCD_pin_spec, GPIO_INPUT | GPIO_PULL_UP); + zephyr::gpio::pin_configure(ch2ZCD_pin_spec, GPIO_INPUT | GPIO_PULL_UP); return 0; } +microseconds_u16_t ZeroCrossDetection::pulsWidth() const +{ + if(_pulsWidths.empty()) + return std::chrono::milliseconds{1}; + + auto totalus = std::accumulate(_pulsWidths.begin(), _pulsWidths.end(), microseconds_u32_t{}); + return totalus / _pulsWidths.size(); +} +microseconds_u16_t ZeroCrossDetection::cycleWidth() const +{ + if(_cyclesWidths.empty()) + return std::chrono::milliseconds{10}; + + auto totalus = std::accumulate(_cyclesWidths.begin(), _cyclesWidths.end(), microseconds_u32_t{}); + return totalus / _cyclesWidths.size(); +} + } // namespace rims diff --git a/rims_app/src/zero_cross_detection.hpp b/rims_app/src/zero_cross_detection.hpp index a3b11dcf75e..d042e160355 100644 --- a/rims_app/src/zero_cross_detection.hpp +++ b/rims_app/src/zero_cross_detection.hpp @@ -12,7 +12,7 @@ namespace rims { struct ZeroCrossDetectionEvent { bool state : 1; - int channel : 4; // max 2^16 channels + int channel : 4; // max 2^4 channels }; extern zephyr_fifo_buffer ZeroCrossDetectionEventQueue; @@ -20,7 +20,7 @@ using ZCDFifo_t = zephyr_fifo_buffer; class ZeroCrossDetection { public: - ZeroCrossDetection(gpio_dt_spec *gpio, ZCDFifo_t *queue, uint8_t channel); + ZeroCrossDetection(const gpio_dt_spec &gpio, ZCDFifo_t *queue, uint8_t channel); ZeroCrossDetection(const ZeroCrossDetection &) = delete; ZeroCrossDetection &operator=(const ZeroCrossDetection &) = delete; @@ -28,19 +28,22 @@ class ZeroCrossDetection { ZeroCrossDetection(ZeroCrossDetection &&) = delete; ZeroCrossDetection &operator=(ZeroCrossDetection &&) = delete; - static void interrupt_handler(const device *dev, gpio_callback *cb, uint32_t pins); - const gpio_dt_spec *_gpio; - const uint8_t _channel; + static void interrupt_handler(const device *dev, gpio_callback *cb, uint32_t pins); - ZCDFifo_t *_msgQueue{nullptr}; // can't be a reference - std::chrono::steady_clock::time_point _lastInt; // last arrived interrupt - gpio_callback cb; + microseconds_u16_t pulsWidth() const; + microseconds_u16_t cycleWidth() const; - // k_sem _intCheckSem; - // k_timer _intCheckTimer; - zephyr::semaphore::sem _intCheckSem; + const uint8_t _channel; + + ZCDFifo_t *_msgQueue{nullptr}; // can't be a reference + zephyr::gpio::callback cb; + + zephyr::semaphore::sem _intCheckSem; RecurringSemaphoreTimer _intCheckTimer; + clock::time_point _pulseDown{}; + rims::ring_buffer _pulsWidths; + rims::ring_buffer _cyclesWidths; }; class ZeroCrossDetectionOrchestrator { -- 2.45.2 From 0e67e17712a167ced3090cc70dadf5d41193998f Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Thu, 24 Apr 2025 11:53:59 +0200 Subject: [PATCH 09/18] Fix uart issues, fix power control, add utilization info in dbg messages --- boards/bartoszek/rims/rims_h503cbt.dts | 2 +- rims_app/prj.conf | 17 ++-- rims_app/src/common.hpp | 21 ++++- rims_app/src/main.cpp | 26 ++++-- rims_app/src/phase_modulation.cpp | 41 +++++---- rims_app/src/temperature_measurements.cpp | 105 ++++++++++++++-------- rims_app/src/temperature_measurements.hpp | 38 ++++---- 7 files changed, 155 insertions(+), 95 deletions(-) diff --git a/boards/bartoszek/rims/rims_h503cbt.dts b/boards/bartoszek/rims/rims_h503cbt.dts index 56c3d194b2d..e741e30848a 100644 --- a/boards/bartoszek/rims/rims_h503cbt.dts +++ b/boards/bartoszek/rims/rims_h503cbt.dts @@ -219,7 +219,7 @@ pinctrl-0 = <&usart1_tx_pb14 &usart1_rx_pb15>; pinctrl-names = "default"; - current-speed = <3000000>; + current-speed = <1000000>; // clocks = <&rcc STM32_CLOCK(APB2, 14)>, <&rcc STM32_SRC_PLL2_Q USART1_SEL(1)>; status = "okay"; diff --git a/rims_app/prj.conf b/rims_app/prj.conf index 18e701bf896..51c9260687c 100644 --- a/rims_app/prj.conf +++ b/rims_app/prj.conf @@ -11,22 +11,26 @@ CONFIG_POSIX_TIMERS=y # CONFIG_TIMER_READS_ITS_FREQUENCY_AT_RUNTIME=y #CONFIG_USERSPACE=y -#CONFIG_HW_STACK_PROTECTION=y -#CONFIG_MPU_STACK_GUARD=y +CONFIG_HW_STACK_PROTECTION=y +CONFIG_MPU_STACK_GUARD=y +CONFIG_ASSERT=y +# modules CONFIG_NANOPB=y +CONFIG_CRC=y CONFIG_CPP=y CONFIG_STD_CPP2B=y CONFIG_CPP_EXCEPTIONS=y CONFIG_CPP_RTTI=n -#CONFIG_COMMON_LIBC_MALLOC_ARENA_SIZE=512 CONFIG_REQUIRES_FULL_LIBCPP=y -#CONFIG_NEWLIB_LIBC=y CONFIG_NEWLIB_LIBC_NANO=n # CONFIG_THREAD_ANALYZER=y CONFIG_THREAD_NAME=y +CONFIG_THREAD_ANALYZER_USE_PRINTK=n +CONFIG_THREAD_ANALYZER_USE_LOG=n +CONFIG_THREAD_ANALYZER_AUTO=n # CONFIG_CONSOLE=n CONFIG_UART_CONSOLE=n @@ -48,9 +52,4 @@ CONFIG_LOG_MODE_IMMEDIATE=n # Log messages are output immediately CONFIG_LOG_BACKEND_UART=n # Use UART for log output CONFIG_MAIN_STACK_SIZE=1500 -# -CONFIG_CRC=y -CONFIG_ASSERT=y - -#CONFIG_NUM_PREEMPT_PRIORITIES=0 diff --git a/rims_app/src/common.hpp b/rims_app/src/common.hpp index b32eab57b05..3bc3f6b52d6 100644 --- a/rims_app/src/common.hpp +++ b/rims_app/src/common.hpp @@ -56,6 +56,12 @@ class Timer { stop(); }; + Timer(const Timer &) = delete; + Timer &operator=(const Timer &) = delete; + + Timer(Timer &&) = default; + Timer &operator=(Timer &&) = default; + void start() { k_timer_start(&timer_, _interval, _period); } @@ -65,6 +71,7 @@ class Timer { } bool isRunning() { + // This routine computes the time remaining before a running timer next expires, in units of system ticks. If the timer is not running, it returns zero. return k_timer_remaining_ticks(&timer_) > 0; } @@ -115,6 +122,7 @@ class RecurringSemaphoreTimer : public Timer { protected: void expiry_cb() override { + // This routine gives sem, unless the semaphore is already at its maximum permitted count. k_sem_give(_sem); } @@ -126,6 +134,13 @@ class SingleShootTimer : public Timer { public: SingleShootTimer(std::function cb, std::chrono::milliseconds interval) : Timer(interval), _cb{std::move(cb)} { } + + SingleShootTimer(const SingleShootTimer &) = delete; + SingleShootTimer &operator=(const SingleShootTimer &) = delete; + + SingleShootTimer(SingleShootTimer &&) = default; + SingleShootTimer &operator=(SingleShootTimer &&) = default; + void expiry_cb() override { _cb(); } @@ -236,12 +251,12 @@ constexpr auto ChannelNumber = 2; constexpr int GPIO_PIN_PB5 = 5; constexpr int GPIO_PIN_PB6 = 6; -using clock = std::chrono::steady_clock; -using time_point = clock::time_point; +using clock = std::chrono::steady_clock; +using time_point = clock::time_point; using microseconds_u16_t = std::chrono::duration; // max ~65ms using microseconds_u32_t = std::chrono::duration; // max ~4294s, ~71 min -using adc_sample = uint16_t; +using adc_sample = uint16_t; } // namespace rims diff --git a/rims_app/src/main.cpp b/rims_app/src/main.cpp index 7c51a7636ec..46c5a5dcfa0 100644 --- a/rims_app/src/main.cpp +++ b/rims_app/src/main.cpp @@ -1,18 +1,18 @@ #include "common.hpp" +#include "log.hpp" #include "messenger.hpp" #include "phase_modulation.hpp" #include "placement_unique_ptr.hpp" #include "temperature_measurements.hpp" #include "uart.hpp" #include "zephyr.hpp" +#include "zephyr/debug/thread_analyzer.h" #include "zero_cross_detection.hpp" #include #include #include -#include - #include #include @@ -65,11 +65,20 @@ LazyInit temperatureSampler; LazyInit zcd; LazyInit phaseModulation; -gpio_dt_spec status = GPIO_DT_SPEC_GET(DT_NODELABEL(status_led), gpios); +gpio_dt_spec status = GPIO_DT_SPEC_GET(DT_NODELABEL(status_led), gpios); gpio_dt_spec gprelay_1_en = GPIO_DT_SPEC_GET(DT_NODELABEL(gprelay_1_en), gpios); gpio_dt_spec gprelay_2_en = GPIO_DT_SPEC_GET(DT_NODELABEL(gprelay_2_en), gpios); // pompka -gpio_dt_spec ch_1_zcd = GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_zcd), gpios); -gpio_dt_spec ch_2_zcd = GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_zcd), gpios); // pompka +gpio_dt_spec ch_1_zcd = GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_zcd), gpios); +gpio_dt_spec ch_2_zcd = GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_zcd), gpios); // pompka + +void mythread_analyzer_cb(struct thread_analyzer_info *info) { + ULOG_DEBUG( + "thread name, memfree, cpu : %s, %d, %d", + info->name, + info->stack_size - info->stack_used, + info->utilization // cpu usage in % + ); +} int main() { using namespace rims; @@ -106,12 +115,11 @@ int main() { zephyr::gpio::pin_configure(gprelay_1_en, GPIO_OUTPUT_INACTIVE); zephyr::gpio::pin_configure(gprelay_2_en, GPIO_OUTPUT_INACTIVE); - zephyr::gpio::pin_configure(ch_1_zcd, GPIO_INPUT | GPIO_PULL_UP); - zephyr::gpio::pin_configure(ch_2_zcd, GPIO_INPUT | GPIO_PULL_UP); - // zephyr::gpio::pin_toggle_dt(gprelay_2_en); while (1) { - std::this_thread::sleep_for(std::chrono::seconds{1}); + std::this_thread::sleep_for(std::chrono::seconds{10}); + + thread_analyzer_run(mythread_analyzer_cb, 0); // const unsigned char data[] = {'b', 0x55}; // uart_fifo_fill(defaultUart()->_dev, data, 1); diff --git a/rims_app/src/phase_modulation.cpp b/rims_app/src/phase_modulation.cpp index c9e0c848a1a..7788a8499b9 100644 --- a/rims_app/src/phase_modulation.cpp +++ b/rims_app/src/phase_modulation.cpp @@ -2,8 +2,6 @@ #include "circular_buffer.hpp" #include "log.hpp" -#include "messenger.hpp" -#include "placement_unique_ptr.hpp" #include "proto/ctrl.pb.h" #include "zephyr.hpp" #include "zero_cross_detection.hpp" @@ -66,7 +64,7 @@ K_FIFO_DEFINE(ctrlEggress); zephyr_fifo_buffer ctrlIngressQueue{ctrlIngress}; zephyr_fifo_buffer ctrlEgressQueue{ctrlEggress}; -std::array pins = { +constexpr std::array pins = { gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_en), gpios), gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_en), gpios) }; @@ -93,7 +91,7 @@ PhaseModulation::PhaseModulation(const gpio_dt_spec &gpio) : PhaseControlBase{gpio}, // _triacTimerStart{[this]() { this->enable(); }, std::chrono::milliseconds{0}}, _triacTimerStop{[this]() { this->disable(); }, std::chrono::milliseconds{0}} { - ULOG_INFO("Started"); + ULOG_INFO("PhaseModulation started for gpio %d", gpio.pin); } void PhaseModulation::tickFallingEdge() { @@ -113,6 +111,7 @@ GroupModulation::GroupModulation(const gpio_dt_spec &gpio) : PhaseControlBase(gp } PhaseModulationOrchestrator::PhaseModulationOrchestrator() : _channel{Disabled{}, Disabled{}} { + _channel[0].emplace(pins[0]); ZeroCrossDetectionEventQueue.k_event_init(_events[0]); ctrlIngressQueue.k_event_init(_events[1]); @@ -134,16 +133,10 @@ void PhaseModulationOrchestrator::loop() { } } -int PhaseModulationThread::do_hardwarenInit() { - auto configure = [](const auto &dt_spec) { zephyr::gpio::pin_configure(dt_spec, GPIO_OUTPUT_INACTIVE); }; - std::for_each(pins.begin(), pins.end(), configure); - return 0; -} - constexpr float PI = 3.141592f; -constexpr float FREQUENCY = 0.5f; // 2 Hz +constexpr float FREQUENCY = 0.25f; constexpr float MIN_VALUE = 10.0f; -constexpr float MAX_VALUE = 25.0f; +constexpr float MAX_VALUE = 20.0f; float sinewave(std::chrono::steady_clock::time_point timePoint) { using namespace std::chrono; @@ -221,7 +214,7 @@ void PhaseModulationOrchestrator::event_ctrlMessageArrived() { std::invoke(fn, this, req, resp); } catch (const ctrl::error &e) { // clear message - resp = std::get<3>(handler); + resp = std::get<3>(handler); // set error on exception resp.has_error = true; resp.error = e._err; @@ -284,16 +277,16 @@ void PhaseModulationOrchestrator::handler_groupModulationConfigRequest( // GroupModulationConfigResponse &resp ) { ULOG_INFO("GroupModulationConfigRequest request handler"); - + checkChannel(request.channel_id); checkCurrentMode(request.channel_id, ctrl_ModeOfOperation_GroupModulation); auto &alg = std::get(_channel[request.channel_id]); - if(request.has_cycles_max){ + if (request.has_cycles_max) { alg.setCyclesMax(request.cycles_max); } - + resp.cycles_max = alg.getCyclesMax(); } @@ -304,13 +297,13 @@ void PhaseModulationOrchestrator::handler_groupModulationControlRequest( // ULOG_INFO("GroupModulationControlRequest request handler"); checkChannel(request.channel_id); checkCurrentMode(request.channel_id, ctrl_ModeOfOperation_GroupModulation); - + auto &alg = std::get(_channel[request.channel_id]); - - if(request.has_cycles){ + + if (request.has_cycles) { alg.setCycles(request.cycles); } - + resp.cycles = alg.getCycles(); } @@ -353,7 +346,7 @@ void PhaseModulationOrchestrator::setMode(uint8_t ch, ModeOfOperation mode) { break; case ctrl_ModeOfOperation_TemperatureSlopeModulation: throw ctrl::unsuported_mode{}; - + case ctrl_ModeOfOperation_ConstantTemperatureModulation: throw ctrl::unsuported_mode{}; @@ -377,6 +370,12 @@ ModeOfOperation PhaseModulationOrchestrator::getMode(uint8_t ch) { return ctrl_ModeOfOperation_Disabled; } +int PhaseModulationThread::do_hardwarenInit() { + auto configure = [](const auto &dt_spec) { zephyr::gpio::pin_configure(dt_spec, GPIO_OUTPUT_INACTIVE); }; + std::for_each(pins.begin(), pins.end(), configure); + return 0; +} + void PhaseModulationThread::threadMain() { ULOG_INFO("PhaseModulationThread start"); /// runs in context of new thread, on new thread stack etc. diff --git a/rims_app/src/temperature_measurements.cpp b/rims_app/src/temperature_measurements.cpp index 9c816b97b92..1154e946481 100644 --- a/rims_app/src/temperature_measurements.cpp +++ b/rims_app/src/temperature_measurements.cpp @@ -19,6 +19,7 @@ #include #include +#include "temperature_measurements.hpp" #include "zephyr.hpp" namespace rims { @@ -102,11 +103,7 @@ template void PB_encode_egress(const T &tempresp) { }); } } // namespace -TemperatureSampler::TemperatureSampler(uint8_t channel) - : _samplerSem{0, 1}, _samplerTimer{_samplerSem, std::chrono::milliseconds{125}}, // - _broadcastSem{0, 1}, _broadcastTimer{_broadcastSem, std::chrono::seconds{10}}, // - _channel{channel} { - +TemperatureSampler::TemperatureSampler(uint8_t channel) : _channel{channel} { calibration(); _broadcastTimer.start(); } @@ -124,7 +121,7 @@ void TemperatureSampler::deactivate() { } void TemperatureSampler::take_sample() { - _samples.push_back(adc_take_sample()); + _samples.push_back(adc()); } void TemperatureSampler::broadcastTemperature() const { @@ -134,13 +131,16 @@ void TemperatureSampler::broadcastTemperature() const { void TemperatureSampler::calibration() { /// TODO Change to current channal select_channel(_channel); - ULOG_INFO("Starting temp adc calibration"); + ULOG_INFO("Calibration at channel %d start", _channel); select_rmin(); - this->_adc_Tmin = takeStableRead(); + auto new_adc_Tmin = takeStableRead(); select_rmax(); - this->_adc_Tmax = takeStableRead(); - ULOG_INFO("Calibration done, min{%d}/max{%d}", _adc_Tmin, _adc_Tmax); + auto new_adc_Tmax = takeStableRead(); + ULOG_INFO("Calibration at channel %d done, min{%d}/max{%d}, prev min{%d}/max{%d}", _channel, new_adc_Tmin, new_adc_Tmax, _adc_Tmin, _adc_Tmax); + + _adc_Tmax = new_adc_Tmax; + _adc_Tmin = new_adc_Tmin; select_rpt(); } @@ -159,8 +159,9 @@ class BlockExecutionDurationAssert { __ASSERT((std::chrono::steady_clock::now() - _start) <= _maxTime, "execution taked too long"); } #else - TimeCheck(std::chrono::nanoseconds) { + BlockExecutionDurationAssert(std::chrono::nanoseconds) { } + #endif }; @@ -177,17 +178,32 @@ void TemperatureSampler::tick() { } BroadcastTemperatureStatistics TemperatureSampler::temperature_statistics() const { - const auto &samples = this->_samples; - std::size_t total_samples = samples.size(); + const auto &samples = this->_samples; + std::uint8_t total_samples = samples.size(); + + if (total_samples == 0) { + /// TODO rise error? + BroadcastTemperatureStatistics btemp; + btemp.channel_id = this->_channel; + btemp.has_temperatureStats = true; + + TemperatureStatistics &temp = btemp.temperatureStats; + temp.temp_c = 0; + temp.temp_avg_c = 0; + temp.n_samples = 0; + temp.temp_stddev_c = 0; + + return btemp; + } // Use only the last N samples or all if fewer - std::size_t count = std::max(std::min(this->_samplesNumber, total_samples), 1U); + std::size_t count = std::min(this->_samplesNumber, total_samples); auto start_it = samples.cend() - count; - auto sum = std::accumulate(start_it, samples.cend(), Sample{}); + auto sum = std::accumulate(start_it, samples.cend(), uint32_t{}); auto mean = sum / count; - auto sq_sum = std::accumulate(start_it, samples.cend(), Sample{}, [&](auto currentSum, auto sample) { + auto sq_sum = std::accumulate(start_it, samples.cend(), uint32_t{}, [&](auto currentSum, auto sample) { return currentSum + ((sample - mean) * (sample - mean)); }); @@ -198,16 +214,17 @@ BroadcastTemperatureStatistics TemperatureSampler::temperature_statistics() cons btemp.has_temperatureStats = true; TemperatureStatistics &temp = btemp.temperatureStats; - temp.temp_c = samples.back(); - temp.temp_avg_c = mean; + temp.temp_c = to_temperature(samples.back()); + temp.temp_avg_c = to_temperature(mean); temp.n_samples = count; - temp.temp_stddev_c = stddev; + temp.temp_stddev_c = adc_to_celsius_weight() * (float)stddev; return btemp; } TemperatureCurrent TemperatureSampler::temperature() const { - return TemperatureCurrent{.temp_c = this->_samples.back()}; + /// TODO rise error on empty buffer? + return TemperatureCurrent{.temp_c = to_temperature(this->_samples.back())}; } void TemperatureSampler::setSamplerConfig(const temperature_SamplerConfig &config) noexcept(false) { @@ -238,10 +255,19 @@ TemperatureSampler::Sample TemperatureSampler::to_temperature(uint32_t adcReadin return temperature; } -uint32_t TemperatureSampler::adc() const { - std::array samples{}; // TODO add emssage for samples fer measurement - constexpr auto ADC_RESOLUTION = 12; - const adc_dt_spec adc_spec = ADC_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 0); +float TemperatureSampler::adc_to_celsius_weight() const { + const auto tempMin = this->_Tmin; + const auto tempMax = this->_Tmax; + const auto adcMax = this->_adc_Tmax; + const auto adcMin = this->_adc_Tmin; + + return (tempMax - tempMin) / static_cast(adcMin - adcMax); +} + +adc_sample TemperatureSampler::adc() const { + std::array samples{}; // TODO add message for samples fer measurement + constexpr auto ADC_RESOLUTION = 12; + const adc_dt_spec adc_spec = ADC_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 0); const adc_sequence_options options = { .interval_us = 0, @@ -285,11 +311,9 @@ template constexpr bool unknownChannelError(const } TemperatureSamplerOrchestrator::TemperatureSamplerOrchestrator() - : // - _channel{0}, // - _temperatureSamplerChannels{TemperatureSampler{1}, TemperatureSampler{2}}, // - _samplerTickSem{0, 1}, _samplerTick{_samplerTickSem, std::chrono::milliseconds{1}}, // - _channelChangeSem{0, 1}, _channelChange{_channelChangeSem, std::chrono::seconds{1}} // + : // + _activeChannel{0}, // + _temperatureSamplerChannels{TemperatureSampler{1}, TemperatureSampler{2}} // { ULOG_INFO("Initialize message events"); @@ -461,6 +485,7 @@ void TemperatureSamplerOrchestrator::handle_filterRequest(const FilterRequest &r void TemperatureSamplerOrchestrator::action_samplersTick() { zephyr::semaphore::k_sem_take_now(_samplerTickSem); + for (auto &sampler : _temperatureSamplerChannels) { sampler.tick(); } @@ -472,8 +497,16 @@ void TemperatureSamplerOrchestrator::action_changeChannel() { sampler.deactivate(); } - _temperatureSamplerChannels.at(_channel).activate(); - _channel = (_channel + 1) % temperatureChannelsNr; + _temperatureSamplerChannels.at(_activeChannel).activate(); + + if (--_recalibrationTicks[_activeChannel] == 0) { + _recalibrationTicks[_activeChannel] = 60; + _samplerTick.stop(); // disable sampling for a while + _temperatureSamplerChannels.at(_activeChannel).calibration(); + _samplerTick.start(); // reenable sampling + } + + _activeChannel = (_activeChannel + 1) % temperatureChannelsNr; } int TemperatureSamplerThread::do_hardwarenInit() { @@ -499,16 +532,16 @@ uint32_t TemperatureSampler::takeStableRead() { const auto start = std::chrono::steady_clock::now(); ULOG_INFO("Starting ADC stable read"); - int maxIterations = 1000; - int STABILITY_COUNT_REQUIRED = 5; + int maxIterations = 100; + int STABILITY_COUNT_REQUIRED = 6; int STABILITY_THRESHOLD = 5; int prevAdc = 0; uint8_t stableCount = 0; while (stableCount < STABILITY_COUNT_REQUIRED && maxIterations > 0) { - std::this_thread::sleep_for(std::chrono::microseconds{1000}); // small delay between samples - uint16_t adcraw = adc(); + std::this_thread::sleep_for(std::chrono::microseconds{500}); // small delay between samples + auto adcraw = adc(); if (std::abs(adcraw - prevAdc) < STABILITY_THRESHOLD) { stableCount++; @@ -522,7 +555,7 @@ uint32_t TemperatureSampler::takeStableRead() { const auto stop = std::chrono::steady_clock::now(); auto ms = (stop - start).count() / 1000 / 1000; - auto itleft = (1000 - maxIterations); + auto itleft = (100 - maxIterations); ULOG_INFO("ADC stable read %d end after %lld ms in %d iterations", prevAdc, ms, itleft); return prevAdc; diff --git a/rims_app/src/temperature_measurements.hpp b/rims_app/src/temperature_measurements.hpp index 3480dc21b7a..b6b8ccd43c6 100644 --- a/rims_app/src/temperature_measurements.hpp +++ b/rims_app/src/temperature_measurements.hpp @@ -16,7 +16,7 @@ namespace rims { extern zephyr_fifo_buffer temperatureIngressQueue; extern zephyr_fifo_buffer temperatureEgressQueue; -static constexpr std::size_t MaxSampleSize = 16; +static constexpr std::size_t MaxSampleSize = 32; static constexpr uint8_t channelNumber = 2; using GetTemperatureRequest = temperature_GetTemperatureRequest; @@ -47,16 +47,17 @@ class TemperatureSampler { TemperatureSampler &operator=(TemperatureSampler &&) = delete; void tick(); - + void calibration(); + void setSamplerConfig(const temperature_SamplerConfig &config) noexcept(false); - + void activate(); void deactivate(); BroadcastTemperatureStatistics temperature_statistics() const; TemperatureCurrent temperature() const; + protected: - void calibration(); bool _running{false}; @@ -65,9 +66,10 @@ class TemperatureSampler { uint32_t takeStableRead(); - Sample to_temperature(uint32_t adc_value) const; - uint32_t adc() const; - Sample adc_take_sample() const; + Sample to_temperature(uint32_t adc_value) const; + float adc_to_celsius_weight() const; + adc_sample adc() const; + Sample adc_take_sample() const; uint32_t _adc_Tmin{}; uint32_t _adc_Tmax{}; @@ -75,12 +77,13 @@ class TemperatureSampler { float _Tmax{103.0f}; zephyr::semaphore::sem _samplerSem{0, 1}; - RecurringSemaphoreTimer _samplerTimer; - zephyr::semaphore::sem _broadcastSem{0, 1}; - RecurringSemaphoreTimer _broadcastTimer; + RecurringSemaphoreTimer _samplerTimer{_samplerSem, std::chrono::milliseconds{75}}; - ring_buffer _samples; - std::size_t _samplesNumber{MaxSampleSize}; + zephyr::semaphore::sem _broadcastSem{0, 1}; + RecurringSemaphoreTimer _broadcastTimer{_broadcastSem, std::chrono::seconds{10}}; + + ring_buffer _samples; + std::uint8_t _samplesNumber{MaxSampleSize}; const std::uint8_t _channel{}; }; @@ -104,16 +107,19 @@ class TemperatureSamplerOrchestrator { void action_samplersTick(); void action_changeChannel(); - uint8_t _channel{0}; + uint8_t _activeChannel{0}; + std::array _temperatureSamplerChannels; zephyr::semaphore::sem _samplerTickSem{0, 1}; - RecurringSemaphoreTimer _samplerTick; + RecurringSemaphoreTimer _samplerTick{_samplerTickSem, std::chrono::milliseconds{1}}; zephyr::semaphore::sem _channelChangeSem{0, 1}; - RecurringSemaphoreTimer _channelChange; - + RecurringSemaphoreTimer _channelChange{_channelChangeSem, std::chrono::seconds{1}}; + std::array _events; // event from timer and from Messenger + + uint8_t _recalibrationTicks[2] = {60,60}; // 2 minutes }; class TemperatureSamplerThread : public ZephyrThread { -- 2.45.2 From f50d9c7b480aa5bcc4c3be0f6da5aa3661d07d5d Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Thu, 24 Apr 2025 16:31:47 +0200 Subject: [PATCH 10/18] add wireid type --- rims_app/src/id.hpp | 125 ++++++++++++++ rims_app/src/main.cpp | 19 +-- rims_app/src/messenger.cpp | 197 ++++++++++------------ rims_app/src/messenger.hpp | 22 +-- rims_app/src/temperature_measurements.hpp | 2 +- rims_app/src/uart.cpp | 2 +- rims_app/src/uart.hpp | 2 +- 7 files changed, 235 insertions(+), 134 deletions(-) create mode 100644 rims_app/src/id.hpp diff --git a/rims_app/src/id.hpp b/rims_app/src/id.hpp new file mode 100644 index 00000000000..52811a2a856 --- /dev/null +++ b/rims_app/src/id.hpp @@ -0,0 +1,125 @@ +#pragma once + +#include + +namespace rims { + +/** + * @brief The ID class + * ID is max 29 bytes. + */ + +enum Priority { + High = 0x7, // + Mid = 0x3, + Low = 0 +}; +enum Type { + Broadcast = 0, // + Request = 1, + Response = 2 +}; + +struct WireFormatID { + int32_t reserved : 6 {0}; // bits 5..0 + uint32_t sender : 9 {0}; // bits 14..6 + uint32_t receiver : 9 {0}; // bits 23..15 + uint32_t type : 2 {0}; // bits 25..24 + uint32_t priority : 3 {0}; // bits 28..26 + uint32_t checksum : 3 {0}; // bits 31..29 (MSBs) + + constexpr uint32_t toWire() const { + uint32_t raw = 0; + // raw |= (reserved & 0x3F); + raw |= (sender & 0x1FF) << 6; + raw |= (receiver & 0x1FF) << 15; + raw |= (type & 0x03) << 24; + raw |= (priority & 0x07) << 26; + + // Now compute and insert checksum + uint8_t cs = compute_checksum3(raw); + raw |= (cs & 0x07) << 29; + + return raw; + } + + // swap sender and receiver + constexpr WireFormatID &swapEdpoints() { + auto tmp = sender; + sender = receiver; + receiver = tmp; + return *this; + } + + constexpr WireFormatID &makeResponse() { + type = static_cast(Type::Response); + swapEdpoints(); + return *this; + } + + constexpr WireFormatID &makeBroadcastFrom(uint32_t sender) { + setSender(sender); + type = Type::Broadcast; + setPriority(Priority::Low); + return *this; + } + + constexpr WireFormatID &setSender(uint16_t id) { + sender = id; + return *this; + } + constexpr WireFormatID &setReceiver(uint16_t id) { + receiver = id; + return *this; + } + constexpr WireFormatID &setPriority(Priority prior) { + priority = static_cast(prior); + return *this; + } + constexpr WireFormatID &setType(Type t) { + type = static_cast(t); + return *this; + } + constexpr bool isValid() const { + uint32_t raw = 0; + // raw |= (reserved & 0x3F); + raw |= (sender & 0x1FF) << 6; + raw |= (receiver & 0x1FF) << 15; + raw |= (type & 0x03) << 24; + raw |= (priority & 0x07) << 26; + + // Now compute and insert checksum + uint8_t expected = compute_checksum3(raw); + return expected == checksum; + } + + constexpr WireFormatID &fromWire(uint32_t packed) { + // reserved = packed & 0x3F; // bits 0..5 + sender = (packed >> 6) & 0x1FF; // bits 6..14 + receiver = (packed >> 15) & 0x1FF; // bits 15..23 + type = (packed >> 24) & 0x03; // bits 24..25 + priority = (packed >> 26) & 0x07; // bits 26..28 + checksum = (packed >> 29) & 0x07; // bits 29..31 (MSBs) + return *this; + } + + private: + constexpr static uint8_t compute_checksum3(uint32_t raw_id) { + // Mask out top 3 bits — ensure we only use bits 0..28 + raw_id &= 0x1FFFFFFF; + + // Simple XOR folding to reduce to 3 bits + uint8_t checksum = 0; + + // Fold 29 bits into 3 using 3-bit chunks + for (int i = 0; i < 29; i += 3) { + checksum ^= (raw_id >> i) & 0x07; // XOR each 3-bit group + } + + return checksum & 0x07; // final 3-bit result + } +}; + +static_assert(sizeof(uint32_t) == sizeof(WireFormatID)); + +} // namespace rims diff --git a/rims_app/src/main.cpp b/rims_app/src/main.cpp index 46c5a5dcfa0..a60e5be58b5 100644 --- a/rims_app/src/main.cpp +++ b/rims_app/src/main.cpp @@ -72,19 +72,17 @@ gpio_dt_spec ch_1_zcd = GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_zcd), gpios); gpio_dt_spec ch_2_zcd = GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_zcd), gpios); // pompka void mythread_analyzer_cb(struct thread_analyzer_info *info) { - ULOG_DEBUG( - "thread name, memfree, cpu : %s, %d, %d", - info->name, - info->stack_size - info->stack_used, - info->utilization // cpu usage in % - ); +// ULOG_DEBUG( +// "thread name, memfree, cpu : %s, %d, %d", +// info->name, +// info->stack_size - info->stack_used, +// info->utilization // cpu usage in % +// ); } int main() { using namespace rims; - const auto wait = []() { std::this_thread::sleep_for(std::chrono::milliseconds{100}); }; - messengerTread.init(messengerStack); uartThread.init(uartStack); temperatureSampler.init(temperatureSamplerStack); @@ -93,23 +91,18 @@ int main() { messengerTread->init_hw(); messengerTread->start(); - wait(); uartThread->init_hw(); uartThread->start(); - wait(); temperatureSampler->init_hw(); temperatureSampler->start(); - wait(); zcd->init_hw(); zcd->start(); - wait(); phaseModulation->init_hw(); phaseModulation->start(); - wait(); zephyr::gpio::pin_configure(status, GPIO_OUTPUT_INACTIVE); zephyr::gpio::pin_configure(gprelay_1_en, GPIO_OUTPUT_INACTIVE); diff --git a/rims_app/src/messenger.cpp b/rims_app/src/messenger.cpp index 0fd053f386e..b86e0849f50 100644 --- a/rims_app/src/messenger.cpp +++ b/rims_app/src/messenger.cpp @@ -17,6 +17,8 @@ #include "proto/message.pb.h" #include "proto/temperature.pb.h" +#include "id.hpp" + #include #include #include @@ -33,48 +35,13 @@ namespace rims { K_MSGQ_DEFINE(messenger_buffer_arrived_queue, sizeof(rims::buffer), 2, 1); +namespace { -/* -def encode_id(id_value: int) -> int: - """Encodes a 29-bit ID by adding a 3-bit checksum at the end.""" - data = id_value & 0x1FFFFFFF # Mask to ensure only 29 bits - checksum = (data ^ (data >> 3) ^ (data >> 6) ^ (data >> 9) ^ (data >> 12)) & 0x7 # Reduce to 3 bits - return (data << 3) | checksum # Store checksum in the last 3 bits - - -def verify_id(received_id: int) -> bool: - """Verifies if the received ID has a valid checksum.""" - data = received_id >> 3 # Extract the first 29 bits - received_checksum = received_id & 0x7 # Extract the last 3 bits (checksum) - - # Recalculate checksum from the extracted 29-bit data - calculated_checksum = (data ^ (data >> 3) ^ (data >> 6) ^ (data >> 9) ^ (data >> 12)) & 0x7 - - return received_checksum == calculated_checksum # Return True if checksum matches - - */ - -static constexpr bool idIsInvalid(uint32_t receivedID) { - // uint32_t data = receivedID >> 3; // Extract the first 29 bits - // uint32_t receivedChecksum = receivedID & 0x7; // Extract the last 3 bits (checksum) - - // // Recalculate checksum from the extracted 29-bit data - // uint32_t calculatedChecksum = (data ^ (data >> 3) ^ (data >> 6) ^ (data >> 9) ^ (data >> 12)) & 0x7; - - // return receivedChecksum == calculatedChecksum; // Return true if checksum matches - return false; -} - -static bool crcIsInvalid(std::span data, std::uint32_t crc) { +bool crcIsInvalid(std::span data, std::uint32_t crc) { return zephyr::crc::crc32_ieee(data) != crc; } -// static uint32_t encodeID(uint32_t id) { -// /// TODO error handling -// uint32_t data = id & 0x1FFFFFFF; // Mask to get the first 29 bits -// uint32_t checksum = (data ^ (data >> 3) ^ (data >> 6) ^ (data >> 9) ^ (data >> 12)) & 0x7; // Reduce to 3 bits -// return (data << 3) | checksum; // Store checksum in the last 3 bits -// } +} // namespace namespace cobs { class error : public std::exception {}; @@ -217,9 +184,10 @@ MessengerThread::MessengerThread(TStackBase &stack) : ZephyrThread{stack, 9, 0, zephyr::event_pool::k_init(_events.at(0), messenger_buffer_arrived_queue); for (auto &ar : _activeRequests) { - ar.type = -1; - ar.cb = nullptr; - ar.requestId = 0; + ar.wireId = {}; + ar.wireId.reserved = -1; // maark unused slot + ar.cb = {}; + ar.requestId = {}; } } @@ -245,50 +213,25 @@ void MessengerThread::threadMain() { } } -void rims::MessengerThread::event_temperatureEgress() { - temperatureEgressQueue.try_consume([&](temperature_EgressMessages &in) { - egressPussh(&in, temperature_EgressMessages_msg, 1, in.has_requestID ? std::optional{in.requestID} : std::nullopt); - }); -} - -void rims::MessengerThread::event_ctrlEgress() { - ctrlEgressQueue.try_consume([&](ctrl_EgressMessages &in) { - egressPussh(&in, ctrl_EgressMessages_msg, 2, in.has_requestID ? std::optional{in.requestID} : std::nullopt); - }); -} - -void rims::MessengerThread::event_configEgress() { - configEgressQueue.try_consume([&](config_EgressMessages &in) { - egressPussh(&in, config_EgressMessages_msg, 3, in.has_requestID ? std::optional{in.requestID} : std::nullopt); - }); -} - -void rims::MessengerThread::event_logEgress() { - logEgressFifoQueueBuffer.try_consume([&](log_EgressMessages &in) { - egressPussh(&in, log_EgressMessages_msg, 4, in.has_requestID ? std::optional{in.requestID} : std::nullopt); - }); -} - void MessengerThread::event_dataArrived() { try { + ULOG_INFO("Data arrived"); buffer buf; k_msgq_get(&messenger_buffer_arrived_queue, &buf, K_NO_WAIT); const std::size_t decoded_len = cobs::decode(buf.data, buf.data); pb_istream_t stream = pb_istream_from_buffer(buf.data.data(), decoded_len); - std::span submessage; - uint32_t id{}, crc{}; + std::span submessage{}; + WireFormatID id{}; + uint32_t crc{}; const auto decode_id = [&]() { try { - id = pb::decode_fixed32(stream); + id.fromWire(pb::decode_fixed32(stream)); } catch (const pb::error &e) { throw messenger::no_id{}; } - if (idIsInvalid(id)) { - throw messenger::bad_id{}; - } }; const auto decode_crc = [&]() { @@ -305,7 +248,6 @@ void MessengerThread::event_dataArrived() { auto submessageBegin = (pb_byte_t *)stream.state; pb::skip_bytes(stream, submessageSize); // skip data submessage = {submessageBegin, submessageSize}; - } catch (const pb::error &e) { throw messenger::no_data{}; } @@ -331,24 +273,28 @@ void MessengerThread::event_dataArrived() { decode(); decode(); + if (not id.isValid()) { + throw messenger::bad_id{}; + } if (crcIsInvalid(submessage, crc)) { throw messenger::bad_crc{}; } - stream = pb::istream_from_span(submessage); - switch (id) { - case 0x01: { /// TODO temperature endpoint ID - handle_temperatureIngressMsg(stream, buf.device); + /// we now got a proper frame, starting to decode a frame + stream = pb::istream_from_span(submessage); // throws on error, no need to check any status + switch (id.receiver) { // check which endpoint is the receiver + case 0x01: { /// TODO temperature endpoint ID + handle_temperatureIngressMsg(id, stream, buf.device); break; } case 0x02: /// TODO configuration endpoint ID { - handle_configIngressMsg(stream, buf.device); + handle_configIngressMsg(id, stream, buf.device); break; } case 0x03: /// TODO phase controll endpoint ID { - handle_ctrlIngressMsg(stream, buf.device); + handle_ctrlIngressMsg(id, stream, buf.device); break; } case 0x04: { @@ -378,56 +324,81 @@ void MessengerThread::event_dataArrived() { } } -void MessengerThread::handle_temperatureIngressMsg(pb_istream_t &stream, AsyncUART *cb) { - /// TODO assert acllback - /// register req id +void MessengerThread::event_temperatureEgress() { + temperatureEgressQueue.try_consume([&](temperature_EgressMessages &in) { + egressPush(&in, temperature_EgressMessages_msg, 1, in.has_requestID ? std::optional{in.requestID} : std::nullopt); + }); +} +void MessengerThread::event_ctrlEgress() { + ctrlEgressQueue.try_consume([&](ctrl_EgressMessages &in) { + egressPush(&in, ctrl_EgressMessages_msg, 2, in.has_requestID ? std::optional{in.requestID} : std::nullopt); + }); +} + +void MessengerThread::event_configEgress() { + configEgressQueue.try_consume([&](config_EgressMessages &in) { + egressPush(&in, config_EgressMessages_msg, 3, in.has_requestID ? std::optional{in.requestID} : std::nullopt); + }); +} + +void MessengerThread::event_logEgress() { + logEgressFifoQueueBuffer.try_consume([&](log_EgressMessages &in) { + egressPush(&in, log_EgressMessages_msg, 4, in.has_requestID ? std::optional{in.requestID} : std::nullopt); + }); +} + +void MessengerThread::handle_temperatureIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb) { temperatureIngressQueue.try_produce([&](temperature_IngressMessages &request) { request = temperature_IngressMessages_init_zero; decode(stream, temperature_IngressMessages_msg, &request); - putActiveRequest(1, request.requestID, cb); + putActiveRequest(id, request.requestID, cb); return true; }); } -void MessengerThread::handle_ctrlIngressMsg(pb_istream_t &stream, AsyncUART *cb) { - // register req id - ctrlIngressQueue.try_consume([&](ctrl_IngressMessages &request) { +void MessengerThread::handle_ctrlIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb) { + ctrlIngressQueue.try_produce([&](ctrl_IngressMessages &request) { request = ctrl_IngressMessages_init_zero; decode(stream, ctrl_IngressMessages_msg, &request); - putActiveRequest(2, request.requestID, cb); + putActiveRequest(id, request.requestID, cb); return true; }); } -void MessengerThread::handle_configIngressMsg(pb_istream_t &stream, AsyncUART *cb) { - // register req id - configIngressQueue.try_consume([&](config_IngressMessages &request) { +void MessengerThread::handle_configIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb) { + configIngressQueue.try_produce([&](config_IngressMessages &request) { request = config_IngressMessages_init_zero; decode(stream, config_IngressMessages_msg, &request); - putActiveRequest(3, request.requestID, cb); + putActiveRequest(id, request.requestID, cb); return true; }); } -void MessengerThread::egressPush(Message &message, int id, std::optional requestId) { - // set IF od sender - message.id = id; /// TODO make response ID +void MessengerThread::egressPush(Message &message, int wireId_receiver, std::optional requestId) { + // set as broadcast message by default, this way no message will be left without a ID + + message.id = WireFormatID{}.makeBroadcastFrom(wireId_receiver).toWire(); message.crc = zephyr::crc::crc32_ieee({message.data.bytes, message.data.size}); - // for requests, we have to have a callback function to return from where we received message if (requestId.has_value()) { - auto cb = takeActiveRequest(message.id, requestId.value()); - if (cb == nullptr) { - cb = defaultUart(); + auto requestData = takeActiveRequest(message.id, requestId.value()); + if (requestData.has_value()) { + if (requestData->cb == nullptr) { + requestData->cb = defaultUart(); + } + + requestData->wireId.makeResponse(); // switch sender/receiver, set type to response + message.id = requestData->wireId.toWire(); + transmit(message, requestData->cb); } - transmit(message, cb); + // if no request was sent before, just drop the message } else { transmit(message, defaultUart()); } } -void MessengerThread::egressPussh(void *submessage, const pb_msgdesc_t &fields, int id, std::optional requestId) { +void MessengerThread::egressPush(void *submessage, const pb_msgdesc_t &fields, int wireId_receiver, std::optional requestId) { // encode embedded message directly to the buffer of output message to save stack Message message = Message_init_zero; @@ -435,7 +406,7 @@ void MessengerThread::egressPussh(void *submessage, const pb_msgdesc_t &fields, pb::encode(ostream, fields, submessage); message.data.size = ostream.bytes_written; - egressPush(message, id, requestId); + egressPush(message, wireId_receiver, requestId); } void MessengerThread::decode(pb_istream_t &stream, const pb_msgdesc_t &fields, void *dest) const { @@ -462,26 +433,36 @@ bool MessengerThread::transmit(Message &msg, AsyncUART *cb) { return true; } -void MessengerThread::putActiveRequest(int type, uint32_t id, AsyncUART *cb) { +void MessengerThread::putActiveRequest(WireFormatID wireId, uint32_t id, AsyncUART *cb) { + if (wireId.type != Type::Request) // We only need to track requests + { + return; + } + for (auto &req : _activeRequests) { - if (req.type == -1) { // Find an empty slot - req.type = type; + if (req.wireId.reserved != 0) { // Find an empty slot + req.wireId = wireId; req.requestId = id; req.cb = cb; return; } } + throw messenger::request_queue_full{}; } -AsyncUART *MessengerThread::takeActiveRequest(int type, uint32_t id) { +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.type == type && req.requestId == id) { - req.type = -1; // Mark as available - return req.cb; + if (req.wireId.receiver == wireId_receiver && req.requestId == id) { + ret = req; // copy to return variable + req.wireId.reserved = -1; // mark as empty slot + break; } } - return {}; + + return ret; } } // namespace rims diff --git a/rims_app/src/messenger.hpp b/rims_app/src/messenger.hpp index 31fe0e938be..538191a048e 100644 --- a/rims_app/src/messenger.hpp +++ b/rims_app/src/messenger.hpp @@ -4,6 +4,8 @@ #include "uart.hpp" #include "zephyr/kernel.h" +#include "id.hpp" + #include "proto/message.pb.h" #include @@ -30,9 +32,9 @@ class MessengerThread : public ZephyrThread { private: struct requestData { - int type; - uint32_t requestId; - AsyncUART *cb; + WireFormatID wireId; // original ID of IngressMessage + uint32_t requestId; // requestID from message + AsyncUART *cb; }; void event_dataArrived(); @@ -41,18 +43,18 @@ class MessengerThread : public ZephyrThread { void event_configEgress(); void event_logEgress(); - void handle_temperatureIngressMsg(pb_istream_t &stream, AsyncUART *cb); - void handle_ctrlIngressMsg(pb_istream_t &stream, AsyncUART *cb); - void handle_configIngressMsg(pb_istream_t &stream, AsyncUART *cb); + void handle_temperatureIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb); + void handle_ctrlIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb); + void handle_configIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb); void decode(pb_istream_t &stream, const pb_msgdesc_t &fields, void *dest) const; bool transmit(Message &msg, AsyncUART *cb); void egressPush(Message &message, int id, std::optional requestId); - void egressPussh(void *submessage, const pb_msgdesc_t &fields, int id, std::optional requestId); - - void putActiveRequest(int type, uint32_t id, AsyncUART *cb); - AsyncUART *takeActiveRequest(int type, uint32_t id); + 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::array _events; std::array _activeRequests; diff --git a/rims_app/src/temperature_measurements.hpp b/rims_app/src/temperature_measurements.hpp index b6b8ccd43c6..11b47eb404c 100644 --- a/rims_app/src/temperature_measurements.hpp +++ b/rims_app/src/temperature_measurements.hpp @@ -80,7 +80,7 @@ class TemperatureSampler { RecurringSemaphoreTimer _samplerTimer{_samplerSem, std::chrono::milliseconds{75}}; zephyr::semaphore::sem _broadcastSem{0, 1}; - RecurringSemaphoreTimer _broadcastTimer{_broadcastSem, std::chrono::seconds{10}}; + RecurringSemaphoreTimer _broadcastTimer{_broadcastSem, std::chrono::seconds{1000}}; ring_buffer _samples; std::uint8_t _samplesNumber{MaxSampleSize}; diff --git a/rims_app/src/uart.cpp b/rims_app/src/uart.cpp index ef7e4f3f962..eec65222766 100644 --- a/rims_app/src/uart.cpp +++ b/rims_app/src/uart.cpp @@ -135,7 +135,6 @@ void AsyncUART::uartCallback(const device *dev, void *user_data) { } void AsyncUART::uartISR() { - __ASSERT(device_is_ready(_dev), "device needs to work"); try { while (uart_irq_update(_dev) && uart_irq_is_pending(_dev)) { if (uart_irq_rx_ready(_dev)) { @@ -189,6 +188,7 @@ uint8_t AsyncUART::rxByte() { } void AsyncUART::processMessage() { + int a; buffer buf{.data = {rxBuffer().data(), rxBuffer().size()}, .device = this}; switchRxBuffer(); k_msgq_put(&messenger_buffer_arrived_queue, &buf, K_MSEC(10)); diff --git a/rims_app/src/uart.hpp b/rims_app/src/uart.hpp index d70f7436cb5..c24a333a8b0 100644 --- a/rims_app/src/uart.hpp +++ b/rims_app/src/uart.hpp @@ -85,7 +85,7 @@ class AsyncUART { class UARTThread : public ZephyrThread { public: // set high priority, to not die when sending logs, but low enough that control is working without interrupt - UARTThread(TStackBase &stack) : ZephyrThread(stack, 4, 0){}; + UARTThread(TStackBase &stack) : ZephyrThread(stack, 4, 0, "UART"){}; // ZephyrThread interface protected: -- 2.45.2 From c5e1649ce50c1e3cf4b49ebac3f2718a83c670e4 Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Mon, 28 Apr 2025 15:48:52 +0200 Subject: [PATCH 11/18] changing operation for temperature sampler, fixing handling temperature messages --- rims_app/can.cpp | 18 +- rims_app/prj.conf | 15 +- rims_app/proto/configuration.proto | 8 +- rims_app/proto/ctrl.proto | 100 ++++--- rims_app/proto/log.proto | 8 +- rims_app/proto/temperature.proto | 99 ++++--- rims_app/proto/utilization.proto | 8 +- rims_app/src/circular_buffer.hpp | 1 + rims_app/src/common.hpp | 6 +- rims_app/src/config.cpp | 4 +- rims_app/src/config.hpp | 8 +- rims_app/src/id.hpp | 29 +- rims_app/src/log.cpp | 12 +- rims_app/src/log.hpp | 6 +- rims_app/src/main.cpp | 20 +- rims_app/src/messenger.cpp | 74 +++-- rims_app/src/phase_modulation.cpp | 32 +-- rims_app/src/phase_modulation.hpp | 4 +- rims_app/src/temperature_measurements.cpp | 332 ++++++++++------------ rims_app/src/temperature_measurements.hpp | 63 ++-- rims_app/src/uart.cpp | 41 +-- 21 files changed, 455 insertions(+), 433 deletions(-) diff --git a/rims_app/can.cpp b/rims_app/can.cpp index 425c46eb817..351cc225571 100644 --- a/rims_app/can.cpp +++ b/rims_app/can.cpp @@ -19,9 +19,9 @@ CAN_MSGQ_DEFINE(can_engress_queue, 5); CAN_MSGQ_DEFINE(can_ingress_queue, 5); // IPC -K_MSGQ_DEFINE(can_temperature_ingress_queue, temperature_IngressMessages_size, 2, 1); -K_MSGQ_DEFINE(can_config_ingress_queue, config_IngressMessages_size, 2, 1); -K_MSGQ_DEFINE(can_ctrl_ingress_queue, ctrl_IngressMessages_size, 2, 1); +K_MSGQ_DEFINE(can_temperature_ingress_queue, temperature_IngressMessage_size, 2, 1); +K_MSGQ_DEFINE(can_config_ingress_queue, config_IngressMessage_size, 2, 1); +K_MSGQ_DEFINE(can_ctrl_ingress_queue, ctrl_IngressMessage_size, 2, 1); void CAN_RX::loop() { struct can_frame frame; @@ -74,20 +74,20 @@ void CAN_RX::loop() { switch (frame.id) { case 0x01: { /// TODO temperature endpoint ID - temperature_IngressMessages msg; - decodeAndPublish(temperature_IngressMessages_fields, &msg, can_temperature_ingress_queue); + temperature_IngressMessage msg; + decodeAndPublish(temperature_IngressMessage_fields, &msg, can_temperature_ingress_queue); break; } case 0x02: /// TODO configuration endpoint ID { - config_IngressMessages msg; - decodeAndPublish(config_IngressMessages_fields, &msg, can_config_ingress_queue); + config_IngressMessage msg; + decodeAndPublish(config_IngressMessage_fields, &msg, can_config_ingress_queue); break; } case 0x03: /// TODO phase controll endpoint ID { - ctrl_IngressMessages msg; - decodeAndPublish(ctrl_IngressMessages_fields, &msg, can_ctrl_ingress_queue); + ctrl_IngressMessage msg; + decodeAndPublish(ctrl_IngressMessage_fields, &msg, can_ctrl_ingress_queue); break; } default: diff --git a/rims_app/prj.conf b/rims_app/prj.conf index 51c9260687c..b33d9b48964 100644 --- a/rims_app/prj.conf +++ b/rims_app/prj.conf @@ -1,5 +1,4 @@ # optimizations - CONFIG_GEN_ISR_TABLES=y CONFIG_GEN_IRQ_VECTOR_TABLE=y @@ -15,6 +14,10 @@ CONFIG_HW_STACK_PROTECTION=y CONFIG_MPU_STACK_GUARD=y CONFIG_ASSERT=y +CONFIG_LOG=n +CONFIG_LOG_MODE_IMMEDIATE=n # Log messages are output immediately +CONFIG_LOG_BACKEND_UART=n # Use UART for log output + # modules CONFIG_NANOPB=y CONFIG_CRC=y @@ -40,16 +43,10 @@ CONFIG_BOOT_BANNER=n CONFIG_SERIAL=y # not working as expected for TX -#CONFIG_UART_ASYNC_API=n -# CONFIG_DMA=n -# CONFIG_UART_INTERRUPT_DRIVEN=y -# -CONFIG_ADC=y -CONFIG_LOG=n -CONFIG_LOG_MODE_IMMEDIATE=n # Log messages are output immediately -CONFIG_LOG_BACKEND_UART=n # Use UART for log output +# subsystems +CONFIG_ADC=y CONFIG_MAIN_STACK_SIZE=1500 diff --git a/rims_app/proto/configuration.proto b/rims_app/proto/configuration.proto index e5fa3b9d906..87a2ba4dd19 100644 --- a/rims_app/proto/configuration.proto +++ b/rims_app/proto/configuration.proto @@ -8,12 +8,12 @@ enum Error { } // only those messages are send -message IngressMessages +message IngressMessage { - uint32 requestID = 255; // Unique request ID + uint32 request_id = 255; }; -message EgressMessages +message EgressMessage { - optional uint32 requestID = 255; // Unique request ID + optional uint32 request_id = 255; }; diff --git a/rims_app/proto/ctrl.proto b/rims_app/proto/ctrl.proto index 05e70e747d6..34de545fd05 100644 --- a/rims_app/proto/ctrl.proto +++ b/rims_app/proto/ctrl.proto @@ -8,45 +8,71 @@ enum Error { WrongMode = 1; UnsupportedMode = 2; - + WrongChannel = 3; UnknownError = 32; } -// algorithm used to control the heating element on given channel +// Power control method used to adjust the heating element on a given channel. +enum PowerControlAlgorithm { + // GroupModulation adjusts power by delivering full AC sine wave cycles in groups. + // It delivers N full cycles ON followed by M cycles OFF (zero voltage). + // Example: For 50% power, it might deliver 5 cycles on, then 5 cycles off. + // To modyfy number of grouped ac cycles, use GroupModulationConfigRequest + GroupModulation = 0; + + // PhaseModulation adjusts power by controlling the phase angle of each AC cycle. + // By cutting off part of the sine wave, it achieves finer control over the delivered power. + // Example: A phase angle of 90 degrees allows only half of the cycle to be applied. + PhaseModulation = 1; +} + +// Mode of operation used to control the heating element on a given channel. enum ModeOfOperation { Disabled = 0; - // GroupModulation mode adjusts power by delivering complete AC sine wave cycles in - // groups. It delivers N full cycles of the waveform followed by M cycles of zero - // voltage (off state). Example: For 50% power, it may deliver 5 cycles on followed - // by 5 cycles off. - GroupModulation = 1; + // ConstantTemperature maintains a target temperature using a feedback loop. + // The system adjusts the heating power dynamically to keep the temperature stable. + ConstantTemperature = 3; - // PhaseModulation mode adjusts power by controlling the phase angle of the AC waveform. - // By cutting off a portion of each sine wave cycle, it achieves finer control of the power - // output. Example: Setting a phase angle of 90 degrees allows only half of the waveform to - // be applied. - PhaseModulation = 2; + // TemperatureSlope controls the rate of temperature change over time. + // Example: Heating at a rate of 1°C per minute by adjusting power output dynamically, holds + // temperature after that. + // Useful for processes requiring a controlled temperature ramp. + TemperatureSlope = 4; - // ConstantTemperatureModulation adjusts power based on a feedback loop maintaining a set - // temperature. The system monitors the temperature and modulates power accordingly to keep - // it constant. This can involve dynamically switching between modes or adjusting power - // level. - ConstantTemperatureModulation = 3; + // ConstantPower applies a fixed power output using the selected power control algorithm. + // No feedback from temperature sensors is used to adjust the output dynamically. + ManualPower = 5; - // TemperatureSlopeModulation controls the heating element to achieve a specified - // temperature change rate. Example: Heating up at a rate of 1°C per minute by adjusting - // power output dynamically. This mode is useful when a precise temperature ramp is required - // over time. - TemperatureSlopeModulation = 4; + // Maintains the temperature within a specified range (min to max). + // Turns heating on/off to stay inside the window. + // Useful for simple thermostatic control. + TemperatureWindow = 9; + + // Special mode for automatic tuning of PID parameters based on system response. + // The system applies test stimuli and calculates optimal regulation parameters. + AutoTune = 7; +} + +message PowerControlAlgorithmRequest +{ + uint32 channel_id = 1; + optional ModeOfOperation mode = 2; +} + +message PowerControlAlgorithmResponse +{ + uint32 channel_id = 1; + PowerControlAlgorithm mode = 2; + optional Error error = 254; } message ModeOfOperationRequest { uint32 channel_id = 1; - optional ModeOfOperation mode = 2; // null indicates a reqd of current mode + optional ModeOfOperation mode = 2; } message ModeOfOperationResponse @@ -131,33 +157,33 @@ message PhaseModulationControlResponse } // only those messages are send through interface -message IngressMessages +message IngressMessage { - uint32 requestID = 255; // Unique request ID + uint32 request_id = 255; oneof data { - ModeOfOperationRequest modeOfOperationRequest = 1; + ModeOfOperationRequest mode_of_operation_request = 1; - GroupModulationConfigRequest groupModulationConfigRequest = 2; - GroupModulationControlRequest groupModulationControlRequest = 3; + GroupModulationConfigRequest group_modulation_config_request = 2; + GroupModulationControlRequest group_modulation_control_request = 3; - PhaseModulationConfigRequest phaseModulationConfigRequest = 4; - PhaseModulationControlRequest phaseModulationControlRequest = 5; + PhaseModulationConfigRequest phase_modulation_config_request = 4; + PhaseModulationControlRequest phase_modulation_control_request = 5; } }; -message EgressMessages +message EgressMessage { - optional uint32 requestID = 255; // Unique request ID + optional uint32 request_id = 255; oneof data { - ModeOfOperationResponse modeOfOperationResponse = 1; + ModeOfOperationResponse mode_of_operation_response = 1; - GroupModulationConfigResponse groupModulationConfigResponse = 2; - GroupModulationControlResponse groupModulationControlResponse = 3; + GroupModulationConfigResponse group_modulation_config_response = 2; + GroupModulationControlResponse group_modulation_control_response = 3; - PhaseModulationConfigResponse phaseModulationConfigResponse = 4; - PhaseModulationControlResponse phaseModulationControlResponse = 5; + PhaseModulationConfigResponse phase_modulation_config_response = 4; + PhaseModulationControlResponse phase_modulation_control_response = 5; // BROADCAST } }; diff --git a/rims_app/proto/log.proto b/rims_app/proto/log.proto index 1b051454700..a5760c4d9e3 100644 --- a/rims_app/proto/log.proto +++ b/rims_app/proto/log.proto @@ -34,18 +34,18 @@ message LogEntry } // only those messages are send through wire at temperature endpoint -message IngressMessages +message IngressMessage { - uint32 requestID = 255; + uint32 request_id = 255; oneof data { LogLevelRequest logLevelRequest = 1; } }; -message EgressMessages +message EgressMessage { - optional uint32 requestID = 255; + optional uint32 request_id = 255; oneof data { LogLevelResponse logLevelResponse = 1; diff --git a/rims_app/proto/temperature.proto b/rims_app/proto/temperature.proto index 6ec114a5410..ea5ef06850d 100644 --- a/rims_app/proto/temperature.proto +++ b/rims_app/proto/temperature.proto @@ -34,9 +34,15 @@ enum FilterType { } /* CONFIGURATION */ +// Only optional values will be set on device message SamplerConfig { - optional uint32 samples = 3; + // Number of samples to take for each temperature measurement. + // The average of these samples is treated as a single measurement. + optional uint32 oversampling = 3; + + // Time interval between consecutive measurements, in milliseconds. + // This applies to single channel. optional uint32 period_ms = 4; } @@ -66,13 +72,13 @@ message TemperatureCurrent } // Statictics of temperature on given channel -message TemperatureStatistics +message TemperatureSummary { - // last update + // very last update float temp_c = 1; - // average from last nsamples in Celcius - float temp_avg_c = 2; + // filtered from last nsamples in Celcius + float temp_filtered_c = 2; // standard deviation of temperature on given channel from last nsamples in Celcius float temp_stddev_c = 3; @@ -103,19 +109,19 @@ message FilterConfigRequest { uint32 channel_id = 1; // skips info about current parameters in response message - bool skipFullResponse = 5; + bool skip_full_response = 5; - optional KalmanFilterParams kalmanFilterParams = 2; - optional EMAFilterParams emaFilterParams = 3; - optional AverageFilterParams averageFilterParams = 4; + optional KalmanFilterParams kalman_pilter_params = 2; + optional EMAFilterParams ema_filter_params = 3; + optional AverageFilterParams average_filter_params = 4; } message FilterConfigResponse { uint32 channel_id = 1; - - KalmanFilterParams kalmanFilterParams = 2; - EMAFilterParams emaFilterParams = 3; - AverageFilterParams averageFilterParams = 4; + + KalmanFilterParams kalman_filter_params = 2; + EMAFilterParams ema_filter_params = 3; + AverageFilterParams average_filter_params = 4; optional Error error = 254; } @@ -123,28 +129,16 @@ message FilterConfigResponse message FilterRequest { uint32 channel_id = 1; - optional FilterType filterType = 2; + optional FilterType filter_type = 2; } message FilterResponse { uint32 channel_id = 1; - FilterType filterType = 2; + FilterType filter_type = 2; optional Error error = 254; } // API -// Read -message GetCurrentTemperatureRequest -{ - uint32 channel_id = 1; -}; -message GetCurrentTemperatureResponse -{ - uint32 channel_id = 1; - TemperatureCurrent temperatureCurrent = 2; - optional Error error = 254; -} - // Read message GetTemperatureRequest { @@ -153,48 +147,61 @@ message GetTemperatureRequest message GetTemperatureResponse { uint32 channel_id = 1; - TemperatureStatistics temperatureStats = 2; + TemperatureCurrent temperature_current = 2; optional Error error = 254; } -// broadcast -message BroadcastTemperatureStatistics { +// Read +message GetTemperatureSummaryRequest +{ uint32 channel_id = 1; - TemperatureStatistics temperatureStats = 2; +}; +message GetTemperatureSummaryResponse +{ + uint32 channel_id = 1; + TemperatureSummary temperature_summary = 2; + optional Error error = 254; +} + +// broadcast +message BroadcastTemperatureSummary +{ + uint32 channel_id = 1; + TemperatureSummary temperature_summary = 2; } // only those messages are send through wire. -message IngressMessages +message IngressMessage { - uint32 requestID = 255; + uint32 request_id = 255; oneof data { // data access - GetCurrentTemperatureRequest currentTemperatureRequest = 1; - GetTemperatureRequest temperatureRequest = 2; + GetTemperatureRequest temperature_request = 1; + GetTemperatureSummaryRequest temperature_summary_request = 2; // configuration - SamplerConfigRequest samplerConfigRequest = 3; - FilterConfigRequest filterConfigRequest = 4; - FilterRequest filterRequest = 5; + SamplerConfigRequest sampler_config_request = 3; + FilterConfigRequest filter_config_request = 4; + FilterRequest filter_request = 5; } }; -message EgressMessages +message EgressMessage { - optional uint32 requestID = 255; + optional uint32 request_id = 255; oneof data { // data access - GetCurrentTemperatureResponse currentTemperatureResponse = 1; - GetTemperatureResponse temperatureResponse = 2; + GetTemperatureResponse temperature_response = 1; + GetTemperatureSummaryResponse temperature_summary_response = 2; // configuration - SamplerConfigResponse samplerConfigResponse = 3; - FilterConfigResponse filterConfigResponse = 4; - FilterResponse filterResponse = 5; + SamplerConfigResponse sampler_config_response = 3; + FilterConfigResponse filter_config_response = 4; + FilterResponse filter_response = 5; // broadcast - BroadcastTemperatureStatistics broadcastTemperatureStatistics = 16; + BroadcastTemperatureSummary broadcast_temperature_summary = 16; } }; diff --git a/rims_app/proto/utilization.proto b/rims_app/proto/utilization.proto index d265246fb1b..c2cfef7bc74 100644 --- a/rims_app/proto/utilization.proto +++ b/rims_app/proto/utilization.proto @@ -12,14 +12,14 @@ message Utilization }; // only those messages are send through wire at temperature endpoint -message IngressMessages +message IngressMessage { - uint32 requestID = 255; + uint32 request_id = 255; }; -message EgressMessages +message EgressMessage { - optional uint32 requestID = 255; + optional uint32 request_id = 255; oneof data { // broadcast diff --git a/rims_app/src/circular_buffer.hpp b/rims_app/src/circular_buffer.hpp index 9b99345a7d8..13b5057e4b1 100644 --- a/rims_app/src/circular_buffer.hpp +++ b/rims_app/src/circular_buffer.hpp @@ -22,6 +22,7 @@ template class static_vector { } constexpr void clean() noexcept { /// TODO call dtors + buffer.fill(0); count = 0; } diff --git a/rims_app/src/common.hpp b/rims_app/src/common.hpp index 3bc3f6b52d6..005f1df1f5b 100644 --- a/rims_app/src/common.hpp +++ b/rims_app/src/common.hpp @@ -2,6 +2,7 @@ #include "log.hpp" #include "syscalls/kernel.h" +#include "zephyr/sys/time_units.h" #include "zephyr/sys_clock.h" #include @@ -32,6 +33,7 @@ constexpr bool operator==(const k_timeout_t &lhs, const k_timeout_t &rhs) { inline constexpr static k_timeout_t chronoToKTimeout(std::chrono::nanoseconds duration) { return K_NSEC(duration.count()); } + static_assert(chronoToKTimeout(std::chrono::milliseconds{345}) == K_MSEC(345)); static_assert(chronoToKTimeout(std::chrono::microseconds{667}) == K_USEC(667)); static_assert(chronoToKTimeout(std::chrono::seconds{2}) == K_SECONDS(2)); @@ -83,10 +85,10 @@ class Timer { } constexpr std::chrono::microseconds interval() const { - return std::chrono::microseconds{k_ticks_to_us_floor64(timer_.timeout.dticks)}; + return std::chrono::microseconds{k_ticks_to_us_floor64(_interval.ticks)}; } constexpr std::chrono::microseconds period() const { - return std::chrono::microseconds{k_ticks_to_us_floor64(timer_.period.ticks)}; + return std::chrono::microseconds{k_ticks_to_us_floor64(_period.ticks)}; } public: diff --git a/rims_app/src/config.cpp b/rims_app/src/config.cpp index 974df6ecf5e..071079be662 100644 --- a/rims_app/src/config.cpp +++ b/rims_app/src/config.cpp @@ -6,7 +6,7 @@ namespace rims { K_FIFO_DEFINE(configIngress); K_FIFO_DEFINE(configEgress); -zephyr_fifo_buffer configIngressQueue{configIngress}; -zephyr_fifo_buffer configEgressQueue{configEgress}; +zephyr_fifo_buffer configIngressQueue{configIngress}; +zephyr_fifo_buffer configEgressQueue{configEgress}; } // namespace rims diff --git a/rims_app/src/config.hpp b/rims_app/src/config.hpp index 33dfccaa328..77f221642f8 100644 --- a/rims_app/src/config.hpp +++ b/rims_app/src/config.hpp @@ -3,9 +3,9 @@ #include "circular_buffer.hpp" #include "proto/configuration.pb.h" -namespace rims{ +namespace rims { -extern zephyr_fifo_buffer configIngressQueue; -extern zephyr_fifo_buffer configEgressQueue; +extern zephyr_fifo_buffer configIngressQueue; +extern zephyr_fifo_buffer configEgressQueue; -} +} // namespace rims diff --git a/rims_app/src/id.hpp b/rims_app/src/id.hpp index 52811a2a856..4082382f0b3 100644 --- a/rims_app/src/id.hpp +++ b/rims_app/src/id.hpp @@ -21,18 +21,18 @@ enum Type { }; struct WireFormatID { - int32_t reserved : 6 {0}; // bits 5..0 - uint32_t sender : 9 {0}; // bits 14..6 - uint32_t receiver : 9 {0}; // bits 23..15 - uint32_t type : 2 {0}; // bits 25..24 - uint32_t priority : 3 {0}; // bits 28..26 - uint32_t checksum : 3 {0}; // bits 31..29 (MSBs) + int32_t reserved : 4 {0}; // bits 3..0 + uint32_t sender : 10 {0}; // bits 13..4 + uint32_t receiver : 10 {0}; // bits 23..14 + uint32_t type : 2 {0}; // bits 25..24 + uint32_t priority : 3 {0}; // bits 28..26 + uint32_t checksum : 3 {0}; // bits 31..29 (MSBs) constexpr uint32_t toWire() const { uint32_t raw = 0; // raw |= (reserved & 0x3F); - raw |= (sender & 0x1FF) << 6; - raw |= (receiver & 0x1FF) << 15; + raw |= (sender & 0x3FF) << 4; + raw |= (receiver & 0x3FF) << 14; raw |= (type & 0x03) << 24; raw |= (priority & 0x07) << 26; @@ -83,8 +83,8 @@ struct WireFormatID { constexpr bool isValid() const { uint32_t raw = 0; // raw |= (reserved & 0x3F); - raw |= (sender & 0x1FF) << 6; - raw |= (receiver & 0x1FF) << 15; + raw |= (sender & 0x3FF) << 4; + raw |= (receiver & 0x3FF) << 14; raw |= (type & 0x03) << 24; raw |= (priority & 0x07) << 26; @@ -95,8 +95,8 @@ struct WireFormatID { constexpr WireFormatID &fromWire(uint32_t packed) { // reserved = packed & 0x3F; // bits 0..5 - sender = (packed >> 6) & 0x1FF; // bits 6..14 - receiver = (packed >> 15) & 0x1FF; // bits 15..23 + sender = (packed >> 4) & 0x3FF; // bits 6..14 + receiver = (packed >> 14) & 0x3FF; // bits 15..23 type = (packed >> 24) & 0x03; // bits 24..25 priority = (packed >> 26) & 0x07; // bits 26..28 checksum = (packed >> 29) & 0x07; // bits 29..31 (MSBs) @@ -121,5 +121,8 @@ struct WireFormatID { }; static_assert(sizeof(uint32_t) == sizeof(WireFormatID)); - +constexpr auto testdata = WireFormatID{}.setPriority(Priority::Mid).setSender(999).setReceiver(1); +static_assert(WireFormatID{}.fromWire(testdata.toWire()).priority == Priority::Mid); +static_assert(WireFormatID{}.fromWire(testdata.toWire()).sender == 999); +static_assert(WireFormatID{}.fromWire(testdata.toWire()).receiver == 1); } // namespace rims diff --git a/rims_app/src/log.cpp b/rims_app/src/log.cpp index 834181ae02d..c40c6f601a2 100644 --- a/rims_app/src/log.cpp +++ b/rims_app/src/log.cpp @@ -1,12 +1,12 @@ #include "log.hpp" #include "zephyr/kernel.h" #include -#include #include +#include namespace rims { K_FIFO_DEFINE(klogFifoQueue); -zephyr_fifo_buffer logEgressFifoQueueBuffer{klogFifoQueue}; +zephyr_fifo_buffer logEgressFifoQueueBuffer{klogFifoQueue}; static std::size_t g_droppedLogs{0}; @@ -66,11 +66,11 @@ void Log::formatLog(Level level, const char *fmt, va_list args) const { // handle case when queue if full and we are waitnig for free space bool ok{false}; do { - ok = logEgressFifoQueueBuffer.try_produce([&](log_EgressMessages &logmsg) { - logmsg = log_EgressMessages_init_zero; + ok = logEgressFifoQueueBuffer.try_produce([&](log_EgressMessage &logmsg) { + logmsg = log_EgressMessage_init_zero; - logmsg.has_requestID = false; - logmsg.which_data = log_EgressMessages_logEntry_tag; + logmsg.has_request_id = false; + logmsg.which_data = log_EgressMessage_logEntry_tag; logmsg.data.logEntry.systick = k_uptime_ticks(); logmsg.data.logEntry.level = toPbLogLevel(level); diff --git a/rims_app/src/log.hpp b/rims_app/src/log.hpp index 993345bf50f..51dba9f153a 100644 --- a/rims_app/src/log.hpp +++ b/rims_app/src/log.hpp @@ -19,11 +19,11 @@ enum class LogLevel : int { // Critical = 4 }; -extern zephyr_fifo_buffer logIngressFifoQueueBuffer; -extern zephyr_fifo_buffer logEgressFifoQueueBuffer; +extern zephyr_fifo_buffer logIngressFifoQueueBuffer; +extern zephyr_fifo_buffer logEgressFifoQueueBuffer; /// TODO move -static LogLevel g_logLevel = LogLevel::Debug; +static LogLevel g_logLevel = LogLevel::Error; class Log { public: diff --git a/rims_app/src/main.cpp b/rims_app/src/main.cpp index a60e5be58b5..7874a698246 100644 --- a/rims_app/src/main.cpp +++ b/rims_app/src/main.cpp @@ -72,12 +72,12 @@ gpio_dt_spec ch_1_zcd = GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_zcd), gpios); gpio_dt_spec ch_2_zcd = GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_zcd), gpios); // pompka void mythread_analyzer_cb(struct thread_analyzer_info *info) { -// ULOG_DEBUG( -// "thread name, memfree, cpu : %s, %d, %d", -// info->name, -// info->stack_size - info->stack_used, -// info->utilization // cpu usage in % -// ); + // ULOG_INFO( + // "thread name, memfree, cpu : %s, %d, %d", + // info->name, + // info->stack_size - info->stack_used, + // info->utilization // cpu usage in % + // ); } int main() { @@ -91,16 +91,12 @@ int main() { messengerTread->init_hw(); messengerTread->start(); - uartThread->init_hw(); uartThread->start(); - temperatureSampler->init_hw(); temperatureSampler->start(); - zcd->init_hw(); zcd->start(); - phaseModulation->init_hw(); phaseModulation->start(); @@ -110,10 +106,10 @@ int main() { // zephyr::gpio::pin_toggle_dt(gprelay_2_en); while (1) { - std::this_thread::sleep_for(std::chrono::seconds{10}); + std::this_thread::sleep_for(std::chrono::seconds{2}); thread_analyzer_run(mythread_analyzer_cb, 0); - + // ULOG_INFO("PING"); // const unsigned char data[] = {'b', 0x55}; // uart_fifo_fill(defaultUart()->_dev, data, 1); } diff --git a/rims_app/src/messenger.cpp b/rims_app/src/messenger.cpp index b86e0849f50..56f1276fd62 100644 --- a/rims_app/src/messenger.cpp +++ b/rims_app/src/messenger.cpp @@ -32,6 +32,7 @@ #include "cobs.h" #include "zephyr.hpp" +#include "zephyr/sys/__assert.h" namespace rims { K_MSGQ_DEFINE(messenger_buffer_arrived_queue, sizeof(rims::buffer), 2, 1); @@ -214,18 +215,17 @@ void MessengerThread::threadMain() { } void MessengerThread::event_dataArrived() { + std::span submessage{}; + WireFormatID id{}; + uint32_t crc{}; + buffer buf; + try { - ULOG_INFO("Data arrived"); - buffer buf; k_msgq_get(&messenger_buffer_arrived_queue, &buf, K_NO_WAIT); const std::size_t decoded_len = cobs::decode(buf.data, buf.data); pb_istream_t stream = pb_istream_from_buffer(buf.data.data(), decoded_len); - std::span submessage{}; - WireFormatID id{}; - uint32_t crc{}; - const auto decode_id = [&]() { try { id.fromWire(pb::decode_fixed32(stream)); @@ -274,12 +274,13 @@ void MessengerThread::event_dataArrived() { decode(); if (not id.isValid()) { + ULOG_ERROR("got from {%d} to {%d} with broken ID", id.sender, id.receiver); throw messenger::bad_id{}; } if (crcIsInvalid(submessage, crc)) { + ULOG_ERROR("got from {%d} to {%d} with broken data", id.sender, id.receiver); throw messenger::bad_crc{}; } - /// we now got a proper frame, starting to decode a frame stream = pb::istream_from_span(submessage); // throws on error, no need to check any status switch (id.receiver) { // check which endpoint is the receiver @@ -307,16 +308,27 @@ void MessengerThread::event_dataArrived() { } } } catch (const messenger::error &e) { + ULOG_ERROR("got from {%d} to {%d} with broken data messenger::error", id.sender, id.receiver); Message message = Message_init_zero; message.error = e._err; message.has_error = true; egressPush(message, 16, std::nullopt); - } catch (const cobs::error *e) { - Message message = Message_init_zero; + } catch (const cobs::decode_error &e) { + ULOG_ERROR("got from {%d} to {%d} with broken data cobs::error{%d}", id.sender, id.receiver, e.ret); + ULOG_ERROR("buf bytes{%d}", buf.data.size_bytes()); + + Message message = Message_init_zero; + + if (buf.data.size_bytes() < 250) { + memcpy(message.data.bytes, buf.data.data(), buf.data.size_bytes()); + message.data.size = buf.data.size_bytes(); + } + message.error = Error_UnknownId; message.has_error = true; egressPush(message, 16, std::nullopt); } catch (const std::exception &e) { + ULOG_ERROR("got from {%d} to {%d} with broken data std::exception", id.sender, id.receiver); Message message = Message_init_zero; message.error = Error_UnknownId; message.has_error = true; @@ -325,64 +337,64 @@ void MessengerThread::event_dataArrived() { } void MessengerThread::event_temperatureEgress() { - temperatureEgressQueue.try_consume([&](temperature_EgressMessages &in) { - egressPush(&in, temperature_EgressMessages_msg, 1, in.has_requestID ? std::optional{in.requestID} : std::nullopt); + temperatureEgressQueue.try_consume([&](temperature_EgressMessage &out) { + egressPush(&out, temperature_EgressMessage_msg, 1, out.has_request_id ? std::optional{out.request_id} : std::nullopt); }); } void MessengerThread::event_ctrlEgress() { - ctrlEgressQueue.try_consume([&](ctrl_EgressMessages &in) { - egressPush(&in, ctrl_EgressMessages_msg, 2, in.has_requestID ? std::optional{in.requestID} : std::nullopt); + ctrlEgressQueue.try_consume([&](ctrl_EgressMessage &out) { + egressPush(&out, ctrl_EgressMessage_msg, 2, out.has_request_id ? std::optional{out.request_id} : std::nullopt); }); } void MessengerThread::event_configEgress() { - configEgressQueue.try_consume([&](config_EgressMessages &in) { - egressPush(&in, config_EgressMessages_msg, 3, in.has_requestID ? std::optional{in.requestID} : std::nullopt); + configEgressQueue.try_consume([&](config_EgressMessage &out) { + egressPush(&out, config_EgressMessage_msg, 3, out.has_request_id ? std::optional{out.request_id} : std::nullopt); }); } void MessengerThread::event_logEgress() { - logEgressFifoQueueBuffer.try_consume([&](log_EgressMessages &in) { - egressPush(&in, log_EgressMessages_msg, 4, in.has_requestID ? std::optional{in.requestID} : std::nullopt); + logEgressFifoQueueBuffer.try_consume([&](log_EgressMessage &out) { + egressPush(&out, log_EgressMessage_msg, 4, out.has_request_id ? std::optional{out.request_id} : std::nullopt); }); } void MessengerThread::handle_temperatureIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb) { - temperatureIngressQueue.try_produce([&](temperature_IngressMessages &request) { - request = temperature_IngressMessages_init_zero; - decode(stream, temperature_IngressMessages_msg, &request); - putActiveRequest(id, request.requestID, cb); + temperatureIngressQueue.try_produce([&](temperature_IngressMessage &request) { + request = temperature_IngressMessage_init_zero; + decode(stream, temperature_IngressMessage_msg, &request); + putActiveRequest(id, request.request_id, cb); return true; }); } void MessengerThread::handle_ctrlIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb) { - ctrlIngressQueue.try_produce([&](ctrl_IngressMessages &request) { - request = ctrl_IngressMessages_init_zero; - decode(stream, ctrl_IngressMessages_msg, &request); - putActiveRequest(id, request.requestID, cb); + ctrlIngressQueue.try_produce([&](ctrl_IngressMessage &request) { + request = ctrl_IngressMessage_init_zero; + decode(stream, ctrl_IngressMessage_msg, &request); + putActiveRequest(id, request.request_id, cb); return true; }); } void MessengerThread::handle_configIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb) { - configIngressQueue.try_produce([&](config_IngressMessages &request) { - request = config_IngressMessages_init_zero; - decode(stream, config_IngressMessages_msg, &request); - putActiveRequest(id, request.requestID, cb); + configIngressQueue.try_produce([&](config_IngressMessage &request) { + request = config_IngressMessage_init_zero; + decode(stream, config_IngressMessage_msg, &request); + putActiveRequest(id, request.request_id, cb); return true; }); } void MessengerThread::egressPush(Message &message, int wireId_receiver, std::optional requestId) { // set as broadcast message by default, this way no message will be left without a ID - message.id = WireFormatID{}.makeBroadcastFrom(wireId_receiver).toWire(); message.crc = zephyr::crc::crc32_ieee({message.data.bytes, message.data.size}); + // for requests, we have to have a callback function to return from where we received message if (requestId.has_value()) { - auto requestData = takeActiveRequest(message.id, requestId.value()); + auto requestData = takeActiveRequest(wireId_receiver, requestId.value()); if (requestData.has_value()) { if (requestData->cb == nullptr) { requestData->cb = defaultUart(); diff --git a/rims_app/src/phase_modulation.cpp b/rims_app/src/phase_modulation.cpp index 7788a8499b9..c050ea461e6 100644 --- a/rims_app/src/phase_modulation.cpp +++ b/rims_app/src/phase_modulation.cpp @@ -61,8 +61,8 @@ struct unsuported_mode : public error { K_FIFO_DEFINE(ctrlIngress); K_FIFO_DEFINE(ctrlEggress); -zephyr_fifo_buffer ctrlIngressQueue{ctrlIngress}; -zephyr_fifo_buffer ctrlEgressQueue{ctrlEggress}; +zephyr_fifo_buffer ctrlIngressQueue{ctrlIngress}; +zephyr_fifo_buffer ctrlEgressQueue{ctrlEggress}; constexpr std::array pins = { gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_en), gpios), @@ -167,36 +167,36 @@ void PhaseModulationOrchestrator::event_zeroCrossDetection() { void PhaseModulationOrchestrator::event_ctrlMessageArrived() { constexpr auto modeOfOperationRequestHandler = std::make_tuple( - ctrl_IngressMessages_modeOfOperationRequest_tag, - ctrl_EgressMessages_modeOfOperationResponse_tag, + ctrl_IngressMessage_modeOfOperationRequest_tag, + ctrl_EgressMessage_modeOfOperationResponse_tag, &PhaseModulationOrchestrator::handler_modeOfOperationRequest, ctrl_ModeOfOperationResponse ctrl_ModeOfOperationResponse_init_zero ); constexpr auto groupModulationConfigRequestHandler = std::make_tuple( - ctrl_IngressMessages_groupModulationConfigRequest_tag, - ctrl_EgressMessages_groupModulationConfigResponse_tag, + ctrl_IngressMessage_groupModulationConfigRequest_tag, + ctrl_EgressMessage_groupModulationConfigResponse_tag, &PhaseModulationOrchestrator::handler_groupModulationConfigRequest, ctrl_GroupModulationConfigResponse ctrl_GroupModulationConfigResponse_init_zero ); constexpr auto groupModulationControlRequestHandler = std::make_tuple( - ctrl_IngressMessages_groupModulationControlRequest_tag, - ctrl_EgressMessages_groupModulationControlResponse_tag, + ctrl_IngressMessage_groupModulationControlRequest_tag, + ctrl_EgressMessage_groupModulationControlResponse_tag, &PhaseModulationOrchestrator::handler_groupModulationControlRequest, ctrl_GroupModulationControlResponse ctrl_GroupModulationControlResponse_init_zero ); constexpr auto phaseModulationConfigRequestHandler = std::make_tuple( - ctrl_IngressMessages_phaseModulationConfigRequest_tag, - ctrl_EgressMessages_phaseModulationConfigResponse_tag, + ctrl_IngressMessage_phaseModulationConfigRequest_tag, + ctrl_EgressMessage_phaseModulationConfigResponse_tag, &PhaseModulationOrchestrator::handler_phaseModulationConfigRequest, ctrl_PhaseModulationConfigResponse ctrl_PhaseModulationConfigResponse_init_zero ); constexpr auto phaseModulationControlRequestHandler = std::make_tuple( - ctrl_IngressMessages_phaseModulationControlRequest_tag, - ctrl_EgressMessages_phaseModulationControlResponse_tag, + ctrl_IngressMessage_phaseModulationControlRequest_tag, + ctrl_EgressMessage_phaseModulationControlResponse_tag, &PhaseModulationOrchestrator::handler_phaseModulationControlRequest, ctrl_PhaseModulationControlResponse ctrl_PhaseModulationControlResponse_init_zero ); @@ -225,12 +225,12 @@ void PhaseModulationOrchestrator::event_ctrlMessageArrived() { } }; - ctrlIngressQueue.try_consume([&](const ctrl_IngressMessages &req) { + ctrlIngressQueue.try_consume([&](const ctrl_IngressMessage &req) { ULOG_INFO("control request message handler"); - ctrlEgressQueue.try_produce([&](ctrl_EgressMessages &resp) { + ctrlEgressQueue.try_produce([&](ctrl_EgressMessage &resp) { ULOG_INFO("control response message handler"); - resp.requestID = req.requestID; - resp.has_requestID = true; + resp.request_id = req.request_id; + resp.has_request_id = true; switch (req.which_data) { case std::get<1>(modeOfOperationRequestHandler): diff --git a/rims_app/src/phase_modulation.hpp b/rims_app/src/phase_modulation.hpp index 52a3cb5a405..55ad790ff61 100644 --- a/rims_app/src/phase_modulation.hpp +++ b/rims_app/src/phase_modulation.hpp @@ -11,8 +11,8 @@ namespace rims { -extern zephyr_fifo_buffer ctrlIngressQueue; -extern zephyr_fifo_buffer ctrlEgressQueue; +extern zephyr_fifo_buffer ctrlIngressQueue; +extern zephyr_fifo_buffer ctrlEgressQueue; using ModeOfOperation = ctrl_ModeOfOperation; diff --git a/rims_app/src/temperature_measurements.cpp b/rims_app/src/temperature_measurements.cpp index 1154e946481..280987d2be4 100644 --- a/rims_app/src/temperature_measurements.cpp +++ b/rims_app/src/temperature_measurements.cpp @@ -1,5 +1,6 @@ #include "temperature_measurements.hpp" +#include #include #include #include @@ -44,8 +45,8 @@ struct unknown_channel_id : error { K_FIFO_DEFINE(temperatureIngress); K_FIFO_DEFINE(temperatureEggress); -zephyr_fifo_buffer temperatureIngressQueue{temperatureIngress}; -zephyr_fifo_buffer temperatureEgressQueue{temperatureEggress}; +zephyr_fifo_buffer temperatureIngressQueue{temperatureIngress}; +zephyr_fifo_buffer temperatureEgressQueue{temperatureEggress}; namespace { constexpr gpio_dt_spec rmin_en_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(rmin_en), gpios); @@ -85,19 +86,19 @@ void select_channel(int channel) { // TODO check channels template void PB_encode_egress(const T &tempresp) { /// TODO repeat on fail - temperatureEgressQueue.try_produce([&](temperature_EgressMessages &egress) { - if constexpr (std::is_same_v) { + temperatureEgressQueue.try_produce([&](temperature_EgressMessage &egress) { + if constexpr (std::is_same_v) { ULOG_DEBUG("Sending GetTemperatureResponse message"); - egress.data.temperatureResponse = tempresp; - egress.which_data = temperature_EgressMessages_temperatureResponse_tag; + egress.data.temperature_response = tempresp; + egress.which_data = temperature_EgressMessage_temperature_response_tag; } else if constexpr (std::is_same_v) { ULOG_DEBUG("Sending SamplerConfigResponse message"); - egress.data.samplerConfigResponse = tempresp; - egress.which_data = temperature_EgressMessages_samplerConfigResponse_tag; + egress.data.sampler_config_response = tempresp; + egress.which_data = temperature_EgressMessage_sampler_config_response_tag; } else if constexpr (std::is_same_v) { ULOG_DEBUG("Sending TemperatureStatistics message"); - egress.data.broadcastTemperatureStatistics = tempresp; - egress.which_data = temperature_EgressMessages_broadcastTemperatureStatistics_tag; + egress.data.broadcast_temperature_summary = tempresp; + egress.which_data = temperature_EgressMessage_broadcast_temperature_summary_tag; } return true; }); @@ -106,38 +107,30 @@ template void PB_encode_egress(const T &tempresp) { TemperatureSampler::TemperatureSampler(uint8_t channel) : _channel{channel} { calibration(); _broadcastTimer.start(); -} - -void TemperatureSampler::activate() { - // TODO check when last time calibrated and recalibrate if needed - select_channel(_channel); - std::this_thread::sleep_for(std::chrono::milliseconds{1}); // TODO check how long the signal needs to stabilize, and set this value here _samplerTimer.start(); - // ULOG_INFO("Channel %d activated", _channel); -} - -void TemperatureSampler::deactivate() { - _samplerTimer.stop(); } void TemperatureSampler::take_sample() { - _samples.push_back(adc()); + auto adc = adcWithCalibration(); + _samples.push_back(adc); } void TemperatureSampler::broadcastTemperature() const { PB_encode_egress(temperature_statistics()); } -void TemperatureSampler::calibration() { - /// TODO Change to current channal +void TemperatureSampler::calibration(bool disableLog) { select_channel(_channel); - ULOG_INFO("Calibration at channel %d start", _channel); + if (not disableLog) ULOG_INFO("Calibration at channel %d start", _channel); select_rmin(); auto new_adc_Tmin = takeStableRead(); select_rmax(); auto new_adc_Tmax = takeStableRead(); - ULOG_INFO("Calibration at channel %d done, min{%d}/max{%d}, prev min{%d}/max{%d}", _channel, new_adc_Tmin, new_adc_Tmax, _adc_Tmin, _adc_Tmax); + if (not disableLog) + ULOG_INFO( + "Calibration at channel %d done, min{%d}/max{%d}, prev min{%d}/max{%d}", _channel, new_adc_Tmin, new_adc_Tmax, _adc_Tmin, _adc_Tmax + ); _adc_Tmax = new_adc_Tmax; _adc_Tmin = new_adc_Tmin; @@ -145,30 +138,8 @@ void TemperatureSampler::calibration() { select_rpt(); } -// asserts function takes less then -// TODO needs to assert execution on this thread only, right? -class BlockExecutionDurationAssert { -#if __ASSERT_ON - const std::chrono::steady_clock::time_point _start; - const std::chrono::steady_clock::duration _maxTime; - - public: - BlockExecutionDurationAssert(std::chrono::nanoseconds maxTime) : _start{std::chrono::steady_clock::now()}, _maxTime{maxTime} { - } - ~BlockExecutionDurationAssert() { - __ASSERT((std::chrono::steady_clock::now() - _start) <= _maxTime, "execution taked too long"); - } -#else - BlockExecutionDurationAssert(std::chrono::nanoseconds) { - } - -#endif -}; - void TemperatureSampler::tick() { // executed every 1ms by the orchestrator - // BlockExecutionDurationAssert _{std::chrono::microseconds{900}}; - if (zephyr::semaphore::k_sem_take_now(_samplerSem) == 0) { take_sample(); } @@ -184,14 +155,14 @@ BroadcastTemperatureStatistics TemperatureSampler::temperature_statistics() cons if (total_samples == 0) { /// TODO rise error? BroadcastTemperatureStatistics btemp; - btemp.channel_id = this->_channel; - btemp.has_temperatureStats = true; + btemp.channel_id = this->_channel; + btemp.has_temperature_summary = true; - TemperatureStatistics &temp = btemp.temperatureStats; - temp.temp_c = 0; - temp.temp_avg_c = 0; - temp.n_samples = 0; - temp.temp_stddev_c = 0; + TemperatureSummary &temp = btemp.temperature_summary; + temp.temp_c = 0; + temp.temp_filtered_c = 0; + temp.n_samples = 0; + temp.temp_stddev_c = 0; return btemp; } @@ -210,38 +181,53 @@ BroadcastTemperatureStatistics TemperatureSampler::temperature_statistics() cons auto stddev = std::sqrt(sq_sum / count); BroadcastTemperatureStatistics btemp; - btemp.channel_id = this->_channel; - btemp.has_temperatureStats = true; + btemp.channel_id = this->_channel; + btemp.has_temperature_summary = true; - TemperatureStatistics &temp = btemp.temperatureStats; - temp.temp_c = to_temperature(samples.back()); - temp.temp_avg_c = to_temperature(mean); - temp.n_samples = count; - temp.temp_stddev_c = adc_to_celsius_weight() * (float)stddev; + TemperatureSummary &temp = btemp.temperature_summary; + temp.temp_c = adcToTemperature(samples.back()); + temp.temp_filtered_c = adcToTemperature(mean); + temp.n_samples = count; + temp.temp_stddev_c = adc2CelciusWeight() * (float)stddev; return btemp; } TemperatureCurrent TemperatureSampler::temperature() const { /// TODO rise error on empty buffer? - return TemperatureCurrent{.temp_c = to_temperature(this->_samples.back())}; + if (_samples.empty()) return TemperatureCurrent{.temp_c = 0}; + return TemperatureCurrent{.temp_c = adcToTemperature(_samples.back())}; } void TemperatureSampler::setSamplerConfig(const temperature_SamplerConfig &config) noexcept(false) { - if (config.has_samples) { - ULOG_INFO("change samples num from %d to %d", this->_samplesNumber, config.samples); - this->_samplesNumber = config.samples; + if (config.has_oversampling) { + ULOG_INFO("oversampling num from %d to %d", this->_samplesNumber, config.oversampling); + this->_oversampling = config.oversampling; } - // if (config.has_period_ms) { - // ULOG_INFO("change period from %lld to %d", this->_samplerTimer.interval().count(), config.samples); - // this->_samplerTimer.setInterval(std::chrono::milliseconds{config.period_ms}); - // } + if (config.has_period_ms) { + ULOG_INFO( + "change sampling period from %lld to %d ms", + std::chrono::duration_cast(_samplerTimer.interval()).count(), + config.period_ms + ); + this->_samplerTimer.setInterval(std::chrono::milliseconds{config.period_ms}); + } } -TemperatureSampler::Sample TemperatureSampler::to_temperature(uint32_t adcReading) const { +SampleConfig TemperatureSampler::samplerConfig() const { + SampleConfig config = temperature_SamplerConfig_init_zero; + + config.has_oversampling = true; + config.has_period_ms = true; + config.oversampling = std::min(MaxOversampling, _oversampling); + config.period_ms = std::chrono::duration_cast(_samplerTimer.interval()).count(); + + return config; +} + +TemperatureSampler::Sample TemperatureSampler::adcToTemperature(uint32_t adcReading) const { const auto tempMin = this->_Tmin; - const auto tempMax = this->_Tmax; const auto adcMax = this->_adc_Tmax; const auto adcMin = this->_adc_Tmin; @@ -250,12 +236,12 @@ TemperatureSampler::Sample TemperatureSampler::to_temperature(uint32_t adcReadin if (adcReading > adcMin) adcReading = adcMin; // Inverse linear interpolation - float temperature = tempMin + (float)(adcMin - adcReading) * (tempMax - tempMin) / (float)(adcMin - adcMax); + float temperature = tempMin + (float)(adcMin - adcReading) * adc2CelciusWeight(); return temperature; } -float TemperatureSampler::adc_to_celsius_weight() const { +float TemperatureSampler::adc2CelciusWeight() const { const auto tempMin = this->_Tmin; const auto tempMax = this->_Tmax; const auto adcMax = this->_adc_Tmax; @@ -264,23 +250,28 @@ float TemperatureSampler::adc_to_celsius_weight() const { return (tempMax - tempMin) / static_cast(adcMin - adcMax); } -adc_sample TemperatureSampler::adc() const { - std::array samples{}; // TODO add message for samples fer measurement - constexpr auto ADC_RESOLUTION = 12; - const adc_dt_spec adc_spec = ADC_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 0); +adc_sample TemperatureSampler::adcWithCalibration() { + calibration(); + return takeStableRead(); +} - const adc_sequence_options options = { - .interval_us = 0, - .callback = nullptr, - .user_data = nullptr, - .extra_samplings = 3, +adc_sample TemperatureSampler::adc() const { + std::array oversampling_samples{}; + constexpr auto ADC_RESOLUTION = 12; + const adc_dt_spec adc_spec = ADC_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 0); + const auto samples = std::min(MaxOversampling, _oversampling); + const adc_sequence_options options = { + .interval_us = 0, + .callback = nullptr, + .user_data = nullptr, + .extra_samplings = 3, }; adc_sequence sequence = { .options = &options, .channels = BIT(15), - .buffer = samples.data(), - .buffer_size = samples.size() * sizeof(int16_t), + .buffer = oversampling_samples.data(), + .buffer_size = samples * sizeof(int16_t), .resolution = ADC_RESOLUTION, .oversampling = 0, }; @@ -291,18 +282,16 @@ adc_sample TemperatureSampler::adc() const { return 0; } - auto sum = std::accumulate(samples.begin(), samples.end(), 0); - auto avg = sum / samples.size(); + auto sum = std::accumulate(oversampling_samples.begin(), oversampling_samples.begin() + samples, 0); + auto avg = sum / samples; return avg; } -TemperatureSampler::Sample TemperatureSampler::adc_take_sample() const { - return to_temperature(adc()); -} - template constexpr bool unknownChannelError(const Req &req, Resp &resp) { if (req.channel_id > ChannelNumber) { + resp.channel_id = req.channel_id; + resp.error = temperature_Error_UnknownChannel; resp.has_error = true; return true; @@ -312,22 +301,17 @@ template constexpr bool unknownChannelError(const TemperatureSamplerOrchestrator::TemperatureSamplerOrchestrator() : // - _activeChannel{0}, // _temperatureSamplerChannels{TemperatureSampler{1}, TemperatureSampler{2}} // { ULOG_INFO("Initialize message events"); temperatureIngressQueue.k_event_init(_events.at(0)); zephyr::event_pool::k_init(_events.at(1), _samplerTickSem); - zephyr::event_pool::k_init(_events.at(2), _channelChangeSem); }; void TemperatureSamplerOrchestrator::loop() { - ULOG_INFO("Starting TemperatureSampler loop"); + ULOG_INFO("Starting TemperatureSamplerOrchestrator loop"); _samplerTick.start(); - _channelChange.start(); - - action_changeChannel(); while (1) { try { @@ -335,7 +319,6 @@ void TemperatureSamplerOrchestrator::loop() { if (ret == 0) { zephyr::event_pool::k_poll_handle(_events[0], [&]() { event_messageArrived(); }); zephyr::event_pool::k_poll_handle(_events[1], [&]() { action_samplersTick(); }); - zephyr::event_pool::k_poll_handle(_events[2], [&]() { action_changeChannel(); }); } } catch (const std::exception &e) { ULOG_ERROR("EXCEPTION %s", e.what()); @@ -344,33 +327,33 @@ void TemperatureSamplerOrchestrator::loop() { } void TemperatureSamplerOrchestrator::event_messageArrived() { - constexpr auto currentTemperatureRequestHandler = std::make_tuple( - temperature_IngressMessages_currentTemperatureRequest_tag, - temperature_EgressMessages_currentTemperatureResponse_tag, - &TemperatureSamplerOrchestrator::handle_getCurrentTemperatureRequest - ); - - constexpr auto temperatureRequestHandler = std::make_tuple( - temperature_IngressMessages_temperatureRequest_tag, - temperature_EgressMessages_temperatureResponse_tag, + constexpr auto currentTemperature_request_Handler = std::make_tuple( + temperature_IngressMessage_temperature_request_tag, + temperature_EgressMessage_temperature_response_tag, &TemperatureSamplerOrchestrator::handle_getTemperatureRequest ); + constexpr auto temperatureSummary_request_Handler = std::make_tuple( + temperature_IngressMessage_temperature_summary_request_tag, + temperature_EgressMessage_temperature_summary_response_tag, + &TemperatureSamplerOrchestrator::handle_getTemperatureSummaryRequest + ); + constexpr auto samplerConfigHandler = std::make_tuple( - temperature_IngressMessages_samplerConfigRequest_tag, - temperature_EgressMessages_samplerConfigResponse_tag, + temperature_IngressMessage_sampler_config_request_tag, + temperature_EgressMessage_sampler_config_response_tag, &TemperatureSamplerOrchestrator::handle_samplerConfigRequest ); constexpr auto filterConfigHandler = std::make_tuple( - temperature_IngressMessages_filterConfigRequest_tag, - temperature_EgressMessages_filterConfigResponse_tag, + temperature_IngressMessage_filter_config_request_tag, + temperature_EgressMessage_filter_config_response_tag, &TemperatureSamplerOrchestrator::handle_filterConfigRequest ); constexpr auto filterHandler = std::make_tuple( - temperature_IngressMessages_filterRequest_tag, - temperature_EgressMessages_filterResponse_tag, + temperature_IngressMessage_filter_request_tag, + temperature_EgressMessage_filter_response_tag, &TemperatureSamplerOrchestrator::handle_filterRequest ); @@ -379,38 +362,39 @@ void TemperatureSamplerOrchestrator::event_messageArrived() { std::invoke(fn, this, request, response); }; - temperatureEgressQueue.try_produce([&](temperature_EgressMessages &resp) { - ULOG_INFO("Producing temperature_EgressMessages response"); - auto ok = temperatureIngressQueue.try_consume([&](temperature_IngressMessages &req) { - ULOG_INFO("Consuming temperature_IngressMessages request"); - resp.requestID = req.requestID; - resp.has_requestID = true; + temperatureEgressQueue.try_produce([&](temperature_EgressMessage &resp) { + ULOG_INFO("Producing temperature_EgressMessage response"); + auto ok = temperatureIngressQueue.try_consume([&](temperature_IngressMessage &req) { + ULOG_INFO("Consuming temperature_IngressMessage request"); + resp.request_id = req.request_id; + resp.has_request_id = true; switch (req.which_data) { - case std::get<1>(currentTemperatureRequestHandler): - /// to jest źle, ustawiany jest xły tag - resp.which_data = std::get<1>(currentTemperatureRequestHandler); - genericHandler(req.data.currentTemperatureRequest, resp.data.currentTemperatureResponse, currentTemperatureRequestHandler); + case std::get<1>(currentTemperature_request_Handler): + resp.which_data = std::get<1>(currentTemperature_request_Handler); + genericHandler(req.data.temperature_request, resp.data.temperature_response, currentTemperature_request_Handler); break; - case std::get<1>(temperatureRequestHandler): - resp.which_data = std::get<1>(temperatureRequestHandler); - genericHandler(req.data.temperatureRequest, resp.data.temperatureResponse, temperatureRequestHandler); + case std::get<1>(temperatureSummary_request_Handler): + resp.which_data = std::get<1>(temperatureSummary_request_Handler); + genericHandler( + req.data.temperature_summary_request, resp.data.temperature_summary_response, temperatureSummary_request_Handler + ); break; case std::get<1>(samplerConfigHandler): resp.which_data = std::get<1>(samplerConfigHandler); - genericHandler(req.data.samplerConfigRequest, resp.data.samplerConfigResponse, samplerConfigHandler); + genericHandler(req.data.sampler_config_request, resp.data.sampler_config_response, samplerConfigHandler); break; case std::get<1>(filterConfigHandler): resp.which_data = std::get<1>(filterConfigHandler); - genericHandler(req.data.filterConfigRequest, resp.data.filterConfigResponse, filterConfigHandler); + genericHandler(req.data.filter_config_request, resp.data.filter_config_response, filterConfigHandler); break; case std::get<1>(filterHandler): resp.which_data = std::get<1>(filterHandler); - genericHandler(req.data.filterRequest, resp.data.filterResponse, filterHandler); + genericHandler(req.data.filter_request, resp.data.filter_response, filterHandler); break; default: @@ -419,31 +403,34 @@ void TemperatureSamplerOrchestrator::event_messageArrived() { } }); if (not ok) { - ULOG_ERROR("Producing temperature_EgressMessages response FAILED"); + ULOG_ERROR("Producing temperature_EgressMessage response FAILED"); } else { - ULOG_ERROR("Producing temperature_EgressMessages response OK"); + ULOG_INFO("Producing temperature_EgressMessage response OK"); } return true; }); } +void TemperatureSamplerOrchestrator::handle_getTemperatureSummaryRequest( + const GetTemperatureSummaryRequest &req, + GetTemperatureSummaryResponse &resp +) const { + if (unknownChannelError(req, resp)) return; + resp.channel_id = req.channel_id; + + resp.has_temperature_summary = true; + resp.temperature_summary = _temperatureSamplerChannels[req.channel_id].temperature_statistics().temperature_summary; +} + void TemperatureSamplerOrchestrator::handle_getTemperatureRequest( // const GetTemperatureRequest &req, GetTemperatureResponse &resp ) const { if (unknownChannelError(req, resp)) return; + resp.channel_id = req.channel_id; - resp.temperatureStats = _temperatureSamplerChannels[req.channel_id].temperature_statistics().temperatureStats; -} - -void TemperatureSamplerOrchestrator::handle_getCurrentTemperatureRequest( - const GetCurrentTemperatureRequest &req, - GetCurrentTemperatureResponse &resp -) const { - if (unknownChannelError(req, resp)) return; - - resp.channel_id = req.channel_id; - resp.temperatureCurrent = _temperatureSamplerChannels[req.channel_id].temperature(); + resp.has_temperature_current = true; + resp.temperature_current = _temperatureSamplerChannels[req.channel_id].temperature(); } void TemperatureSamplerOrchestrator::handle_samplerConfigRequest( // @@ -455,60 +442,48 @@ void TemperatureSamplerOrchestrator::handle_samplerConfigRequest( // if (req.has_config) { _temperatureSamplerChannels[req.channel_id].setSamplerConfig(req.config); /// TODO apply configuration - } else { - /// TODO get configuration only } + + resp.has_config = true; + resp.config = _temperatureSamplerChannels[req.channel_id].samplerConfig(); } -void TemperatureSamplerOrchestrator::handle_filterConfigRequest(const FilterConfigRequest &req, FilterConfigResponse &resp) const { +void TemperatureSamplerOrchestrator::handle_filterConfigRequest( // + const FilterConfigRequest &req, + FilterConfigResponse &resp +) const { if (unknownChannelError(req, resp)) return; - if (req.has_emaFilterParams) { + if (req.has_ema_filter_params) { /// TODO set } - if (req.has_kalmanFilterParams) { + if (req.has_kalman_pilter_params) { /// TODO set } - if (req.has_averageFilterParams) { + if (req.has_average_filter_params) { /// TODO set } - if (not req.skipFullResponse) { + if (not req.skip_full_response) { } } -void TemperatureSamplerOrchestrator::handle_filterRequest(const FilterRequest &req, FilterResponse &resp) const { +void TemperatureSamplerOrchestrator::handle_filterRequest( // + const FilterRequest &req, + FilterResponse &resp +) const { if (unknownChannelError(req, resp)) return; } void TemperatureSamplerOrchestrator::action_samplersTick() { zephyr::semaphore::k_sem_take_now(_samplerTickSem); - for (auto &sampler : _temperatureSamplerChannels) { sampler.tick(); } } -void TemperatureSamplerOrchestrator::action_changeChannel() { - zephyr::semaphore::k_sem_take_now(_channelChangeSem); - for (auto &sampler : _temperatureSamplerChannels) { - sampler.deactivate(); - } - - _temperatureSamplerChannels.at(_activeChannel).activate(); - - if (--_recalibrationTicks[_activeChannel] == 0) { - _recalibrationTicks[_activeChannel] = 60; - _samplerTick.stop(); // disable sampling for a while - _temperatureSamplerChannels.at(_activeChannel).calibration(); - _samplerTick.start(); // reenable sampling - } - - _activeChannel = (_activeChannel + 1) % temperatureChannelsNr; -} - int TemperatureSamplerThread::do_hardwarenInit() { __ASSERT(device_is_ready(adc), "adc needs to work"); @@ -528,19 +503,20 @@ void TemperatureSamplerThread::threadMain() { thread.loop(); } -uint32_t TemperatureSampler::takeStableRead() { +uint32_t TemperatureSampler::takeStableRead(bool disableLog) { const auto start = std::chrono::steady_clock::now(); - ULOG_INFO("Starting ADC stable read"); + if (not disableLog) ULOG_INFO("Starting ADC stable read"); - int maxIterations = 100; - int STABILITY_COUNT_REQUIRED = 6; - int STABILITY_THRESHOLD = 5; + int maxIterations = 100; + const int STABILITY_COUNT_REQUIRED = 6; + const int STABILITY_THRESHOLD = 5; - int prevAdc = 0; - uint8_t stableCount = 0; + int prevAdc = 0; + int stableCount = 0; while (stableCount < STABILITY_COUNT_REQUIRED && maxIterations > 0) { - std::this_thread::sleep_for(std::chrono::microseconds{500}); // small delay between samples + // an adc takes something like ~1us, so x max number of samples this should take ~20us + std::this_thread::sleep_for(std::chrono::microseconds{100}); // small delay between measurements auto adcraw = adc(); if (std::abs(adcraw - prevAdc) < STABILITY_THRESHOLD) { @@ -556,7 +532,7 @@ uint32_t TemperatureSampler::takeStableRead() { const auto stop = std::chrono::steady_clock::now(); auto ms = (stop - start).count() / 1000 / 1000; auto itleft = (100 - maxIterations); - ULOG_INFO("ADC stable read %d end after %lld ms in %d iterations", prevAdc, ms, itleft); + if (not disableLog) ULOG_INFO("ADC stable read %d end after %lld ms in %d iterations", prevAdc, ms, itleft); return prevAdc; } diff --git a/rims_app/src/temperature_measurements.hpp b/rims_app/src/temperature_measurements.hpp index 11b47eb404c..f028770f906 100644 --- a/rims_app/src/temperature_measurements.hpp +++ b/rims_app/src/temperature_measurements.hpp @@ -13,16 +13,17 @@ namespace rims { -extern zephyr_fifo_buffer temperatureIngressQueue; -extern zephyr_fifo_buffer temperatureEgressQueue; +extern zephyr_fifo_buffer temperatureIngressQueue; +extern zephyr_fifo_buffer temperatureEgressQueue; -static constexpr std::size_t MaxSampleSize = 32; -static constexpr uint8_t channelNumber = 2; +static constexpr std::size_t MaxSampleSize = 32; +static constexpr std::uint8_t MaxOversampling = 16; +static constexpr uint8_t channelNumber = 2; +using GetTemperatureSummaryRequest = temperature_GetTemperatureSummaryRequest; +using GetTemperatureSummaryResponse = temperature_GetTemperatureSummaryResponse; using GetTemperatureRequest = temperature_GetTemperatureRequest; using GetTemperatureResponse = temperature_GetTemperatureResponse; -using GetCurrentTemperatureRequest = temperature_GetCurrentTemperatureRequest; -using GetCurrentTemperatureResponse = temperature_GetCurrentTemperatureResponse; using SamplerConfigRequest = temperature_SamplerConfigRequest; using SamplerConfigResponse = temperature_SamplerConfigResponse; using FilterConfigRequest = temperature_FilterConfigRequest; @@ -30,9 +31,10 @@ using FilterConfigResponse = temperature_FilterConfigResponse; using FilterRequest = temperature_FilterRequest; using FilterResponse = temperature_FilterResponse; -using TemperatureStatistics = temperature_TemperatureStatistics; +using SampleConfig = temperature_SamplerConfig; +using TemperatureSummary = temperature_TemperatureSummary; using TemperatureCurrent = temperature_TemperatureCurrent; -using BroadcastTemperatureStatistics = temperature_BroadcastTemperatureStatistics; +using BroadcastTemperatureStatistics = temperature_BroadcastTemperatureSummary; class TemperatureSampler { public: @@ -47,29 +49,27 @@ class TemperatureSampler { TemperatureSampler &operator=(TemperatureSampler &&) = delete; void tick(); - void calibration(); - - void setSamplerConfig(const temperature_SamplerConfig &config) noexcept(false); - - void activate(); - void deactivate(); - + void calibration(bool disableLog = false); + + void setSamplerConfig(const SampleConfig &config) noexcept(false); + SampleConfig samplerConfig() const; + BroadcastTemperatureStatistics temperature_statistics() const; TemperatureCurrent temperature() const; - - protected: + protected: bool _running{false}; void take_sample(); void broadcastTemperature() const; - uint32_t takeStableRead(); + uint32_t takeStableRead(bool disableLog = false); - Sample to_temperature(uint32_t adc_value) const; - float adc_to_celsius_weight() const; + float adc2CelciusWeight() const; + Sample adcToTemperature(uint32_t adc_value) const; + + adc_sample adcWithCalibration(); adc_sample adc() const; - Sample adc_take_sample() const; uint32_t _adc_Tmin{}; uint32_t _adc_Tmax{}; @@ -77,13 +77,16 @@ class TemperatureSampler { float _Tmax{103.0f}; zephyr::semaphore::sem _samplerSem{0, 1}; - RecurringSemaphoreTimer _samplerTimer{_samplerSem, std::chrono::milliseconds{75}}; + RecurringSemaphoreTimer _samplerTimer{_samplerSem, std::chrono::milliseconds{100}}; zephyr::semaphore::sem _broadcastSem{0, 1}; - RecurringSemaphoreTimer _broadcastTimer{_broadcastSem, std::chrono::seconds{1000}}; + RecurringSemaphoreTimer _broadcastTimer{_broadcastSem, std::chrono::seconds{60}}; ring_buffer _samples; - std::uint8_t _samplesNumber{MaxSampleSize}; + // number of samples taken to filter + std::uint8_t _samplesNumber{MaxSampleSize}; + + std::uint8_t _oversampling{8}; // number of samples takes for each measurement const std::uint8_t _channel{}; }; @@ -98,28 +101,20 @@ class TemperatureSamplerOrchestrator { // events handled in main loop void event_messageArrived(); + void handle_getTemperatureSummaryRequest(const GetTemperatureSummaryRequest &req, GetTemperatureSummaryResponse &resp) const; void handle_getTemperatureRequest(const GetTemperatureRequest &req, GetTemperatureResponse &resp) const; - void handle_getCurrentTemperatureRequest(const GetCurrentTemperatureRequest &req, GetCurrentTemperatureResponse &resp) const; void handle_samplerConfigRequest(const SamplerConfigRequest &req, SamplerConfigResponse &resp); void handle_filterConfigRequest(const FilterConfigRequest &req, FilterConfigResponse &resp) const; void handle_filterRequest(const FilterRequest &req, FilterResponse &resp) const; void action_samplersTick(); - void action_changeChannel(); - - uint8_t _activeChannel{0}; std::array _temperatureSamplerChannels; zephyr::semaphore::sem _samplerTickSem{0, 1}; RecurringSemaphoreTimer _samplerTick{_samplerTickSem, std::chrono::milliseconds{1}}; - zephyr::semaphore::sem _channelChangeSem{0, 1}; - RecurringSemaphoreTimer _channelChange{_channelChangeSem, std::chrono::seconds{1}}; - - std::array _events; // event from timer and from Messenger - - uint8_t _recalibrationTicks[2] = {60,60}; // 2 minutes + std::array _events; // event from timer and from Messenger }; class TemperatureSamplerThread : public ZephyrThread { diff --git a/rims_app/src/uart.cpp b/rims_app/src/uart.cpp index eec65222766..8e00391b2d9 100644 --- a/rims_app/src/uart.cpp +++ b/rims_app/src/uart.cpp @@ -136,11 +136,13 @@ void AsyncUART::uartCallback(const device *dev, void *user_data) { void AsyncUART::uartISR() { try { - while (uart_irq_update(_dev) && uart_irq_is_pending(_dev)) { - if (uart_irq_rx_ready(_dev)) { + if (uart_irq_update(_dev)) { + bool rxready = uart_irq_rx_ready(_dev); + if (rxready) { readByteUart(); } - if (uart_irq_tx_ready(_dev)) { + bool txready = uart_irq_tx_ready(_dev); + if (txready) { writeByteUart(); } } @@ -157,19 +159,25 @@ void AsyncUART::uartISR() { } void AsyncUART::readByteUart() { - if (_faultFlag) { - if (rxByte() == 0) _faultFlag = false; - } - // throw on buffer overflow - else if (rxBuffer().full()) { - throw uart_rx_buffer_overflow{}; - } - // push_back returns last placed byte, if the byte is 0x00 we got end of frame - else if (rxBuffer().push_back(rxByte()) == 0) { - if (rxBuffer().size() > 1) { - processMessage(); - } else { + uint8_t byte; + + while (uart_fifo_read(_dev, &byte, 1) == 1) { + if (_faultFlag) { + if (byte == 0) _faultFlag = false; + } + // throw on buffer overflow + else if (rxBuffer().full()) { rxBuffer().clean(); + _faultFlag = true; + // throw uart_rx_buffer_overflow{}; + } + // push_back returns last placed byte, if the byte is 0x00 we got end of frame + else if (rxBuffer().push_back(byte) == 0) { + if (rxBuffer().size() > 1) { + processMessage(); + } else { + rxBuffer().clean(); + } } } } @@ -188,10 +196,9 @@ uint8_t AsyncUART::rxByte() { } void AsyncUART::processMessage() { - int a; buffer buf{.data = {rxBuffer().data(), rxBuffer().size()}, .device = this}; switchRxBuffer(); - k_msgq_put(&messenger_buffer_arrived_queue, &buf, K_MSEC(10)); + k_msgq_put(&messenger_buffer_arrived_queue, &buf, K_NO_WAIT); } AsyncUART::rx_buffer_t &AsyncUART::rxBuffer() { -- 2.45.2 From 545eae978ae1f80d2ad883ecd5a0363515a0151a Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Tue, 29 Apr 2025 13:01:30 +0200 Subject: [PATCH 12/18] refactor power control module --- rims_app/prj.conf | 2 +- rims_app/proto/ctrl.proto | 21 + rims_app/src/circular_buffer.hpp | 44 +- rims_app/src/common.hpp | 30 +- rims_app/src/inplace_vector.hpp | 769 ++++++++++++++++++ rims_app/src/main.cpp | 2 +- rims_app/src/messenger.cpp | 2 +- rims_app/src/phase_modulation.hpp | 152 ---- ...phase_modulation.cpp => power_control.cpp} | 176 ++-- rims_app/src/power_control.hpp | 293 +++++++ rims_app/src/temperature_measurements.cpp | 2 +- rims_app/src/uart.cpp | 8 +- rims_app/src/uart.hpp | 3 +- rims_app/src/zero_cross_detection.cpp | 46 +- rims_app/src/zero_cross_detection.hpp | 6 +- 15 files changed, 1233 insertions(+), 323 deletions(-) create mode 100644 rims_app/src/inplace_vector.hpp delete mode 100644 rims_app/src/phase_modulation.hpp rename rims_app/src/{phase_modulation.cpp => power_control.cpp} (69%) create mode 100644 rims_app/src/power_control.hpp diff --git a/rims_app/prj.conf b/rims_app/prj.conf index b33d9b48964..ebf1a07cb3a 100644 --- a/rims_app/prj.conf +++ b/rims_app/prj.conf @@ -12,7 +12,7 @@ CONFIG_POSIX_TIMERS=y #CONFIG_USERSPACE=y CONFIG_HW_STACK_PROTECTION=y CONFIG_MPU_STACK_GUARD=y -CONFIG_ASSERT=y +CONFIG_ASSERT=n CONFIG_LOG=n CONFIG_LOG_MODE_IMMEDIATE=n # Log messages are output immediately diff --git a/rims_app/proto/ctrl.proto b/rims_app/proto/ctrl.proto index 34de545fd05..428980a46bf 100644 --- a/rims_app/proto/ctrl.proto +++ b/rims_app/proto/ctrl.proto @@ -1,5 +1,6 @@ syntax = "proto3"; +import "nanopb.proto"; package ctrl; // channel configuration @@ -156,6 +157,26 @@ message PhaseModulationControlResponse optional Error error = 254; } +//message ActiveChannelsRequest +//{ +// // gets number of active channels +//} + +//message ActiveChannelResponse +//{ +// // info about ZCD, linked channels? +//} + +//message LinkChannelRequest +//{ +// repeated uint32 channel_id =1 [ (nanopb).max_count = 16 ]; +//} + +//message LinkChannelResponse +//{ +// uint32 cchannel_id; +//} + // only those messages are send through interface message IngressMessage { diff --git a/rims_app/src/circular_buffer.hpp b/rims_app/src/circular_buffer.hpp index 13b5057e4b1..324d2a621c7 100644 --- a/rims_app/src/circular_buffer.hpp +++ b/rims_app/src/circular_buffer.hpp @@ -7,49 +7,11 @@ #include #include +#include #include namespace rims { -template class static_vector { - private: - std::array buffer; - std::size_t count = 0; - - public: - constexpr std::size_t size() const noexcept { - return count; - } - - constexpr void clean() noexcept { /// TODO call dtors - buffer.fill(0); - count = 0; - } - - constexpr const T *data() const noexcept { - return buffer.data(); - } - constexpr T *data() noexcept { - return buffer.data(); - } - - constexpr bool full() const noexcept { - return count >= Capacity; - } - - constexpr bool empty() const noexcept { - return size() == 0; - } - - constexpr T &push_back(const T &value) { - if (count >= Capacity) { - throw std::out_of_range("static_vector is full"); - } - buffer[count++] = value; - return buffer[count - 1]; - } -}; - struct RingIndex { using index_t = uint16_t; @@ -191,6 +153,10 @@ struct RingIndex { template class ring_buffer { 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} { } diff --git a/rims_app/src/common.hpp b/rims_app/src/common.hpp index 005f1df1f5b..597c26e4fbd 100644 --- a/rims_app/src/common.hpp +++ b/rims_app/src/common.hpp @@ -33,7 +33,7 @@ constexpr bool operator==(const k_timeout_t &lhs, const k_timeout_t &rhs) { inline constexpr static k_timeout_t chronoToKTimeout(std::chrono::nanoseconds duration) { return K_NSEC(duration.count()); } - + static_assert(chronoToKTimeout(std::chrono::milliseconds{345}) == K_MSEC(345)); static_assert(chronoToKTimeout(std::chrono::microseconds{667}) == K_USEC(667)); static_assert(chronoToKTimeout(std::chrono::seconds{2}) == K_SECONDS(2)); @@ -48,8 +48,8 @@ static_assert(kTimeoutToChrono(K_SECONDS(2)) == std::chrono::seconds{2}); class Timer { public: // Constructor - Timer(std::chrono::microseconds interval, std::optional period = std::nullopt) - : _interval(chronoToKTimeout(interval)), _period{period.has_value() ? chronoToKTimeout(*period) : K_NO_WAIT} { + Timer(std::chrono::microseconds duration, std::optional period = std::nullopt) + : _duration(chronoToKTimeout(duration)), _period{period.has_value() ? chronoToKTimeout(*period) : K_NO_WAIT} { memset(&timer_, 0, sizeof(timer_)); k_timer_init(&timer_, &Timer::expiryHandler, &Timer::stopHandler); k_timer_user_data_set(&timer_, this); @@ -65,7 +65,7 @@ class Timer { Timer &operator=(Timer &&) = default; void start() { - k_timer_start(&timer_, _interval, _period); + k_timer_start(&timer_, _duration, _period); } void stop() { @@ -77,15 +77,15 @@ class Timer { return k_timer_remaining_ticks(&timer_) > 0; } - void setInterval(std::chrono::microseconds interval) { - _interval = chronoToKTimeout(interval); + void setDuration(std::chrono::microseconds duration) { + _duration = chronoToKTimeout(duration); if (isRunning()) { - start(); // Restart the timer with the new interval + start(); // Restart the timer with the new duration } } constexpr std::chrono::microseconds interval() const { - return std::chrono::microseconds{k_ticks_to_us_floor64(_interval.ticks)}; + return std::chrono::microseconds{k_ticks_to_us_floor64(_duration.ticks)}; } constexpr std::chrono::microseconds period() const { return std::chrono::microseconds{k_ticks_to_us_floor64(_period.ticks)}; @@ -95,11 +95,15 @@ class Timer { virtual void expiry_cb() = 0; virtual void stop_cb() {}; - // Zephyr timer instance, mutable for c compatybility + // Zephyr timer instance k_timer timer_; - // Timer interval - k_timeout_t _interval; + /* 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; // Static timer handler @@ -113,7 +117,7 @@ class Timer { class RecurringSemaphoreTimer : public Timer { public: - RecurringSemaphoreTimer(k_sem &sem, std::chrono::milliseconds interval) : Timer(interval, interval), _sem{&sem} { + RecurringSemaphoreTimer(k_sem &sem, std::chrono::microseconds interval) : Timer(interval, interval), _sem{&sem} { } RecurringSemaphoreTimer(const RecurringSemaphoreTimer &) = delete; @@ -134,7 +138,7 @@ class RecurringSemaphoreTimer : public Timer { class SingleShootTimer : public Timer { public: - SingleShootTimer(std::function cb, std::chrono::milliseconds interval) : Timer(interval), _cb{std::move(cb)} { + SingleShootTimer(std::function cb, std::chrono::microseconds duration) : Timer(duration), _cb{std::move(cb)} { } SingleShootTimer(const SingleShootTimer &) = delete; diff --git a/rims_app/src/inplace_vector.hpp b/rims_app/src/inplace_vector.hpp new file mode 100644 index 00000000000..6afb4193cb0 --- /dev/null +++ b/rims_app/src/inplace_vector.hpp @@ -0,0 +1,769 @@ +#pragma once +#pragma GCC diagnostic ignored "-Wunused-parameter" + +#include // for rotate... +#include +#include // for lots... +#include // for size_t +#include // for fixed-width integer types +#include // for assertion diagnostics +#include // for abort +// #include // for less and equal_to +#include // for reverse_iterator and iterator traits +#include // for numeric_limits +#include // for destroy +#include // for operator new +#include +#include // for length_error +#include // for aligned_storage and all meta-functions + +// Artifact from previous implementation, can be used as hints for optimizer +#define IV_EXPECT(EXPR) + +// beman::from_range_t +namespace beman { +struct from_range_t {}; +inline constexpr from_range_t from_range; +}; // namespace beman + +// Private utilities +namespace beman::details::inplace_vector { + +// clang-format off +// Smallest unsigned integer that can represent values in [0, N]. +template +using smallest_size_t + = std::conditional_t<(N < std::numeric_limits::max()), uint8_t, + std::conditional_t<(N < std::numeric_limits::max()), uint16_t, + std::conditional_t<(N < std::numeric_limits::max()), uint32_t, + std::conditional_t<(N < std::numeric_limits::max()), uint64_t, + size_t>>>>; +// clang-format on + +// Index a random-access and sized range doing bound checks in debug builds +template +static constexpr decltype(auto) index(Rng &&rng, Index i) noexcept + requires(std::ranges::sized_range) +{ + IV_EXPECT(static_cast(i) < std::ranges::size(rng)); + return std::begin(std::forward(rng))[std::forward(i)]; +} + +// http://eel.is/c++draft/container.requirements.general#container.intro.reqmts-2 +template +concept container_compatible_range = std::ranges::input_range && std::convertible_to, T>; + +template +concept satify_constexpr = N == 0 || std::is_trivial_v; + +} // namespace beman::details::inplace_vector + +// Types implementing the `inplace_vector`'s storage +namespace beman::details::inplace_vector::storage { + +// Storage for zero elements. +template struct zero_sized { + protected: + using size_type = uint8_t; + static constexpr T *storage_data() noexcept { + return nullptr; + } + static constexpr size_type storage_size() noexcept { + return 0; + } + static constexpr void unsafe_set_size(size_t new_size) noexcept { + IV_EXPECT(new_size == 0 && "tried to change size of empty storage to non-zero value"); + } + + public: + constexpr zero_sized() = default; + constexpr zero_sized(zero_sized const &) = default; + constexpr zero_sized &operator=(zero_sized const &) = default; + constexpr zero_sized(zero_sized &&) = default; + constexpr zero_sized &operator=(zero_sized &&) = default; + constexpr ~zero_sized() = default; +}; + +// Storage for trivial types. +template struct trivial { + static_assert(std::is_trivial_v, "storage::trivial requires Trivial"); + static_assert(N != size_t{0}, "N == 0, use zero_sized"); + + protected: + using size_type = smallest_size_t; + + private: + // If value_type is const, then const std::array of non-const elements: + using array_based_storage = std::conditional_t, std::array, const std::array, N>>; + alignas(alignof(T)) array_based_storage storage_data_{}; + size_type storage_size_ = 0; + + protected: + constexpr const T *storage_data() const noexcept { + return storage_data_.data(); + } + constexpr T *storage_data() noexcept { + return storage_data_.data(); + } + constexpr size_type storage_size() const noexcept { + return storage_size_; + } + constexpr void unsafe_set_size(size_t new_size) noexcept { + IV_EXPECT(size_type(new_size) <= N && "new_size out-of-bounds [0, N]"); + storage_size_ = size_type(new_size); + } + + public: + constexpr trivial() noexcept = default; + constexpr trivial(trivial const &) noexcept = default; + constexpr trivial &operator=(trivial const &) noexcept = default; + constexpr trivial(trivial &&) noexcept = default; + constexpr trivial &operator=(trivial &&) noexcept = default; + constexpr ~trivial() = default; +}; + +template struct raw_byte_based_storage { + alignas(T) std::byte _d[sizeof(T) * N]; + constexpr T *storage_data(size_t i) noexcept { + IV_EXPECT(i < N); + return reinterpret_cast(_d) + i; + } + constexpr const T *storage_data(size_t i) const noexcept { + IV_EXPECT(i < N); + return reinterpret_cast(_d) + i; + } +}; + +/// Storage for non-trivial elements. +template struct non_trivial { + static_assert(!std::is_trivial_v, "use storage::trivial for Trivial elements"); + static_assert(N != size_t{0}, "use storage::zero for N==0"); + + protected: + using size_type = smallest_size_t; + + private: + using byte_based_storage = + std::conditional_t, raw_byte_based_storage, const raw_byte_based_storage, N>>; + byte_based_storage storage_data_{}; // BUGBUG: test SIMD types + size_type storage_size_ = 0; + + protected: + constexpr const T *storage_data() const noexcept { + return storage_data_.storage_data(0); + } + constexpr T *storage_data() noexcept { + return storage_data_.storage_data(0); + } + constexpr size_type storage_size() const noexcept { + return storage_size_; + } + constexpr void unsafe_set_size(size_t new_size) noexcept { + IV_EXPECT(size_type(new_size) <= N && "new_size out-of-bounds [0, N)"); + storage_size_ = size_type(new_size); + } + + public: + constexpr non_trivial() noexcept = default; + constexpr non_trivial(non_trivial const &) noexcept = default; + constexpr non_trivial &operator=(non_trivial const &) noexcept = default; + constexpr non_trivial(non_trivial &&) noexcept = default; + constexpr non_trivial &operator=(non_trivial &&) noexcept = default; + + constexpr ~non_trivial() + requires(std::is_trivially_destructible_v) + = default; + constexpr ~non_trivial() { + std::destroy(storage_data(), storage_data() + storage_size()); + } +}; + +// Selects the vector storage. +template +using storage_for = std::conditional_t, non_trivial, std::conditional_t, trivial>>; + +} // namespace beman::details::inplace_vector::storage + +namespace beman { + +template +concept has_constexpr_support = details::inplace_vector::satify_constexpr; + +/// Dynamically-resizable fixed-N vector with inplace storage. +template struct inplace_vector : private details::inplace_vector::storage::storage_for { + private: + static_assert(std::is_nothrow_destructible_v, "T must be nothrow destructible"); + using base_t = details::inplace_vector::storage::storage_for; + using base_t::storage_data; + using base_t::storage_size; + using base_t::unsafe_set_size; + + public: + using value_type = T; + using pointer = T *; + using const_pointer = const T *; + using reference = value_type &; + using const_reference = const value_type &; + using size_type = size_t; + using difference_type = std::ptrdiff_t; + using iterator = pointer; + using const_iterator = const_pointer; + using reverse_iterator = std::reverse_iterator; + using const_reverse_iterator = std::reverse_iterator; + + // [containers.sequences.inplace_vector.cons], construct/copy/destroy + constexpr inplace_vector() noexcept = default; + // constexpr explicit inplace_vector(size_type n); + // constexpr inplace_vector(size_type n, const T& value); + // template // BUGBUG: why not model input_iterator? + // constexpr inplace_vector(InputIterator first, InputIterator + // last); + // template R> + // constexpr inplace_vector(from_range_t, R&& rg); + // from base-class, trivial if is_trivially_copy_constructible_v: + // constexpr inplace_vector(const inplace_vector&); + // from base-class, trivial if is_trivially_move_constructible_v + // constexpr inplace_vector(inplace_vector&&) noexcept(N == 0 || + // std::is_nothrow_move_constructible_v); + // constexpr inplace_vector(std::initializer_list il); + // from base-class, trivial if is_trivially_destructible_v + // constexpr ~inplace_vector(); + // from base-class, trivial if is_trivially_destructible_v && + // is_trivially_copy_assignable_v + // constexpr inplace_vector& operator=(const inplace_vector& other); + // from base-class, trivial if is_trivially_destructible_v && + // is_trivially_copy_assignable_v + // constexpr inplace_vector& operator=(inplace_vector&& other) + // noexcept(N == 0 || is_nothrow_move_assignable_v); + // template // BUGBUG: why not model input_iterator + // constexpr void assign(InputIterator first, InputIterator last); + // template R> + // constexpr void assign_range(R&& rg); + // constexpr void assign(size_type n, const T& u); + // constexpr void assign(std::initializer_list il); + + // iterators + constexpr iterator begin() noexcept { + return storage_data(); + } + constexpr const_iterator begin() const noexcept { + return storage_data(); + } + constexpr iterator end() noexcept { + return begin() + size(); + } + constexpr const_iterator end() const noexcept { + return begin() + size(); + } + constexpr reverse_iterator rbegin() noexcept { + return reverse_iterator(end()); + } + constexpr const_reverse_iterator rbegin() const noexcept { + return const_reverse_iterator(end()); + } + constexpr reverse_iterator rend() noexcept { + return reverse_iterator(begin()); + } + constexpr const_reverse_iterator rend() const noexcept { + return const_reverse_iterator(begin()); + } + + constexpr const_iterator cbegin() const noexcept { + return storage_data(); + } + constexpr const_iterator cend() const noexcept { + return cbegin() + size(); + } + constexpr const_reverse_iterator crbegin() const noexcept { + return const_reverse_iterator(cend()); + } + constexpr const_reverse_iterator crend() const noexcept { + return const_reverse_iterator(cbegin()); + } + [[nodiscard]] constexpr bool full() const noexcept{ + return size() == capacity(); + } + [[nodiscard]] constexpr bool empty() const noexcept { + return storage_size() == 0; + }; + constexpr size_type size() const noexcept { + return storage_size(); + } + static constexpr size_type max_size() noexcept { + return N; + } + static constexpr size_type capacity() noexcept { + return N; + } + + // constexpr void resize(size_type sz); + // constexpr void resize(size_type sz, const T& c); + constexpr void reserve(size_type n) { + if (n > N) [[unlikely]] + throw std::bad_alloc(); + } + constexpr void shrink_to_fit() { + } + + // element access + constexpr reference operator[](size_type n) { + return details::inplace_vector::index(*this, n); + } + constexpr const_reference operator[](size_type n) const { + return details::inplace_vector::index(*this, n); + } + // constexpr const_reference at(size_type n) const; + // constexpr reference at(size_type n); + constexpr reference front() { + return details::inplace_vector::index(*this, size_type(0)); + } + constexpr const_reference front() const { + return details::inplace_vector::index(*this, size_type(0)); + } + constexpr reference back() { + return details::inplace_vector::index(*this, size() - size_type(1)); + } + constexpr const_reference back() const { + return details::inplace_vector::index(*this, size() - size_type(1)); + } + + // [containers.sequences.inplace_vector.data], data access + constexpr T *data() noexcept { + return storage_data(); + } + constexpr const T *data() const noexcept { + return storage_data(); + } + + // [containers.sequences.inplace_vector.modifiers], modifiers + // template + // constexpr T& emplace_back(Args&&... args); + // constexpr T& push_back(const T& x); + // constexpr T& push_back(T&& x); + // template R> + // constexpr void append_range(R&& rg); + // constexpr void pop_back(); + + // template + // constexpr T* try_emplace_back(Args&&... args); + // constexpr T* try_push_back(const T& value); + // constexpr T* try_push_back(T&& value); + + // template + // constexpr T& unchecked_emplace_back(Args&&... args); + // constexpr T& unchecked_push_back(const T& value); + // constexpr T& unchecked_push_back(T&& value); + + // template + // constexpr iterator emplace(const_iterator position, Args&&... args); + // constexpr iterator insert(const_iterator position, const T& x); + // constexpr iterator insert(const_iterator position, T&& x); + // constexpr iterator insert(const_iterator position, size_type n, const + // T& x); + // template + // constexpr iterator insert(const_iterator position, InputIterator + // first, InputIterator last); + // template R> + // constexpr iterator insert_range(const_iterator position, R&& rg); + // constexpr iterator insert(const_iterator position, + // std::initializer_list + // il); constexpr iterator erase(const_iterator position); constexpr + // iterator erase(const_iterator first, const_iterator last); constexpr + // void swap(inplace_vector& x) + // noexcept(N == 0 || (std::is_nothrow_swappable_v && + // std::is_nothrow_move_constructible_v)); + // constexpr void clear() noexcept; + + constexpr friend bool operator==(const inplace_vector &x, const inplace_vector &y) { + return x.size() == y.size() && std::ranges::equal(x, y); + } + // constexpr friend auto /*synth-three-way-result*/ + // operator<=>(const inplace_vector& x, const inplace_vector& y); + constexpr friend void + swap(inplace_vector &x, inplace_vector &y) noexcept(N == 0 || (std::is_nothrow_swappable_v && std::is_nothrow_move_constructible_v)) { + x.swap(y); + } + + private: // Utilities + constexpr void assert_iterator_in_range(const_iterator it) noexcept { + IV_EXPECT(begin() <= it && "iterator not in range"); + IV_EXPECT(it <= end() && "iterator not in range"); + } + constexpr void assert_valid_iterator_pair(const_iterator first, const_iterator last) noexcept { + IV_EXPECT(first <= last && "invalid iterator pair"); + } + constexpr void assert_iterator_pair_in_range(const_iterator first, const_iterator last) noexcept { + assert_iterator_in_range(first); + assert_iterator_in_range(last); + assert_valid_iterator_pair(first, last); + } + constexpr void unsafe_destroy(T *first, T *last) noexcept(std::is_nothrow_destructible_v) { + assert_iterator_pair_in_range(first, last); + if constexpr (N > 0 && !std::is_trivial_v) { + for (; first != last; ++first) + first->~T(); + } + } + + public: + // Implementation + + // [containers.sequences.inplace_vector.modifiers], modifiers + + template + constexpr T &unchecked_emplace_back(Args &&...args) + requires(std::constructible_from) + { + IV_EXPECT(size() < capacity() && "inplace_vector out-of-memory"); + std::construct_at(end(), std::forward(args)...); + unsafe_set_size(size() + size_type(1)); + return back(); + } + + template constexpr T *try_emplace_back(Args &&...args) { + if (size() == capacity()) [[unlikely]] + return nullptr; + return &unchecked_emplace_back(std::forward(args)...); + } + + template + constexpr T &emplace_back(Args &&...args) + requires(std::constructible_from) + { + if (!try_emplace_back(std::forward(args)...)) [[unlikely]] + throw std::bad_alloc(); + return back(); + } + constexpr T &push_back(const T &x) + requires(std::constructible_from) + { + emplace_back(x); + return back(); + } + constexpr T &push_back(T &&x) + requires(std::constructible_from) + { + emplace_back(std::forward(x)); + return back(); + } + + constexpr T *try_push_back(const T &x) + requires(std::constructible_from) + { + return try_emplace_back(x); + } + constexpr T *try_push_back(T &&x) + requires(std::constructible_from) + { + return try_emplace_back(std::forward(x)); + } + + constexpr T &unchecked_push_back(const T &x) + requires(std::constructible_from) + { + return unchecked_emplace_back(x); + } + constexpr T &unchecked_push_back(T &&x) + requires(std::constructible_from) + { + return unchecked_emplace_back(std::forward(x)); + } + + template R> + constexpr void append_range(R &&rg) + requires(std::constructible_from>) + { + if constexpr (std::ranges::sized_range) { + if (size() + std::ranges::size(rg) > capacity()) [[unlikely]] + throw std::bad_alloc(); + } + for (auto &&e : rg) { + if (size() == capacity()) [[unlikely]] + throw std::bad_alloc(); + emplace_back(std::forward(e)); + } + } + + template + constexpr iterator emplace(const_iterator position, Args &&...args) + requires(std::constructible_from && std::movable) + { + assert_iterator_in_range(position); + auto b = end(); + emplace_back(std::forward(args)...); + auto pos = begin() + (position - begin()); + std::rotate(pos, b, end()); + return pos; + } + + template + constexpr iterator insert(const_iterator position, InputIterator first, InputIterator last) + requires(std::constructible_from> && std::movable) + { + assert_iterator_in_range(position); + if constexpr (std::random_access_iterator) { + if (size() + static_cast(std::distance(first, last)) > capacity()) [[unlikely]] + throw std::bad_alloc{}; + } + auto b = end(); + for (; first != last; ++first) + emplace_back(std::move(*first)); + auto pos = begin() + (position - begin()); + std::rotate(pos, b, end()); + return pos; + } + + template R> + constexpr iterator insert_range(const_iterator position, R &&rg) + requires(std::constructible_from> && std::movable) + { + return insert(position, std::begin(rg), std::end(rg)); + } + + constexpr iterator insert(const_iterator position, std::initializer_list il) + requires(std::constructible_from>> && std::movable) + { + return insert_range(position, il); + } + + constexpr iterator insert(const_iterator position, size_type n, const T &x) + requires(std::constructible_from && std::copyable) + { + assert_iterator_in_range(position); + auto b = end(); + for (size_type i = 0; i < n; ++i) + emplace_back(x); + auto pos = begin() + (position - begin()); + std::rotate(pos, b, end()); + return pos; + } + + constexpr iterator insert(const_iterator position, const T &x) + requires(std::constructible_from && std::copyable) + { + return insert(position, 1, x); + } + + constexpr iterator insert(const_iterator position, T &&x) + requires(std::constructible_from && std::movable) + { + return emplace(position, std::move(x)); + } + + constexpr inplace_vector(std::initializer_list il) + requires(std::constructible_from>> && std::movable) + { + insert(begin(), il); + } + + constexpr inplace_vector(size_type n, const T &value) + requires(std::constructible_from && std::copyable) + { + insert(begin(), n, value); + } + + constexpr explicit inplace_vector(size_type n) + requires(std::constructible_from && std::default_initializable) + { + for (size_type i = 0; i < n; ++i) + emplace_back(T{}); + } + + template // BUGBUG: why not std::ranges::input_iterator? + constexpr inplace_vector(InputIterator first, InputIterator last) + requires(std::constructible_from> && std::movable) + { + insert(begin(), first, last); + } + + template R> + constexpr inplace_vector(beman::from_range_t, R &&rg) + requires(std::constructible_from> && std::movable) + { + insert_range(begin(), std::forward(rg)); + } + + constexpr iterator erase(const_iterator first, const_iterator last) + requires(std::movable) + { + assert_iterator_pair_in_range(first, last); + iterator f = begin() + (first - begin()); + if (first != last) { + unsafe_destroy(std::move(f + (last - first), end(), f), end()); + unsafe_set_size(size() - static_cast(last - first)); + } + return f; + } + + constexpr iterator erase(const_iterator position) + requires(std::movable) + { + return erase(position, position + 1); + } + + constexpr void clear() noexcept { + unsafe_destroy(begin(), end()); + unsafe_set_size(0); + } + + constexpr void resize(size_type sz, const T &c) + requires(std::constructible_from && std::copyable) + { + if (sz == size()) return; + else if (sz > N) [[unlikely]] throw std::bad_alloc{}; + else if (sz > size()) insert(end(), sz - size(), c); + else { + unsafe_destroy(begin() + sz, end()); + unsafe_set_size(sz); + } + } + constexpr void resize(size_type sz) + requires(std::constructible_from && std::default_initializable) + { + if (sz == size()) return; + else if (sz > N) [[unlikely]] throw std::bad_alloc{}; + else if (sz > size()) + while (size() != sz) + emplace_back(T{}); + else { + unsafe_destroy(begin() + sz, end()); + unsafe_set_size(sz); + } + } + + constexpr reference at(size_type pos) { + if (pos >= size()) [[unlikely]] + throw std::out_of_range("inplace_vector::at"); + return details::inplace_vector::index(*this, pos); + } + constexpr const_reference at(size_type pos) const { + if (pos >= size()) [[unlikely]] + throw std::out_of_range("inplace_vector::at"); + return details::inplace_vector::index(*this, pos); + } + + constexpr void pop_back() { + IV_EXPECT(size() > 0 && "pop_back from empty inplace_vector!"); + unsafe_destroy(end() - 1, end()); + unsafe_set_size(size() - 1); + } + + constexpr inplace_vector(const inplace_vector &x) + requires(N == 0 || std::is_trivially_copy_constructible_v) + = default; + + constexpr inplace_vector(const inplace_vector &x) + requires(N != 0 && !std::is_trivially_copy_constructible_v && std::copyable) + { + for (auto &&e : x) + emplace_back(e); + } + + constexpr inplace_vector(inplace_vector &&x) + requires(N == 0 || std::is_trivially_move_constructible_v) + = default; + + constexpr inplace_vector(inplace_vector &&x) + requires(N != 0 && !std::is_trivially_move_constructible_v && std::movable) + { + for (auto &&e : x) + emplace_back(std::move(e)); + } + + constexpr inplace_vector &operator=(const inplace_vector &x) + requires(N == 0 || (std::is_trivially_destructible_v && std::is_trivially_copy_constructible_v && + std::is_trivially_copy_assignable_v)) + = default; + + constexpr inplace_vector &operator=(const inplace_vector &x) + requires(N != 0 && !(std::is_trivially_destructible_v && std::is_trivially_copy_constructible_v && std::is_trivially_copy_assignable_v) && std::copyable) + { + clear(); + for (auto &&e : x) + emplace_back(e); + return *this; + } + + constexpr inplace_vector &operator=(inplace_vector &&x) + requires(N == 0 || (std::is_trivially_destructible_v && std::is_trivially_move_constructible_v && + std::is_trivially_move_assignable_v)) + = default; + + constexpr inplace_vector &operator=(inplace_vector &&x) + requires(N != 0 && !(std::is_trivially_destructible_v && std::is_trivially_move_constructible_v && std::is_trivially_move_assignable_v) && std::movable) + { + clear(); + for (auto &&e : x) + emplace_back(std::move(e)); + return *this; + } + + constexpr void swap(inplace_vector &x) noexcept(N == 0 || (std::is_nothrow_swappable_v && std::is_nothrow_move_constructible_v)) + requires(std::movable) + { + auto tmp = std::move(x); + x = std::move(*this); + (*this) = std::move(tmp); + } + + template + constexpr void assign(InputIterator first, InputIterator last) + requires(std::constructible_from> && std::movable) + { + clear(); + insert(begin(), first, last); + } + template R> + constexpr void assign_range(R &&rg) + requires(std::constructible_from> && std::movable) + { + assign(std::begin(rg), std::end(rg)); + } + constexpr void assign(size_type n, const T &u) + requires(std::constructible_from && std::movable) + { + clear(); + insert(begin(), n, u); + } + constexpr void assign(std::initializer_list il) + requires(std::constructible_from>> && std::movable) + { + clear(); + insert_range(begin(), il); + } + + constexpr friend int /*synth-three-way-result*/ + operator<=>(const inplace_vector & x, const inplace_vector & y) { + if (x.size() < y.size()) return -1; + if (x.size() > y.size()) return +1; + + bool all_equal = true; + bool all_less = true; + for (size_type i = 0; i < x.size(); ++i) { + if (x[i] < y[i]) all_equal = false; + if (x[i] == y[i]) all_less = false; + } + + if (all_equal) return 0; + if (all_less) return -1; + return 1; + } +}; + +template constexpr std::size_t erase(inplace_vector &c, const U &value) { + auto it = std::remove(c.begin(), c.end(), value); + auto r = std::distance(it, c.end()); + c.erase(it, c.end()); + return r; +} + +template constexpr std::size_t erase_if(inplace_vector &c, Predicate pred) { + auto it = std::remove_if(c.begin(), c.end(), pred); + auto r = std::distance(it, c.end()); + c.erase(it, c.end()); + return r; +} + +} // namespace beman + +#undef IV_EXPECT diff --git a/rims_app/src/main.cpp b/rims_app/src/main.cpp index 7874a698246..4999ca5e3c3 100644 --- a/rims_app/src/main.cpp +++ b/rims_app/src/main.cpp @@ -2,7 +2,7 @@ #include "log.hpp" #include "messenger.hpp" -#include "phase_modulation.hpp" +#include "power_control.hpp" #include "placement_unique_ptr.hpp" #include "temperature_measurements.hpp" #include "uart.hpp" diff --git a/rims_app/src/messenger.cpp b/rims_app/src/messenger.cpp index 56f1276fd62..7b15ac8fb8c 100644 --- a/rims_app/src/messenger.cpp +++ b/rims_app/src/messenger.cpp @@ -4,7 +4,7 @@ #include "log.hpp" #include "config.hpp" -#include "phase_modulation.hpp" +#include "power_control.hpp" #include "temperature_measurements.hpp" #include "uart.hpp" diff --git a/rims_app/src/phase_modulation.hpp b/rims_app/src/phase_modulation.hpp deleted file mode 100644 index 55ad790ff61..00000000000 --- a/rims_app/src/phase_modulation.hpp +++ /dev/null @@ -1,152 +0,0 @@ -#pragma once - -/* Kernel event for notifying other threads */ -#include "circular_buffer.hpp" -#include "common.hpp" -#include "proto/ctrl.pb.h" - -#include -#include -#include - -namespace rims { - -extern zephyr_fifo_buffer ctrlIngressQueue; -extern zephyr_fifo_buffer ctrlEgressQueue; - -using ModeOfOperation = ctrl_ModeOfOperation; - -using ModeOfOperationRequest = ctrl_ModeOfOperationRequest; -using ModeOfOperationResponse = ctrl_ModeOfOperationResponse; - -using GroupModulationConfigRequest = ctrl_GroupModulationConfigRequest; -using GroupModulationConfigResponse = ctrl_GroupModulationConfigResponse; -using GroupModulationControlRequest = ctrl_GroupModulationControlRequest; -using GroupModulationControlResponse = ctrl_GroupModulationControlResponse; - -using PhaseModulationConfigRequest = ctrl_PhaseModulationConfigRequest; -using PhaseModulationConfigResponse = ctrl_PhaseModulationConfigResponse; -using PhaseModulationControlRequest = ctrl_PhaseModulationControlRequest; -using PhaseModulationControlResponse = ctrl_PhaseModulationControlResponse; - -constexpr int Channels = 2; - -class PhaseControlBase { - public: - PhaseControlBase(const gpio_dt_spec &gpio); - ~PhaseControlBase() { - disable(); - } - void disable() const; - void enable() const; - - const gpio_dt_spec &_gpio; -}; - -class PhaseModulation : public PhaseControlBase { - public: - PhaseModulation(const gpio_dt_spec &gpio); - ~PhaseModulation() = default; - - // function triggered 100 times / sec - void setPower(float percent) { - _power = percent; - } - void tickFallingEdge(); - void tickRisingEdge() {}; - - private: - SingleShootTimer _triacTimerStart, _triacTimerStop; - float _power; -}; - -class GroupModulation : public PhaseControlBase { - public: - GroupModulation(const gpio_dt_spec &gpio); - - ~GroupModulation() = default; - - void tickFallingEdge() {}; - void tickRisingEdge() {}; - - void setPower(float percent) { - } - - void setCyclesMax(uint32_t cycles) { - _cyclesMax = cycles; - } - uint32_t getCyclesMax() { - return _cyclesMax; - } - - void setCycles(uint32_t cycles) { - _cycles = cycles; - } - uint32_t getCycles() { - return _cycles; - } - - private: - // config variables - uint32_t _cyclesMax{}; // number of cycles in a "full pass" - uint32_t _cycles{}; // number of ON cycles in a "full pass" AKA "power" - - uint32_t _cyclesLeft{}; - uint32_t _cyclesOnLeft{}; -}; - -class Disabled { - public: - void setPower(float percent) { - } - void tickFallingEdge() {}; - void tickRisingEdge() {}; -}; - -/// manages all phase controll messages -class PhaseModulationOrchestrator { - using Variant = std::variant< // - Disabled, - PhaseModulation, - GroupModulation>; - - public: - PhaseModulationOrchestrator(); - void loop(); - - private: - void event_zeroCrossDetection(); - void event_ctrlMessageArrived(); - - void handler_modeOfOperationRequest(const ModeOfOperationRequest &request, ModeOfOperationResponse &resp); - - void handler_groupModulationConfigRequest(const GroupModulationConfigRequest &request, GroupModulationConfigResponse &resp); - void handler_groupModulationControlRequest(const GroupModulationControlRequest &request, GroupModulationControlResponse &resp); - - void handler_phaseModulationConfigRequest(const PhaseModulationConfigRequest &request, PhaseModulationConfigResponse &resp); - void handler_phaseModulationControlRequest(const PhaseModulationControlRequest &request, PhaseModulationControlResponse &resp); - - void setMode(uint8_t ch, ModeOfOperation mode); - ModeOfOperation getMode(uint8_t ch); - - bool setup(); - int gpio_init(uint_fast8_t channel); - - void checkChannel(const uint8_t channel); - void checkCurrentMode(const uint8_t channel, ModeOfOperation mode); - - std::array _channel; - std::array _events; // event from ZCD and from CAN -}; - -class PhaseModulationThread : public ZephyrThread { - public: - PhaseModulationThread(TStackBase &stack) : ZephyrThread(stack, 3, 0, "PhaseModulation"){}; - - void threadMain() override; - - protected: - int do_hardwarenInit() override; -}; - -} // namespace rims diff --git a/rims_app/src/phase_modulation.cpp b/rims_app/src/power_control.cpp similarity index 69% rename from rims_app/src/phase_modulation.cpp rename to rims_app/src/power_control.cpp index c050ea461e6..70d847b418d 100644 --- a/rims_app/src/phase_modulation.cpp +++ b/rims_app/src/power_control.cpp @@ -1,6 +1,7 @@ -#include "phase_modulation.hpp" +#include "power_control.hpp" #include "circular_buffer.hpp" +#include "common.hpp" #include "log.hpp" #include "proto/ctrl.pb.h" #include "zephyr.hpp" @@ -64,54 +65,63 @@ K_FIFO_DEFINE(ctrlEggress); zephyr_fifo_buffer ctrlIngressQueue{ctrlIngress}; zephyr_fifo_buffer ctrlEgressQueue{ctrlEggress}; -constexpr std::array pins = { +constexpr std::array pins = { gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_en), gpios), gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_en), gpios) + }; -struct PhaseControl { - int cycles{100}; - PhaseControl() { - } -}; - -PhaseControlBase::PhaseControlBase(const gpio_dt_spec &gpio) : _gpio{gpio} { - ULOG_INFO("triac GPIO %s/%d", gpio.port->name, gpio.pin); -} - -void PhaseControlBase::enable() const { - zephyr::gpio::pin_set_dt(_gpio, 1); -} - -void PhaseControlBase::disable() const { - zephyr::gpio::pin_set_dt(_gpio, 0); -} - PhaseModulation::PhaseModulation(const gpio_dt_spec &gpio) - : PhaseControlBase{gpio}, // - _triacTimerStart{[this]() { this->enable(); }, std::chrono::milliseconds{0}}, - _triacTimerStop{[this]() { this->disable(); }, std::chrono::milliseconds{0}} { + : PinControlStrategyBase(gpio), // + _triacTimerStart{[this]() { this->on(); }, std::chrono::milliseconds{0}}, + _triacTimerStop{[this]() { this->off(); }, std::chrono::milliseconds{0}} { ULOG_INFO("PhaseModulation started for gpio %d", gpio.pin); } -void PhaseModulation::tickFallingEdge() { - // run on zero crossing, calculate time from ZCD to triac enable, and set timer accordingly - /// TODO calculate time needed to achieve given power requirements and start a single shoot timer +static constexpr microseconds_u16_t tous(float percent, microseconds_u16_t full) { + const float us = full.count(); + return microseconds_u16_t{static_cast(us * percent / 100.0f)}; +} +static constexpr microseconds_u16_t delay(float percent, microseconds_u16_t full) { + return full - tous(percent, full); +} - // power - // auto fullCycleTime = std::chrono::milliseconds{10}; - auto offset = (1000 - int(_power * 10)) * std::chrono::microseconds{10}; - _triacTimerStart.setInterval(offset); +void PhaseModulation::zeroCrossDetectionTick(rims::microseconds_u16_t usToNext) { + // Disable triac immediately on zero detect to fix flickering at low power + this->off(); + + constexpr auto minimalGatePulse = microseconds_u16_t{75}; + constexpr auto minimalLatchTime = microseconds_u16_t{3}; + constexpr auto minimalSafeMargin = microseconds_u16_t{120}; + + // 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 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.setDuration(onDelay); _triacTimerStart.start(); - _triacTimerStop.setInterval(offset + std::chrono::microseconds{100}); + + // Schedule triac to turn OFF shortly after firing (e.g., 100 us pulse) + _triacTimerStop.setDuration(onDelay + minimalGatePulse); _triacTimerStop.start(); } -GroupModulation::GroupModulation(const gpio_dt_spec &gpio) : PhaseControlBase(gpio) { +GroupModulation::GroupModulation(const gpio_dt_spec &gpio) : PinControlStrategyBase(gpio) { + ULOG_INFO("GroupModulation started for gpio %d", gpio.pin); } -PhaseModulationOrchestrator::PhaseModulationOrchestrator() : _channel{Disabled{}, Disabled{}} { - _channel[0].emplace(pins[0]); +PhaseModulationOrchestrator::PhaseModulationOrchestrator() : _channelControlStrategy{NoModulation{}, NoModulation{}} { + _channelControlStrategy[0].emplace(pins[0]); + + setMode(0, ModeOfOperation::ctrl_ModeOfOperation_ManualPower); ZeroCrossDetectionEventQueue.k_event_init(_events[0]); ctrlIngressQueue.k_event_init(_events[1]); @@ -119,7 +129,6 @@ PhaseModulationOrchestrator::PhaseModulationOrchestrator() : _channel{Disabled{} // thread main void PhaseModulationOrchestrator::loop() { - /// TODO power limiter (after power measurement) while (1) { try { auto ret = zephyr::event_pool::k_poll_forever(_events); @@ -135,8 +144,8 @@ void PhaseModulationOrchestrator::loop() { constexpr float PI = 3.141592f; constexpr float FREQUENCY = 0.25f; -constexpr float MIN_VALUE = 10.0f; -constexpr float MAX_VALUE = 20.0f; +constexpr float MIN_VALUE = 0.0f; +constexpr float MAX_VALUE = 25.0f; float sinewave(std::chrono::steady_clock::time_point timePoint) { using namespace std::chrono; @@ -145,58 +154,49 @@ float sinewave(std::chrono::steady_clock::time_point timePoint) { float elapsedSeconds = duration(timePoint - startTime).count(); float sineValue = sinf(2.0f * PI * FREQUENCY * elapsedSeconds); - // return std::sin(2.0 * PI * FREQUENCY * elapsedSeconds); return MIN_VALUE + (sineValue + 1.0f) * 0.5f * (MAX_VALUE - MIN_VALUE); } void PhaseModulationOrchestrator::event_zeroCrossDetection() { - auto tickFallingEdge = [](auto &channel) { channel.tickFallingEdge(); }; - auto tickRisingEdge = [](auto &channel) { channel.tickRisingEdge(); }; - auto setPower = [](auto &channel) { channel.setPower(sinewave(std::chrono::steady_clock::now())); }; + _powerPublisher.notifyPowerUpdate(sinewave(std::chrono::steady_clock::now()), 0); - std::visit(setPower, _channel[0]); - /// TODO check proper channel ZeroCrossDetectionEventQueue.try_consume([&](ZeroCrossDetectionEvent &event) { - if (event.state) { - std::visit(tickRisingEdge, _channel[event.channel]); - } else { - std::visit(tickFallingEdge, _channel[event.channel]); - } + std::visit([&event](auto &channel) { channel.zeroCrossDetectionTick(event.timeToNext); }, _channelControlStrategy[event.channel]); }); } void PhaseModulationOrchestrator::event_ctrlMessageArrived() { constexpr auto modeOfOperationRequestHandler = std::make_tuple( - ctrl_IngressMessage_modeOfOperationRequest_tag, - ctrl_EgressMessage_modeOfOperationResponse_tag, + ctrl_IngressMessage_mode_of_operation_request_tag, + ctrl_EgressMessage_mode_of_operation_response_tag, &PhaseModulationOrchestrator::handler_modeOfOperationRequest, ctrl_ModeOfOperationResponse ctrl_ModeOfOperationResponse_init_zero ); constexpr auto groupModulationConfigRequestHandler = std::make_tuple( - ctrl_IngressMessage_groupModulationConfigRequest_tag, - ctrl_EgressMessage_groupModulationConfigResponse_tag, + ctrl_IngressMessage_group_modulation_config_request_tag, + ctrl_EgressMessage_group_modulation_config_response_tag, &PhaseModulationOrchestrator::handler_groupModulationConfigRequest, ctrl_GroupModulationConfigResponse ctrl_GroupModulationConfigResponse_init_zero ); constexpr auto groupModulationControlRequestHandler = std::make_tuple( - ctrl_IngressMessage_groupModulationControlRequest_tag, - ctrl_EgressMessage_groupModulationControlResponse_tag, + ctrl_IngressMessage_group_modulation_control_request_tag, + ctrl_EgressMessage_group_modulation_control_response_tag, &PhaseModulationOrchestrator::handler_groupModulationControlRequest, ctrl_GroupModulationControlResponse ctrl_GroupModulationControlResponse_init_zero ); constexpr auto phaseModulationConfigRequestHandler = std::make_tuple( - ctrl_IngressMessage_phaseModulationConfigRequest_tag, - ctrl_EgressMessage_phaseModulationConfigResponse_tag, + ctrl_IngressMessage_phase_modulation_config_request_tag, + ctrl_EgressMessage_phase_modulation_config_response_tag, &PhaseModulationOrchestrator::handler_phaseModulationConfigRequest, ctrl_PhaseModulationConfigResponse ctrl_PhaseModulationConfigResponse_init_zero ); constexpr auto phaseModulationControlRequestHandler = std::make_tuple( - ctrl_IngressMessage_phaseModulationControlRequest_tag, - ctrl_EgressMessage_phaseModulationControlResponse_tag, + ctrl_IngressMessage_phase_modulation_control_request_tag, + ctrl_EgressMessage_phase_modulation_control_response_tag, &PhaseModulationOrchestrator::handler_phaseModulationControlRequest, ctrl_PhaseModulationControlResponse ctrl_PhaseModulationControlResponse_init_zero ); @@ -295,16 +295,16 @@ void PhaseModulationOrchestrator::handler_groupModulationControlRequest( // GroupModulationControlResponse &resp ) { ULOG_INFO("GroupModulationControlRequest request handler"); - checkChannel(request.channel_id); - checkCurrentMode(request.channel_id, ctrl_ModeOfOperation_GroupModulation); + // checkChannel(request.channel_id); + // checkCurrentMode(request.channel_id, ctrl_ModeOfOperation_GroupModulation); - auto &alg = std::get(_channel[request.channel_id]); + // auto &alg = std::get(_channel[request.channel_id]); - if (request.has_cycles) { - alg.setCycles(request.cycles); - } + // if (request.has_cycles) { + // alg.setCycles(request.cycles); + // } - resp.cycles = alg.getCycles(); + // resp.cycles = alg.getCycles(); } void PhaseModulationOrchestrator::handler_phaseModulationConfigRequest( // @@ -313,7 +313,7 @@ void PhaseModulationOrchestrator::handler_phaseModulationConfigRequest( // ) { ULOG_INFO("PhaseModulationConfigRequest request handler"); checkChannel(request.channel_id); - checkCurrentMode(request.channel_id, ctrl_ModeOfOperation_PhaseModulation); + // checkCurrentMode(request.channel_id, ctrl_ModeOfOperation_PhaseModulation); } void PhaseModulationOrchestrator::handler_phaseModulationControlRequest( // @@ -322,32 +322,34 @@ void PhaseModulationOrchestrator::handler_phaseModulationControlRequest( // ) { ULOG_INFO("PhaseModulationControlRequest request handler"); checkChannel(request.channel_id); - checkCurrentMode(request.channel_id, ctrl_ModeOfOperation_PhaseModulation); + // checkCurrentMode(request.channel_id, ctrl_ModeOfOperation_PhaseModulation); } void PhaseModulationOrchestrator::checkChannel(uint8_t channel_id) { - if (channel_id >= Channels) throw ctrl::wrong_channel{}; + if (channel_id >= MaxChannels) throw ctrl::wrong_channel{}; } void PhaseModulationOrchestrator::checkCurrentMode(const uint8_t channel, ModeOfOperation mode) { if (getMode(channel) != mode) throw ctrl::wrong_mode{}; } +/// TODO set power control + void PhaseModulationOrchestrator::setMode(uint8_t ch, ModeOfOperation mode) { switch (mode) { case ctrl_ModeOfOperation_Disabled: - _channel[ch] = Disabled{}; + _chModeOfOperationStorage[ch].emplace(); break; - case ctrl_ModeOfOperation_GroupModulation: - _channel[ch].emplace(pins[ch]); - break; - case ctrl_ModeOfOperation_PhaseModulation: - _channel[ch].emplace(pins[ch]); - break; - case ctrl_ModeOfOperation_TemperatureSlopeModulation: + case ctrl_ModeOfOperation_ConstantTemperature: throw ctrl::unsuported_mode{}; - - case ctrl_ModeOfOperation_ConstantTemperatureModulation: + case ctrl_ModeOfOperation_TemperatureSlope: + throw ctrl::unsuported_mode{}; + case ctrl_ModeOfOperation_ManualPower: + _chModeOfOperationStorage[ch].emplace(strategy(ch), _powerPublisher, ch); + break; + case ctrl_ModeOfOperation_TemperatureWindow: + throw ctrl::unsuported_mode{}; + case ctrl_ModeOfOperation_AutoTune: throw ctrl::unsuported_mode{}; default: @@ -357,15 +359,15 @@ void PhaseModulationOrchestrator::setMode(uint8_t ch, ModeOfOperation mode) { } ModeOfOperation PhaseModulationOrchestrator::getMode(uint8_t ch) { - if (std::holds_alternative(_channel.at(ch))) { - return ctrl_ModeOfOperation_Disabled; - } - if (std::holds_alternative(_channel.at(ch))) { - return ctrl_ModeOfOperation_PhaseModulation; - } - if (std::holds_alternative(_channel.at(ch))) { - return ctrl_ModeOfOperation_GroupModulation; - } + // if (std::holds_alternative(_channel.at(ch))) { + // return ctrl_ModeOfOperation_Disabled; + // } + // if (std::holds_alternative(_channel.at(ch))) { + // return ctrl_ModeOfOperation_PhaseModulation; + // } + // if (std::holds_alternative(_channel.at(ch))) { + // return ctrl_ModeOfOperation_GroupModulation; + // } return ctrl_ModeOfOperation_Disabled; } diff --git a/rims_app/src/power_control.hpp b/rims_app/src/power_control.hpp new file mode 100644 index 00000000000..1c63627de09 --- /dev/null +++ b/rims_app/src/power_control.hpp @@ -0,0 +1,293 @@ +#pragma once + +/* Kernel event for notifying other threads */ +#include "circular_buffer.hpp" +#include "common.hpp" +#include "inplace_vector.hpp" +#include "proto/ctrl.pb.h" + +#include +#include +#include +#include +#include + +namespace rims { + +extern zephyr_fifo_buffer ctrlIngressQueue; +extern zephyr_fifo_buffer ctrlEgressQueue; + +using ModeOfOperation = ctrl_ModeOfOperation; + +using ModeOfOperationRequest = ctrl_ModeOfOperationRequest; +using ModeOfOperationResponse = ctrl_ModeOfOperationResponse; + +using GroupModulationConfigRequest = ctrl_GroupModulationConfigRequest; +using GroupModulationConfigResponse = ctrl_GroupModulationConfigResponse; +using GroupModulationControlRequest = ctrl_GroupModulationControlRequest; +using GroupModulationControlResponse = ctrl_GroupModulationControlResponse; + +using PhaseModulationConfigRequest = ctrl_PhaseModulationConfigRequest; +using PhaseModulationConfigResponse = ctrl_PhaseModulationConfigResponse; +using PhaseModulationControlRequest = ctrl_PhaseModulationControlRequest; +using PhaseModulationControlResponse = ctrl_PhaseModulationControlResponse; + +constexpr int MaxChannels = 2; + +class PowerControlStrategy { + public: + PowerControlStrategy() = default; + virtual ~PowerControlStrategy() noexcept = default; + // function called each AC signal crossed 0 + // 100 times a second + virtual void zeroCrossDetectionTick(rims::microseconds_u16_t usToNext) = 0; + virtual void setPower(float power) { + _power = power; + }; + virtual float power() const { + return _power; + }; + + virtual void off() const = 0; + virtual void on() const = 0; + + float _power; +}; + +class PinControlStrategyBase : public PowerControlStrategy { + public: + PinControlStrategyBase(const gpio_dt_spec &gpio) : _gpio{gpio} { + } + ~PinControlStrategyBase() { + off(); + } + + void off() const override { + zephyr::gpio::pin_set_dt(_gpio, 0); + } + + void on() const override { + zephyr::gpio::pin_set_dt(_gpio, 1); + } + + private: + const gpio_dt_spec &_gpio; +}; + +class PhaseModulation : public PinControlStrategyBase { + public: + PhaseModulation(const gpio_dt_spec &gpio); + ~PhaseModulation() = default; + void zeroCrossDetectionTick(rims::microseconds_u16_t usToNext) override; + + private: + SingleShootTimer _triacTimerStart, _triacTimerStop; +}; + +class GroupModulation : public PinControlStrategyBase { + public: + GroupModulation(const gpio_dt_spec &gpio); + + ~GroupModulation() = default; + + void zeroCrossDetectionTick(rims::microseconds_u16_t usToNext) override; + + void setCyclesMax(uint32_t cycles) { + _cyclesMax = cycles; + } + uint32_t getCyclesMax() { + return _cyclesMax; + } + + void setCycles(uint32_t cycles) { + _cycles = cycles; + } + uint32_t getCycles() { + return _cycles; + } + + private: + // config variables + uint32_t _cyclesMax{}; // number of cycles in a "full pass" + uint32_t _cycles{}; // number of ON cycles in a "full pass" AKA "power" + + uint32_t _cyclesLeft{}; + uint32_t _cyclesOnLeft{}; +}; + +class NoModulation : public PowerControlStrategy { + public: + void zeroCrossDetectionTick(rims::microseconds_u16_t) override { + } + void on() const override { + } + void off() const override { + } +}; + +class Linked : public PowerControlStrategy { + void zeroCrossDetectionTick(rims::microseconds_u16_t) override { + } + void on() const override { + } + void off() const override { + } +}; + +class ModeOfOperationStrategy { + public: + ModeOfOperationStrategy(PowerControlStrategy &pc) : _powerControl{pc} { + } + virtual ~ModeOfOperationStrategy() { + _powerControl.off(); + }; + virtual void zeroCrossDetectionTick(microseconds_u16_t usToNext) = 0; + + PowerControlStrategy &_powerControl; +}; + +class DisabledMode : public ModeOfOperationStrategy { + inline static NoModulation _noPowerModulationStrategy{}; + + public: + DisabledMode() : ModeOfOperationStrategy(_noPowerModulationStrategy){}; + void zeroCrossDetectionTick(microseconds_u16_t usToNext) override { /* nothing */ + } +}; + +class TargetPowerObserver { + public: + virtual ~TargetPowerObserver() = default; + virtual void onPowerUpdate(float newPowerPercent) = 0; +}; + +class PowerPublisher { + public: + void subscribe(TargetPowerObserver *observer, uint8_t channel_id) { + _observers.push_back(observer); + _channels.push_back(channel_id); + } + + void unsubscribe(TargetPowerObserver *observer) { + auto it = std::find(_observers.cbegin(), _observers.cend(), observer); + if (it != _observers.cend()) { + // Get the index of the found observer + std::size_t index = std::distance(_observers.cbegin(), it); + + _observers.erase(it); + _channels.erase(_channels.cbegin() + index); + } + } + + void notifyPowerUpdate(float newPowerPercent, uint8_t channel) { + for (std::size_t i = 0; i < _observers.size(); ++i) { + if (_observers[i] && _channels[i] == channel) { + _observers[i]->onPowerUpdate(newPowerPercent); + } + } + } + + private: + beman::inplace_vector _observers; + beman::inplace_vector _channels; +}; + +class PowerSubscription { + public: + PowerSubscription(PowerPublisher &publisher, TargetPowerObserver *observer, uint8_t observable_channel) + : _publisher(publisher), _observer(observer) { + _publisher.subscribe(_observer, observable_channel); + } + + ~PowerSubscription() { + _publisher.unsubscribe(_observer); + } + + // Non-copyable + PowerSubscription(const PowerSubscription &) = delete; + PowerSubscription &operator=(const PowerSubscription &) = delete; + + // Movable if needed + PowerSubscription(PowerSubscription &&) = default; + PowerSubscription &operator=(PowerSubscription &&) = delete; + + private: + PowerPublisher &_publisher; + TargetPowerObserver *_observer; +}; + +class ManualPowerMode : public ModeOfOperationStrategy, public TargetPowerObserver { + public: + ManualPowerMode(PowerControlStrategy &pc, PowerPublisher &publisher, uint8_t channel) + : ModeOfOperationStrategy(pc), _subscription(publisher, this, channel) { + } + // update state of heat element + void zeroCrossDetectionTick(microseconds_u16_t usToNext) override { /* nothing */ + _powerControl.zeroCrossDetectionTick(usToNext); + } + + void onPowerUpdate(float newPowerPercent) override { + // only passing power percent to + _powerControl.setPower(newPowerPercent); + } + + private: + PowerSubscription _subscription; +}; + +/// manages all phase controll messages +class PhaseModulationOrchestrator { + using ControlStrategy = std::variant< // + NoModulation, + PhaseModulation, + GroupModulation>; + + using OperationStrategy = std::variant; + + public: + PhaseModulationOrchestrator(); + void loop(); + + private: + void event_zeroCrossDetection(); + void event_ctrlMessageArrived(); + + void handler_modeOfOperationRequest(const ModeOfOperationRequest &request, ModeOfOperationResponse &resp); + + void handler_groupModulationConfigRequest(const GroupModulationConfigRequest &request, GroupModulationConfigResponse &resp); + void handler_groupModulationControlRequest(const GroupModulationControlRequest &request, GroupModulationControlResponse &resp); + + void handler_phaseModulationConfigRequest(const PhaseModulationConfigRequest &request, PhaseModulationConfigResponse &resp); + void handler_phaseModulationControlRequest(const PhaseModulationControlRequest &request, PhaseModulationControlResponse &resp); + + void setMode(uint8_t ch, ModeOfOperation mode); + ModeOfOperation getMode(uint8_t ch); + + bool setup(); + int gpio_init(uint_fast8_t channel); + + void checkChannel(const uint8_t channel); + void checkCurrentMode(const uint8_t channel, ModeOfOperation mode); + + constexpr PowerControlStrategy &strategy(uint8_t channel) { + return std::visit([](auto &s) -> PowerControlStrategy & { return s; }, _channelControlStrategy[channel]); + } + std::array _channelControlStrategy; // storage for different strategies, per channal + std::array _chModeOfOperationStorage; + + PowerPublisher _powerPublisher; + + std::array _events; // event from ZCD and from CAN +}; + +class PhaseModulationThread : public ZephyrThread { + public: + PhaseModulationThread(TStackBase &stack) : ZephyrThread(stack, 3, 0, "PhaseModulation"){}; + + void threadMain() override; + + protected: + int do_hardwarenInit() override; +}; + +} // namespace rims diff --git a/rims_app/src/temperature_measurements.cpp b/rims_app/src/temperature_measurements.cpp index 280987d2be4..40ea8bf1c83 100644 --- a/rims_app/src/temperature_measurements.cpp +++ b/rims_app/src/temperature_measurements.cpp @@ -211,7 +211,7 @@ void TemperatureSampler::setSamplerConfig(const temperature_SamplerConfig &confi std::chrono::duration_cast(_samplerTimer.interval()).count(), config.period_ms ); - this->_samplerTimer.setInterval(std::chrono::milliseconds{config.period_ms}); + this->_samplerTimer.setDuration(std::chrono::milliseconds{config.period_ms}); } } diff --git a/rims_app/src/uart.cpp b/rims_app/src/uart.cpp index 8e00391b2d9..fee2c80fee1 100644 --- a/rims_app/src/uart.cpp +++ b/rims_app/src/uart.cpp @@ -167,7 +167,7 @@ void AsyncUART::readByteUart() { } // throw on buffer overflow else if (rxBuffer().full()) { - rxBuffer().clean(); + rxBuffer().clear(); _faultFlag = true; // throw uart_rx_buffer_overflow{}; } @@ -176,7 +176,7 @@ void AsyncUART::readByteUart() { if (rxBuffer().size() > 1) { processMessage(); } else { - rxBuffer().clean(); + rxBuffer().clear(); } } } @@ -207,7 +207,7 @@ AsyncUART::rx_buffer_t &AsyncUART::rxBuffer() { void AsyncUART::switchRxBuffer() { _currentBufferIndex = _currentBufferIndex == 1 ? 0 : 1; - rxBuffer().clean(); + rxBuffer().clear(); } bool AsyncUART::txHasByte() const { @@ -234,7 +234,7 @@ void AsyncUART::handleRxBufferOverflowError(const uart_rx_buffer_overflow &e) { k_work_submit(&uart_log_work.work); // clean current buffer as is is usless for us now - rxBuffer().clean(); + rxBuffer().clear(); // indicate the driver to skip bytes until end of frame _faultFlag = true; diff --git a/rims_app/src/uart.hpp b/rims_app/src/uart.hpp index c24a333a8b0..73396f9ce47 100644 --- a/rims_app/src/uart.hpp +++ b/rims_app/src/uart.hpp @@ -2,6 +2,7 @@ #include "circular_buffer.hpp" #include "common.hpp" +#include "inplace_vector.hpp" #include #include @@ -17,7 +18,7 @@ class uart_rx_buffer_overflow; class uart_rx_not_ready_error; class AsyncUART { - using rx_buffer_t = static_vector; + using rx_buffer_t = beman::inplace_vector; using tx_buffer_t = ring_buffer; public: diff --git a/rims_app/src/zero_cross_detection.cpp b/rims_app/src/zero_cross_detection.cpp index 299e2cbdcd3..fa5b9646e4b 100644 --- a/rims_app/src/zero_cross_detection.cpp +++ b/rims_app/src/zero_cross_detection.cpp @@ -30,15 +30,12 @@ void ZeroCrossDetection::interrupt_handler(const device *dev, struct gpio_callba auto _this = CONTAINER_OF(cb, ZeroCrossDetection, cb); int pin_state = gpio_pin_get(dev, gpio_pin_mask_to_index(pins)); - if (not _this->_msgQueue->try_produce([&](ZeroCrossDetectionEvent &event) { - event.state = static_cast(pin_state); - event.channel = _this->_channel; - return true; - }) - ) { - } - - if(pin_state==0){ + if (pin_state == 0) { + // calculate true zero based on statistical data + _this->_trueZeroDetection.setDuration(_this->pulsWidth() / 2); + _this->_trueZeroDetection.start(); + + // add sample _this->_cyclesWidths.push_back(std::chrono::duration_cast(clock::now() - _this->_pulseDown)); _this->_pulseDown = clock::now(); } else { @@ -48,10 +45,21 @@ void ZeroCrossDetection::interrupt_handler(const device *dev, struct gpio_callba ZeroCrossDetection::ZeroCrossDetection(const gpio_dt_spec &gpio, ZCDFifo_t *queue, uint8_t channel) : _channel{channel}, _msgQueue{queue}, cb{ZeroCrossDetection::interrupt_handler, BIT(gpio.pin)}, // - _intCheckSem{0, 1}, _intCheckTimer{_intCheckSem, std::chrono::seconds{30}} { + _intCheckSem{0, 1}, _intCheckTimer{_intCheckSem, std::chrono::seconds{30}}, + _trueZeroDetection{ + [&]() { + this->_msgQueue->try_produce([&](ZeroCrossDetectionEvent &event) { + /// todo add timestamp? + event.channel = this->_channel; + event.timeToNext = this->cycleWidth(); + return true; + }); + }, + std::chrono::microseconds{500} + } { _intCheckTimer.start(); _pulseDown = clock::now(); - + zephyr::gpio::pin_interrupt_configure(gpio, GPIO_INT_EDGE_BOTH); zephyr::gpio::add_callback(gpio, cb); } @@ -107,19 +115,15 @@ int ZeroCrossDetectionThread::do_hardwarenInit() { return 0; } -microseconds_u16_t ZeroCrossDetection::pulsWidth() const -{ - if(_pulsWidths.empty()) - return std::chrono::milliseconds{1}; - +microseconds_u16_t ZeroCrossDetection::pulsWidth() const { + if (_pulsWidths.empty()) return std::chrono::milliseconds{1}; + auto totalus = std::accumulate(_pulsWidths.begin(), _pulsWidths.end(), microseconds_u32_t{}); return totalus / _pulsWidths.size(); } -microseconds_u16_t ZeroCrossDetection::cycleWidth() const -{ - if(_cyclesWidths.empty()) - return std::chrono::milliseconds{10}; - +microseconds_u16_t ZeroCrossDetection::cycleWidth() const { + if (_cyclesWidths.empty()) return std::chrono::milliseconds{10}; + auto totalus = std::accumulate(_cyclesWidths.begin(), _cyclesWidths.end(), microseconds_u32_t{}); return totalus / _cyclesWidths.size(); } diff --git a/rims_app/src/zero_cross_detection.hpp b/rims_app/src/zero_cross_detection.hpp index d042e160355..a97e4947fb3 100644 --- a/rims_app/src/zero_cross_detection.hpp +++ b/rims_app/src/zero_cross_detection.hpp @@ -11,8 +11,8 @@ namespace rims { struct ZeroCrossDetectionEvent { - bool state : 1; - int channel : 4; // max 2^4 channels + rims::microseconds_u16_t timeToNext; + uint8_t channel; }; extern zephyr_fifo_buffer ZeroCrossDetectionEventQueue; @@ -40,6 +40,8 @@ class ZeroCrossDetection { zephyr::semaphore::sem _intCheckSem; RecurringSemaphoreTimer _intCheckTimer; + + SingleShootTimer _trueZeroDetection; clock::time_point _pulseDown{}; rims::ring_buffer _pulsWidths; -- 2.45.2 From fd2e07f3003329400bccda4d64b08801970b802f Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Wed, 30 Apr 2025 08:53:46 +0200 Subject: [PATCH 13/18] fix problems with ctrl messages handling --- boards/bartoszek/rims/rims_h503cbt.dts | 6 +- rims_app/proto/ctrl.proto | 57 ++++++----- rims_app/src/messenger.cpp | 4 +- rims_app/src/power_control.cpp | 125 +++++++++++++++++-------- rims_app/src/power_control.hpp | 23 +++-- 5 files changed, 144 insertions(+), 71 deletions(-) diff --git a/boards/bartoszek/rims/rims_h503cbt.dts b/boards/bartoszek/rims/rims_h503cbt.dts index e741e30848a..194eacf0dc3 100644 --- a/boards/bartoszek/rims/rims_h503cbt.dts +++ b/boards/bartoszek/rims/rims_h503cbt.dts @@ -219,7 +219,11 @@ pinctrl-0 = <&usart1_tx_pb14 &usart1_rx_pb15>; pinctrl-names = "default"; - current-speed = <1000000>; + current-speed = <250000>; + + data-bits = <8>; + //parity = <0>; + stop-bits = "2"; // Sets to one stop bit // clocks = <&rcc STM32_CLOCK(APB2, 14)>, <&rcc STM32_SRC_PLL2_Q USART1_SEL(1)>; status = "okay"; diff --git a/rims_app/proto/ctrl.proto b/rims_app/proto/ctrl.proto index 428980a46bf..09324454448 100644 --- a/rims_app/proto/ctrl.proto +++ b/rims_app/proto/ctrl.proto @@ -17,16 +17,19 @@ enum Error { // Power control method used to adjust the heating element on a given channel. enum PowerControlAlgorithm { + // NoModulation disables the output. + NoModulation = 0; + // GroupModulation adjusts power by delivering full AC sine wave cycles in groups. // It delivers N full cycles ON followed by M cycles OFF (zero voltage). // Example: For 50% power, it might deliver 5 cycles on, then 5 cycles off. // To modyfy number of grouped ac cycles, use GroupModulationConfigRequest - GroupModulation = 0; + GroupModulation = 1; // PhaseModulation adjusts power by controlling the phase angle of each AC cycle. // By cutting off part of the sine wave, it achieves finer control over the delivered power. // Example: A phase angle of 90 degrees allows only half of the cycle to be applied. - PhaseModulation = 1; + PhaseModulation = 2; } // Mode of operation used to control the heating element on a given channel. @@ -60,13 +63,13 @@ enum ModeOfOperation { message PowerControlAlgorithmRequest { uint32 channel_id = 1; - optional ModeOfOperation mode = 2; + optional PowerControlAlgorithm alg = 2; } message PowerControlAlgorithmResponse { uint32 channel_id = 1; - PowerControlAlgorithm mode = 2; + PowerControlAlgorithm alg = 2; optional Error error = 254; } @@ -157,25 +160,31 @@ message PhaseModulationControlResponse optional Error error = 254; } -//message ActiveChannelsRequest +// message ActiveChannelsRequest //{ // // gets number of active channels -//} +// } -//message ActiveChannelResponse +// message ActiveChannelResponse //{ // // info about ZCD, linked channels? -//} +// } -//message LinkChannelRequest +// message LinkChannelRequest //{ // repeated uint32 channel_id =1 [ (nanopb).max_count = 16 ]; -//} +// } -//message LinkChannelResponse +// message LinkChannelResponse //{ // uint32 cchannel_id; -//} +// } + +// Device on start uses NoModulation power control algorithm, meaning that is efectively disabled. +// To start +// * select a proper algorithm +// * optionally, set it up +// * set mode of operation // only those messages are send through interface message IngressMessage @@ -183,28 +192,30 @@ message IngressMessage uint32 request_id = 255; oneof data { - ModeOfOperationRequest mode_of_operation_request = 1; + PowerControlAlgorithmRequest power_control_algorithm_request = 1; + ModeOfOperationRequest mode_of_operation_request = 2; - GroupModulationConfigRequest group_modulation_config_request = 2; - GroupModulationControlRequest group_modulation_control_request = 3; + GroupModulationConfigRequest group_modulation_config_request = 3; + GroupModulationControlRequest group_modulation_control_request = 4; - PhaseModulationConfigRequest phase_modulation_config_request = 4; - PhaseModulationControlRequest phase_modulation_control_request = 5; + PhaseModulationConfigRequest phase_modulation_config_request = 5; + PhaseModulationControlRequest phase_modulation_control_request = 6; } }; message EgressMessage { - optional uint32 request_id = 255; + optional uint32 request_id = 255; // not set for broadcast oneof data { - ModeOfOperationResponse mode_of_operation_response = 1; + PowerControlAlgorithmResponse power_control_algorithm_response = 1; + ModeOfOperationResponse mode_of_operation_response = 2; - GroupModulationConfigResponse group_modulation_config_response = 2; - GroupModulationControlResponse group_modulation_control_response = 3; + GroupModulationConfigResponse group_modulation_config_response = 3; + GroupModulationControlResponse group_modulation_control_response = 4; - PhaseModulationConfigResponse phase_modulation_config_response = 4; - PhaseModulationControlResponse phase_modulation_control_response = 5; + PhaseModulationConfigResponse phase_modulation_config_response = 5; + PhaseModulationControlResponse phase_modulation_control_response = 6; // BROADCAST } }; diff --git a/rims_app/src/messenger.cpp b/rims_app/src/messenger.cpp index 7b15ac8fb8c..1a7765d7768 100644 --- a/rims_app/src/messenger.cpp +++ b/rims_app/src/messenger.cpp @@ -290,12 +290,12 @@ void MessengerThread::event_dataArrived() { } case 0x02: /// TODO configuration endpoint ID { - handle_configIngressMsg(id, stream, buf.device); + handle_ctrlIngressMsg(id, stream, buf.device); break; } case 0x03: /// TODO phase controll endpoint ID { - handle_ctrlIngressMsg(id, stream, buf.device); + handle_configIngressMsg(id, stream, buf.device); break; } case 0x04: { diff --git a/rims_app/src/power_control.cpp b/rims_app/src/power_control.cpp index 70d847b418d..72faab48f69 100644 --- a/rims_app/src/power_control.cpp +++ b/rims_app/src/power_control.cpp @@ -68,7 +68,6 @@ zephyr_fifo_buffer ctrlEgressQueue{ctrlEggress}; constexpr std::array pins = { gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_en), gpios), gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_en), gpios) - }; PhaseModulation::PhaseModulation(const gpio_dt_spec &gpio) @@ -119,10 +118,6 @@ GroupModulation::GroupModulation(const gpio_dt_spec &gpio) : PinControlStrategyB } PhaseModulationOrchestrator::PhaseModulationOrchestrator() : _channelControlStrategy{NoModulation{}, NoModulation{}} { - _channelControlStrategy[0].emplace(pins[0]); - - setMode(0, ModeOfOperation::ctrl_ModeOfOperation_ManualPower); - ZeroCrossDetectionEventQueue.k_event_init(_events[0]); ctrlIngressQueue.k_event_init(_events[1]); } @@ -142,23 +137,23 @@ void PhaseModulationOrchestrator::loop() { } } -constexpr float PI = 3.141592f; -constexpr float FREQUENCY = 0.25f; -constexpr float MIN_VALUE = 0.0f; -constexpr float MAX_VALUE = 25.0f; +// constexpr float PI = 3.141592f; +// constexpr float FREQUENCY = 0.25f; +// constexpr float MIN_VALUE = 0.0f; +// constexpr float MAX_VALUE = 25.0f; -float sinewave(std::chrono::steady_clock::time_point timePoint) { - using namespace std::chrono; - static auto startTime = steady_clock::now(); +// float sinewave(std::chrono::steady_clock::time_point timePoint) { +// using namespace std::chrono; +// static auto startTime = steady_clock::now(); - float elapsedSeconds = duration(timePoint - startTime).count(); - float sineValue = sinf(2.0f * PI * FREQUENCY * elapsedSeconds); +// float elapsedSeconds = duration(timePoint - startTime).count(); +// float sineValue = sinf(2.0f * PI * FREQUENCY * elapsedSeconds); - return MIN_VALUE + (sineValue + 1.0f) * 0.5f * (MAX_VALUE - MIN_VALUE); -} +// return MIN_VALUE + (sineValue + 1.0f) * 0.5f * (MAX_VALUE - MIN_VALUE); +// } void PhaseModulationOrchestrator::event_zeroCrossDetection() { - _powerPublisher.notifyPowerUpdate(sinewave(std::chrono::steady_clock::now()), 0); + // _powerPublisher.notifyPowerUpdate(sinewave(std::chrono::steady_clock::now()), 0); ZeroCrossDetectionEventQueue.try_consume([&](ZeroCrossDetectionEvent &event) { std::visit([&event](auto &channel) { channel.zeroCrossDetectionTick(event.timeToNext); }, _channelControlStrategy[event.channel]); @@ -166,6 +161,13 @@ void PhaseModulationOrchestrator::event_zeroCrossDetection() { } void PhaseModulationOrchestrator::event_ctrlMessageArrived() { + constexpr auto powerControlAlgorithmHandler = std::make_tuple( + ctrl_IngressMessage_power_control_algorithm_request_tag, + ctrl_EgressMessage_power_control_algorithm_response_tag, + &PhaseModulationOrchestrator::handler_powerControlAlgorithmRequest, + ctrl_PowerControlAlgorithmResponse ctrl_PowerControlAlgorithmResponse_init_zero + ); + constexpr auto modeOfOperationRequestHandler = std::make_tuple( ctrl_IngressMessage_mode_of_operation_request_tag, ctrl_EgressMessage_mode_of_operation_response_tag, @@ -202,6 +204,8 @@ void PhaseModulationOrchestrator::event_ctrlMessageArrived() { ); auto genericHandler = [&](const auto &request, auto &response, auto handler) { + response.request_id = request.request_id; + auto fn = std::get<2>(handler); response.which_data = std::get<1>(handler); @@ -233,6 +237,10 @@ void PhaseModulationOrchestrator::event_ctrlMessageArrived() { resp.has_request_id = true; switch (req.which_data) { + case std::get<1>(powerControlAlgorithmHandler): + genericHandler(req, resp, powerControlAlgorithmHandler); + break; + case std::get<1>(modeOfOperationRequestHandler): genericHandler(req, resp, modeOfOperationRequestHandler); break; @@ -258,6 +266,19 @@ void PhaseModulationOrchestrator::event_ctrlMessageArrived() { }); } +void PhaseModulationOrchestrator::handler_powerControlAlgorithmRequest( + const PowerControlAlgorithmRequest &request, + PowerControlAlgorithmResponse &resp +) { + ULOG_INFO("PowerControlAlgorithm request handler"); + checkChannel(request.channel_id); + if (request.has_alg) { + setPowerControlAlgorithm(request.channel_id, request.alg); + } + + resp.alg = powerControlAlgorithm(request.channel_id); +} + void PhaseModulationOrchestrator::handler_modeOfOperationRequest( // const ModeOfOperationRequest &request, ModeOfOperationResponse &resp @@ -269,7 +290,7 @@ void PhaseModulationOrchestrator::handler_modeOfOperationRequest( // setMode(request.channel_id, request.mode); } - resp.mode = getMode(request.mode); + resp.mode = mode(request.mode); } void PhaseModulationOrchestrator::handler_groupModulationConfigRequest( // @@ -278,16 +299,16 @@ void PhaseModulationOrchestrator::handler_groupModulationConfigRequest( // ) { ULOG_INFO("GroupModulationConfigRequest request handler"); - checkChannel(request.channel_id); - checkCurrentMode(request.channel_id, ctrl_ModeOfOperation_GroupModulation); + // checkChannel(request.channel_id); + // checkCurrentMode(request.channel_id, ctrl_ModeOfOperation_GroupModulation); - auto &alg = std::get(_channel[request.channel_id]); + // auto &alg = std::get(_channel[request.channel_id]); - if (request.has_cycles_max) { - alg.setCyclesMax(request.cycles_max); - } + // if (request.has_cycles_max) { + // alg.setCyclesMax(request.cycles_max); + // } - resp.cycles_max = alg.getCyclesMax(); + // resp.cycles_max = alg.getCyclesMax(); } void PhaseModulationOrchestrator::handler_groupModulationControlRequest( // @@ -329,8 +350,8 @@ void PhaseModulationOrchestrator::checkChannel(uint8_t channel_id) { if (channel_id >= MaxChannels) throw ctrl::wrong_channel{}; } -void PhaseModulationOrchestrator::checkCurrentMode(const uint8_t channel, ModeOfOperation mode) { - if (getMode(channel) != mode) throw ctrl::wrong_mode{}; +void PhaseModulationOrchestrator::checkCurrentMode(const uint8_t channel, ModeOfOperation moo) { + if (mode(channel) != moo) throw ctrl::wrong_mode{}; } /// TODO set power control @@ -351,27 +372,55 @@ void PhaseModulationOrchestrator::setMode(uint8_t ch, ModeOfOperation mode) { throw ctrl::unsuported_mode{}; case ctrl_ModeOfOperation_AutoTune: throw ctrl::unsuported_mode{}; - default: throw ctrl::wrong_mode{}; break; } } -ModeOfOperation PhaseModulationOrchestrator::getMode(uint8_t ch) { - // if (std::holds_alternative(_channel.at(ch))) { - // return ctrl_ModeOfOperation_Disabled; - // } - // if (std::holds_alternative(_channel.at(ch))) { - // return ctrl_ModeOfOperation_PhaseModulation; - // } - // if (std::holds_alternative(_channel.at(ch))) { - // return ctrl_ModeOfOperation_GroupModulation; - // } +ModeOfOperation PhaseModulationOrchestrator::mode(uint8_t ch) { + if (std::holds_alternative(_chModeOfOperationStorage.at(ch))) { + return ctrl_ModeOfOperation_Disabled; + } + if (std::holds_alternative(_chModeOfOperationStorage.at(ch))) { + return ctrl_ModeOfOperation_ManualPower; + } + return ctrl_ModeOfOperation_Disabled; } +void PhaseModulationOrchestrator::setPowerControlAlgorithm(uint8_t ch, PowerControlAlgorithm alg) { + checkChannel(ch); // throws on bad channel + + switch (alg) { + case ctrl_PowerControlAlgorithm_NoModulation: + _channelControlStrategy[ch].emplace(); + break; + case ctrl_PowerControlAlgorithm_GroupModulation: + _channelControlStrategy[ch].emplace(pins[ch]); + break; + case ctrl_PowerControlAlgorithm_PhaseModulation: + _channelControlStrategy[ch].emplace(pins[ch]); + break; + default: + break; + } +} + +PowerControlAlgorithm PhaseModulationOrchestrator::powerControlAlgorithm(uint8_t ch) { + checkChannel(ch); + + if (std::holds_alternative(_channelControlStrategy.at(ch))) { + return ctrl_PowerControlAlgorithm_GroupModulation; + } + if (std::holds_alternative(_channelControlStrategy.at(ch))) { + return ctrl_PowerControlAlgorithm_PhaseModulation; + } + + return ctrl_PowerControlAlgorithm_NoModulation; +} + int PhaseModulationThread::do_hardwarenInit() { auto configure = [](const auto &dt_spec) { zephyr::gpio::pin_configure(dt_spec, GPIO_OUTPUT_INACTIVE); }; std::for_each(pins.begin(), pins.end(), configure); diff --git a/rims_app/src/power_control.hpp b/rims_app/src/power_control.hpp index 1c63627de09..1c6287bf89f 100644 --- a/rims_app/src/power_control.hpp +++ b/rims_app/src/power_control.hpp @@ -8,8 +8,8 @@ #include #include -#include #include + #include namespace rims { @@ -17,10 +17,13 @@ namespace rims { extern zephyr_fifo_buffer ctrlIngressQueue; extern zephyr_fifo_buffer ctrlEgressQueue; -using ModeOfOperation = ctrl_ModeOfOperation; +using ModeOfOperation = ctrl_ModeOfOperation; +using PowerControlAlgorithm = ctrl_PowerControlAlgorithm; -using ModeOfOperationRequest = ctrl_ModeOfOperationRequest; -using ModeOfOperationResponse = ctrl_ModeOfOperationResponse; +using PowerControlAlgorithmRequest = ctrl_PowerControlAlgorithmRequest; +using PowerControlAlgorithmResponse = ctrl_PowerControlAlgorithmResponse; +using ModeOfOperationRequest = ctrl_ModeOfOperationRequest; +using ModeOfOperationResponse = ctrl_ModeOfOperationResponse; using GroupModulationConfigRequest = ctrl_GroupModulationConfigRequest; using GroupModulationConfigResponse = ctrl_GroupModulationConfigResponse; @@ -89,8 +92,10 @@ class GroupModulation : public PinControlStrategyBase { GroupModulation(const gpio_dt_spec &gpio); ~GroupModulation() = default; - - void zeroCrossDetectionTick(rims::microseconds_u16_t usToNext) override; + + void zeroCrossDetectionTick(rims::microseconds_u16_t usToNext) override{ + + } void setCyclesMax(uint32_t cycles) { _cyclesMax = cycles; @@ -252,6 +257,7 @@ class PhaseModulationOrchestrator { void event_zeroCrossDetection(); void event_ctrlMessageArrived(); + void handler_powerControlAlgorithmRequest(const PowerControlAlgorithmRequest &request, PowerControlAlgorithmResponse &resp); void handler_modeOfOperationRequest(const ModeOfOperationRequest &request, ModeOfOperationResponse &resp); void handler_groupModulationConfigRequest(const GroupModulationConfigRequest &request, GroupModulationConfigResponse &resp); @@ -261,7 +267,10 @@ class PhaseModulationOrchestrator { void handler_phaseModulationControlRequest(const PhaseModulationControlRequest &request, PhaseModulationControlResponse &resp); void setMode(uint8_t ch, ModeOfOperation mode); - ModeOfOperation getMode(uint8_t ch); + ModeOfOperation mode(uint8_t ch); + + void setPowerControlAlgorithm(uint8_t ch, PowerControlAlgorithm alg); + PowerControlAlgorithm powerControlAlgorithm(uint8_t ch); bool setup(); int gpio_init(uint_fast8_t channel); -- 2.45.2 From d8ad9a7a394d9a1800c39c87538dcba79d56474a Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Mon, 5 May 2025 20:14:47 +0200 Subject: [PATCH 14/18] add GPIO control messages --- boards/bartoszek/rims/rims_h503cbt_defconfig | 3 +- rims_app/proto/ctrl.proto | 47 +++++- rims_app/proto/gpio.proto | 46 ++++++ rims_app/src/circular_buffer.hpp | 4 +- rims_app/src/common.hpp | 46 +++++- rims_app/src/gpio.cpp | 145 +++++++++++++++++++ rims_app/src/gpio.hpp | 41 ++++++ rims_app/src/log.cpp | 4 +- rims_app/src/log.hpp | 2 +- rims_app/src/main.cpp | 26 ++-- rims_app/src/messenger.cpp | 72 ++++++--- rims_app/src/messenger.hpp | 4 +- rims_app/src/power_control.cpp | 129 +++++++++-------- rims_app/src/power_control.hpp | 56 +++++-- rims_app/src/zephyr.hpp | 4 + rims_app/src/zero_cross_detection.cpp | 24 ++- rims_app/src/zero_cross_detection.hpp | 15 +- 17 files changed, 527 insertions(+), 141 deletions(-) create mode 100644 rims_app/proto/gpio.proto create mode 100644 rims_app/src/gpio.cpp create mode 100644 rims_app/src/gpio.hpp diff --git a/boards/bartoszek/rims/rims_h503cbt_defconfig b/boards/bartoszek/rims/rims_h503cbt_defconfig index 13a555f929c..bdca13c023b 100644 --- a/boards/bartoszek/rims/rims_h503cbt_defconfig +++ b/boards/bartoszek/rims/rims_h503cbt_defconfig @@ -18,8 +18,7 @@ CONFIG_CLOCK_CONTROL=y CONFIG_PINCTRL=y # enable CAN controller -#CONFIG_CAN=y -#CONFIG_CAN_FD_MODE=y +CONFIG_CAN=n # Enable MPU CONFIG_ARM_MPU=y diff --git a/rims_app/proto/ctrl.proto b/rims_app/proto/ctrl.proto index 09324454448..51099dcff86 100644 --- a/rims_app/proto/ctrl.proto +++ b/rims_app/proto/ctrl.proto @@ -12,6 +12,8 @@ enum Error { WrongChannel = 3; + PowerLevelFailedToSet = 4; + UnknownError = 32; } @@ -86,6 +88,31 @@ message ModeOfOperationResponse optional Error error = 254; } +// Request message to get or set the power level as a percentage (0.0–100.0). +// Applicable only when the mode of operation is set to ManualPower. +message PowerLevelRequest +{ + // ID of the target channel + uint32 channel_id = 1; + + // Optional power level percentage (0.0–100.0). + // If omitted, the current level will be queried (read operation). + optional float level = 2; +} + +// Response message for PowerLevelRequest. +message PowerLevelResponse +{ + // ID of the channel the response relates to + uint32 channel_id = 1; + + // The current or newly set power level percentage (0.0–100.0) + float level = 2; + + // Optional error field if the request could not be fulfilled + optional Error error = 3; +} + // Request message for configuring Group Modulation parameters. message GroupModulationConfigRequest { @@ -195,11 +222,13 @@ message IngressMessage PowerControlAlgorithmRequest power_control_algorithm_request = 1; ModeOfOperationRequest mode_of_operation_request = 2; - GroupModulationConfigRequest group_modulation_config_request = 3; - GroupModulationControlRequest group_modulation_control_request = 4; + PowerLevelRequest power_level_request = 3; - PhaseModulationConfigRequest phase_modulation_config_request = 5; - PhaseModulationControlRequest phase_modulation_control_request = 6; + GroupModulationConfigRequest group_modulation_config_request = 10; + GroupModulationControlRequest group_modulation_control_request = 11; + + PhaseModulationConfigRequest phase_modulation_config_request = 12; + PhaseModulationControlRequest phase_modulation_control_request = 13; } }; @@ -211,11 +240,13 @@ message EgressMessage PowerControlAlgorithmResponse power_control_algorithm_response = 1; ModeOfOperationResponse mode_of_operation_response = 2; - GroupModulationConfigResponse group_modulation_config_response = 3; - GroupModulationControlResponse group_modulation_control_response = 4; + PowerLevelResponse power_level_response = 3; - PhaseModulationConfigResponse phase_modulation_config_response = 5; - PhaseModulationControlResponse phase_modulation_control_response = 6; + GroupModulationConfigResponse group_modulation_config_response = 10; + GroupModulationControlResponse group_modulation_control_response = 11; + + PhaseModulationConfigResponse phase_modulation_config_response = 12; + PhaseModulationControlResponse phase_modulation_control_response = 13; // BROADCAST } }; diff --git a/rims_app/proto/gpio.proto b/rims_app/proto/gpio.proto new file mode 100644 index 00000000000..7bff9f9290b --- /dev/null +++ b/rims_app/proto/gpio.proto @@ -0,0 +1,46 @@ +syntax = "proto3"; + +package gpio; + +enum Error { + NoError = 0; + BadId = 1; + Other = 2; +} + +enum GPIO { + GPIO_pin_1 = 0; + GPIO_pin_2 = 1; +}; + +message GpioRequest +{ + GPIO gpio = 1; + optional bool state = 2; +}; + +message GpioResponse +{ + GPIO gpio = 1; + bool state = 2; + optional Error error = 254; +}; + +// only those messages are send through wire at temperature endpoint +message IngressMessage +{ + uint32 request_id = 255; + oneof data + { + GpioRequest gpio_request = 1; + } +}; + +message EgressMessage +{ + optional uint32 request_id = 255; + oneof data + { + GpioResponse gpio_response = 1; + } +}; diff --git a/rims_app/src/circular_buffer.hpp b/rims_app/src/circular_buffer.hpp index 324d2a621c7..0fb8f98a09d 100644 --- a/rims_app/src/circular_buffer.hpp +++ b/rims_app/src/circular_buffer.hpp @@ -513,6 +513,7 @@ template struct ZephyrFifoElement { template class zephyr_fifo_buffer { public: + // static_assert(std::is_trivial_v); zephyr_fifo_buffer(k_fifo &fifo) : _fifo{fifo} { } @@ -550,9 +551,10 @@ template class zephyr_fifo_buffer { { auto &el = _elements.push_back(std::move(tmp)); k_fifo_put(&_fifo, &el); // put data into a queue + return true; } - return true; + return false; } private: diff --git a/rims_app/src/common.hpp b/rims_app/src/common.hpp index 597c26e4fbd..d2e05b7514e 100644 --- a/rims_app/src/common.hpp +++ b/rims_app/src/common.hpp @@ -23,9 +23,35 @@ namespace rims { +// Primary template (for general case) +template struct NthArgument; + +// Specialization for normal functions +template struct NthArgument { + using type = std::tuple_element_t>; +}; + +// Specialization for member functions (non-const) +template struct NthArgument { + using type = std::tuple_element_t>; +}; + +// Specialization for member functions (const) +template struct NthArgument { + using type = std::tuple_element_t>; +}; + +// Helper alias template +template using NthArgument_t = typename NthArgument::type; + using clock = std::chrono::steady_clock; using time_point = clock::time_point; +using microseconds_u16_t = std::chrono::duration; // max ~65ms +using microseconds_u32_t = std::chrono::duration; // max ~4294s, ~71 min + +using adc_sample = uint16_t; + constexpr bool operator==(const k_timeout_t &lhs, const k_timeout_t &rhs) { return lhs.ticks == rhs.ticks; } @@ -150,6 +176,20 @@ class SingleShootTimer : public Timer { void expiry_cb() override { _cb(); } + + void fire_at(clock::time_point target) { + auto now = clock::now(); + if (target <= now) { + // Fire immediately (e.g., 1 microsecond delay to ensure expiry_cb is called) + _duration = chronoToKTimeout(std::chrono::microseconds{1}); + } else { + auto delay = std::chrono::duration_cast(target - now); + _duration = chronoToKTimeout(delay); + } + + _period = K_NO_WAIT; // Ensure it's a one-shot + start(); + } std::function _cb; }; @@ -257,12 +297,6 @@ constexpr auto ChannelNumber = 2; constexpr int GPIO_PIN_PB5 = 5; constexpr int GPIO_PIN_PB6 = 6; -using clock = std::chrono::steady_clock; -using time_point = clock::time_point; -using microseconds_u16_t = std::chrono::duration; // max ~65ms -using microseconds_u32_t = std::chrono::duration; // max ~4294s, ~71 min - -using adc_sample = uint16_t; } // namespace rims diff --git a/rims_app/src/gpio.cpp b/rims_app/src/gpio.cpp new file mode 100644 index 00000000000..9e31c9e5c14 --- /dev/null +++ b/rims_app/src/gpio.cpp @@ -0,0 +1,145 @@ +#include "gpio.hpp" +#include "proto/gpio.pb.h" +#include "zephyr.hpp" +#include + +namespace rims { + +namespace gpio { +struct error : public std::exception { + gpio_Error _err; + error(gpio_Error err) : _err{err} { + } +}; +struct bad_id : public error { + bad_id() : error{gpio_Error_BadId} { + } +}; +} // namespace gpio + +K_FIFO_DEFINE(gpioIngress); +K_FIFO_DEFINE(gpioEggress); + +zephyr_fifo_buffer gpioIngressQueue{gpioIngress}; +zephyr_fifo_buffer gpioEgressQueue{gpioEggress}; + +constexpr int channels = 2; + +constexpr std::array pins = { + gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(gprelay_1_en), gpios), + gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(gprelay_2_en), gpios) +}; + +GPIO::GPIO() { + gpioIngressQueue.k_event_init(_events.at(0)); +} +// thread main +void GPIO::loop() { + while (1) { + try { + auto ret = zephyr::event_pool::k_poll_forever(_events); + if (ret == 0) { + zephyr::event_pool::k_poll_handle(_events[0], [&]() { event_gpioMessageArrived(); }); + } + } catch (const std::exception &e) { + ULOG_ERROR("EXCEPTION %s", e.what()); + } + } +} + +void GPIO::handle_gpioRequest(const GpioRequest &request, GpioResponse &response) { + ULOG_INFO("GpioRequest request handler"); + // if(request.gpio) // todo check channel + + response.gpio = request.gpio; + + if (request.has_state) { + switch (request.gpio) { + case gpio_GPIO_GPIO_pin_1: + zephyr::gpio::pin_set_dt(pins[0], request.state); + break; + case gpio_GPIO_GPIO_pin_2: + zephyr::gpio::pin_set_dt(pins[1], request.state); + break; + default: + throw gpio::bad_id{}; + } + } + switch (request.gpio) { + case gpio_GPIO_GPIO_pin_1: + response.state = zephyr::gpio::pin_get_state(pins[0]); + break; + case gpio_GPIO_GPIO_pin_2: + response.state = zephyr::gpio::pin_get_state(pins[1]); + break; + default: + throw gpio::bad_id{}; + } +} + +void GPIO::event_gpioMessageArrived() { + constexpr auto gpioStateHandler = std::make_tuple( + gpio_IngressMessage_gpio_request_tag, + gpio_EgressMessage_gpio_response_tag, + &GPIO::handle_gpioRequest, + gpio_GpioResponse gpio_GpioResponse_init_zero + ); + + auto genericHandler = [&](const auto &request, auto &response, auto handler) { + response.request_id = request.request_id; + + auto fn = std::get<2>(handler); + response.which_data = std::get<1>(handler); + + // cast request and response union to needed type + const auto &req = reinterpret_cast>(request.data); + auto &resp = reinterpret_cast>(response.data); + + try { + // invoke message handler + std::invoke(fn, this, req, resp); + } catch (const gpio::error &e) { + // clear message + resp = std::get<3>(handler); + // set error on exception + resp.has_error = true; + resp.error = e._err; + } catch (const std::exception &e) { + resp = std::get<3>(handler); + resp.has_error = true; + resp.error = gpio_Error_Other; + } + }; + + gpioIngressQueue.try_consume([&](const gpio_IngressMessage &req) { + ULOG_INFO("gpio request message handler"); + gpioEgressQueue.try_produce([&](gpio_EgressMessage &resp) { + ULOG_INFO("gpio response message handler"); + resp.request_id = req.request_id; + resp.has_request_id = true; + + switch (req.which_data) { + case std::get<1>(gpioStateHandler): + genericHandler(req, resp, gpioStateHandler); + return true; + } + + return false; + }); + }); +} + +void GpioThread::threadMain() { + ULOG_INFO("GPIOThread start"); + /// runs in context of new thread, on new thread stack etc. + GPIO thread{}; + thread.loop(); +} + +int GpioThread::do_hardwarenInit() { + auto configure = [](const auto &dt_spec) { zephyr::gpio::pin_configure(dt_spec, GPIO_OUTPUT_INACTIVE); }; + std::for_each(pins.begin(), pins.end(), configure); + return 0; +} + +} // namespace rims diff --git a/rims_app/src/gpio.hpp b/rims_app/src/gpio.hpp new file mode 100644 index 00000000000..e09f3f41918 --- /dev/null +++ b/rims_app/src/gpio.hpp @@ -0,0 +1,41 @@ +#include "common.hpp" + +#include "circular_buffer.hpp" + +#include "proto/gpio.pb.h" +#include "zephyr.hpp" +#include "zephyr/kernel.h" +#include + +namespace rims { + +using GpioRequest = gpio_GpioRequest; +using GpioResponse = gpio_GpioResponse; + +extern zephyr_fifo_buffer gpioIngressQueue; +extern zephyr_fifo_buffer gpioEgressQueue; + +class GPIO { + public: + GPIO(); + void loop(); + + private: + void handle_gpioRequest(const GpioRequest &req, GpioResponse &resp); + + void event_gpioMessageArrived(); + + std::array _events; +}; + +class GpioThread : public ZephyrThread { + public: + GpioThread(TStackBase &stack) : ZephyrThread(stack, 5, 0, "Gpio"){}; + + void threadMain() override; + + protected: + int do_hardwarenInit() override; +}; + +} // namespace rims diff --git a/rims_app/src/log.cpp b/rims_app/src/log.cpp index c40c6f601a2..5e86ea189da 100644 --- a/rims_app/src/log.cpp +++ b/rims_app/src/log.cpp @@ -6,7 +6,7 @@ namespace rims { K_FIFO_DEFINE(klogFifoQueue); -zephyr_fifo_buffer logEgressFifoQueueBuffer{klogFifoQueue}; +zephyr_fifo_buffer logEgressQueue{klogFifoQueue}; static std::size_t g_droppedLogs{0}; @@ -66,7 +66,7 @@ void Log::formatLog(Level level, const char *fmt, va_list args) const { // handle case when queue if full and we are waitnig for free space bool ok{false}; do { - ok = logEgressFifoQueueBuffer.try_produce([&](log_EgressMessage &logmsg) { + ok = logEgressQueue.try_produce([&](log_EgressMessage &logmsg) { logmsg = log_EgressMessage_init_zero; logmsg.has_request_id = false; diff --git a/rims_app/src/log.hpp b/rims_app/src/log.hpp index 51dba9f153a..bc45f840bbb 100644 --- a/rims_app/src/log.hpp +++ b/rims_app/src/log.hpp @@ -20,7 +20,7 @@ enum class LogLevel : int { // }; extern zephyr_fifo_buffer logIngressFifoQueueBuffer; -extern zephyr_fifo_buffer logEgressFifoQueueBuffer; +extern zephyr_fifo_buffer logEgressQueue; /// TODO move static LogLevel g_logLevel = LogLevel::Error; diff --git a/rims_app/src/main.cpp b/rims_app/src/main.cpp index 4999ca5e3c3..e492e5a49ce 100644 --- a/rims_app/src/main.cpp +++ b/rims_app/src/main.cpp @@ -1,9 +1,10 @@ #include "common.hpp" +#include "gpio.hpp" #include "log.hpp" #include "messenger.hpp" -#include "power_control.hpp" #include "placement_unique_ptr.hpp" +#include "power_control.hpp" #include "temperature_measurements.hpp" #include "uart.hpp" #include "zephyr.hpp" @@ -38,6 +39,9 @@ TStack zeroCrossDetectionStack{k_zeroCrossDetectionStack, K_THREAD_STACK_SIZEOF( static K_THREAD_STACK_DEFINE(k_phaseModulationStack, 2048); TStack phaseModulationStack{k_phaseModulationStack, K_THREAD_STACK_SIZEOF(k_phaseModulationStack)}; +static K_THREAD_STACK_DEFINE(k_gpioStack, 1024); +TStack gpioStack{k_gpioStack, K_THREAD_STACK_SIZEOF(k_gpioStack)}; + /// TOP LEVEL TODOS ;) /// TODO power control implementation /// TODO power measurement thread @@ -64,12 +68,11 @@ LazyInit uartThread; LazyInit temperatureSampler; LazyInit zcd; LazyInit phaseModulation; +LazyInit gpio; -gpio_dt_spec status = GPIO_DT_SPEC_GET(DT_NODELABEL(status_led), gpios); -gpio_dt_spec gprelay_1_en = GPIO_DT_SPEC_GET(DT_NODELABEL(gprelay_1_en), gpios); -gpio_dt_spec gprelay_2_en = GPIO_DT_SPEC_GET(DT_NODELABEL(gprelay_2_en), gpios); // pompka -gpio_dt_spec ch_1_zcd = GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_zcd), gpios); -gpio_dt_spec ch_2_zcd = GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_zcd), gpios); // pompka +gpio_dt_spec status = GPIO_DT_SPEC_GET(DT_NODELABEL(status_led), gpios); +gpio_dt_spec ch_1_zcd = GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_zcd), gpios); +gpio_dt_spec ch_2_zcd = GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_zcd), gpios); void mythread_analyzer_cb(struct thread_analyzer_info *info) { // ULOG_INFO( @@ -88,26 +91,31 @@ int main() { temperatureSampler.init(temperatureSamplerStack); zcd.init(zeroCrossDetectionStack); phaseModulation.init(phaseModulationStack); + gpio.init(gpioStack); messengerTread->init_hw(); messengerTread->start(); + uartThread->init_hw(); uartThread->start(); + temperatureSampler->init_hw(); temperatureSampler->start(); + zcd->init_hw(); zcd->start(); + phaseModulation->init_hw(); phaseModulation->start(); + gpio->init_hw(); + gpio->start(); + zephyr::gpio::pin_configure(status, GPIO_OUTPUT_INACTIVE); - zephyr::gpio::pin_configure(gprelay_1_en, GPIO_OUTPUT_INACTIVE); - zephyr::gpio::pin_configure(gprelay_2_en, GPIO_OUTPUT_INACTIVE); // zephyr::gpio::pin_toggle_dt(gprelay_2_en); while (1) { std::this_thread::sleep_for(std::chrono::seconds{2}); - thread_analyzer_run(mythread_analyzer_cb, 0); // ULOG_INFO("PING"); // const unsigned char data[] = {'b', 0x55}; diff --git a/rims_app/src/messenger.cpp b/rims_app/src/messenger.cpp index 1a7765d7768..e202a9e60df 100644 --- a/rims_app/src/messenger.cpp +++ b/rims_app/src/messenger.cpp @@ -4,6 +4,7 @@ #include "log.hpp" #include "config.hpp" +#include "gpio.hpp" #include "power_control.hpp" #include "temperature_measurements.hpp" #include "uart.hpp" @@ -11,8 +12,10 @@ #include "pb.h" #include "pb_decode.h" #include "pb_encode.h" + #include "proto/configuration.pb.h" #include "proto/ctrl.pb.h" +#include "proto/gpio.pb.h" #include "proto/log.pb.h" #include "proto/message.pb.h" #include "proto/temperature.pb.h" @@ -32,7 +35,6 @@ #include "cobs.h" #include "zephyr.hpp" -#include "zephyr/sys/__assert.h" namespace rims { K_MSGQ_DEFINE(messenger_buffer_arrived_queue, sizeof(rims::buffer), 2, 1); @@ -196,7 +198,8 @@ void MessengerThread::threadMain() { temperatureEgressQueue.k_event_init(_events.at(1)); ctrlEgressQueue.k_event_init(_events.at(2)); configEgressQueue.k_event_init(_events.at(3)); - logEgressFifoQueueBuffer.k_event_init(_events.at(4)); + logEgressQueue.k_event_init(_events.at(4)); + gpioEgressQueue.k_event_init(_events.at(5)); while (1) { // TODO add try catch for incoming data errors try { @@ -207,6 +210,7 @@ void MessengerThread::threadMain() { zephyr::event_pool::k_poll_handle(_events[2], std::bind(&MessengerThread::event_ctrlEgress, this)); zephyr::event_pool::k_poll_handle(_events[3], std::bind(&MessengerThread::event_configEgress, this)); zephyr::event_pool::k_poll_handle(_events[4], std::bind(&MessengerThread::event_logEgress, this)); + zephyr::event_pool::k_poll_handle(_events[5], std::bind(&MessengerThread::event_gpioEgress, this)); } } catch (const std::exception &e) { /// TODO @@ -302,6 +306,10 @@ void MessengerThread::event_dataArrived() { // logIngressFifoQueueBuffer.try_produce() break; } + case 0x05: { + handle_gpioIngressMsg(id, stream, buf.device); + break; + } default: { AsyncUART::transmit(buf.device, buf.data); break; @@ -355,36 +363,56 @@ void MessengerThread::event_configEgress() { } void MessengerThread::event_logEgress() { - logEgressFifoQueueBuffer.try_consume([&](log_EgressMessage &out) { + logEgressQueue.try_consume([&](log_EgressMessage &out) { egressPush(&out, log_EgressMessage_msg, 4, out.has_request_id ? std::optional{out.request_id} : std::nullopt); }); } -void MessengerThread::handle_temperatureIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb) { - temperatureIngressQueue.try_produce([&](temperature_IngressMessage &request) { - request = temperature_IngressMessage_init_zero; - decode(stream, temperature_IngressMessage_msg, &request); - putActiveRequest(id, request.request_id, cb); - return true; +void MessengerThread::event_gpioEgress() { + gpioEgressQueue.try_consume([&](gpio_EgressMessage &out) { + egressPush(&out, gpio_EgressMessage_msg, 5, out.has_request_id ? std::optional{out.request_id} : std::nullopt); }); } +void MessengerThread::handle_temperatureIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb) { + if (not temperatureIngressQueue.try_produce([&](temperature_IngressMessage &request) { + request = temperature_IngressMessage_init_zero; + decode(stream, temperature_IngressMessage_msg, &request); + putActiveRequest(id, request.request_id, cb); + return true; + })) + messenger::request_queue_full{}; + ; +} + void MessengerThread::handle_ctrlIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb) { - ctrlIngressQueue.try_produce([&](ctrl_IngressMessage &request) { - request = ctrl_IngressMessage_init_zero; - decode(stream, ctrl_IngressMessage_msg, &request); - putActiveRequest(id, request.request_id, cb); - return true; - }); + if (not ctrlIngressQueue.try_produce([&](ctrl_IngressMessage &request) { + request = ctrl_IngressMessage_init_zero; + decode(stream, ctrl_IngressMessage_msg, &request); + putActiveRequest(id, request.request_id, cb); + return true; + })) + messenger::request_queue_full{}; } void MessengerThread::handle_configIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb) { - configIngressQueue.try_produce([&](config_IngressMessage &request) { - request = config_IngressMessage_init_zero; - decode(stream, config_IngressMessage_msg, &request); - putActiveRequest(id, request.request_id, cb); - return true; - }); + if (not configIngressQueue.try_produce([&](config_IngressMessage &request) { + request = config_IngressMessage_init_zero; + decode(stream, config_IngressMessage_msg, &request); + putActiveRequest(id, request.request_id, cb); + return true; + })) + throw messenger::request_queue_full{}; +} + +void MessengerThread::handle_gpioIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb) { + if (not gpioIngressQueue.try_produce([&](gpio_IngressMessage &request) { + request = gpio_IngressMessage_init_zero; + decode(stream, gpio_IngressMessage_msg, &request); + putActiveRequest(id, request.request_id, cb); + return true; + })) + messenger::request_queue_full{}; } void MessengerThread::egressPush(Message &message, int wireId_receiver, std::optional requestId) { @@ -434,6 +462,8 @@ bool MessengerThread::transmit(Message &msg, AsyncUART *cb) { static_assert(TEMPERATURE_PROTO_TEMPERATURE_PB_H_MAX_SIZE <= sizeof(Message::data.bytes)); static_assert(CTRL_PROTO_CTRL_PB_H_MAX_SIZE <= sizeof(Message::data.bytes)); static_assert(CONFIG_PROTO_CONFIGURATION_PB_H_MAX_SIZE <= sizeof(Message::data.bytes)); + static_assert(GPIO_PROTO_GPIO_PB_H_MAX_SIZE <= sizeof(Message::data.bytes)); + pb_byte_t outBuf[PROTO_MESSAGE_PB_H_MAX_SIZE + 8] = {}; auto outputStream = std::span{outBuf + 8, sizeof(outBuf) - 8}; diff --git a/rims_app/src/messenger.hpp b/rims_app/src/messenger.hpp index 538191a048e..2a7f4c5be51 100644 --- a/rims_app/src/messenger.hpp +++ b/rims_app/src/messenger.hpp @@ -42,10 +42,12 @@ class MessengerThread : public ZephyrThread { void event_ctrlEgress(); void event_configEgress(); void event_logEgress(); + void event_gpioEgress(); void handle_temperatureIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb); void handle_ctrlIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb); void handle_configIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb); + void handle_gpioIngressMsg(WireFormatID id, pb_istream_t &stream, AsyncUART *cb); void decode(pb_istream_t &stream, const pb_msgdesc_t &fields, void *dest) const; bool transmit(Message &msg, AsyncUART *cb); @@ -56,7 +58,7 @@ class MessengerThread : public ZephyrThread { void putActiveRequest(WireFormatID wireId, uint32_t id, AsyncUART *cb); std::optional takeActiveRequest(int type, uint32_t id); - std::array _events; + std::array _events; std::array _activeRequests; uint8_t _activeRequestsNr; }; diff --git a/rims_app/src/power_control.cpp b/rims_app/src/power_control.cpp index 72faab48f69..ad2e2dadc2f 100644 --- a/rims_app/src/power_control.cpp +++ b/rims_app/src/power_control.cpp @@ -12,28 +12,9 @@ #include #include #include +#include #include -// Primary template (for general case) -template struct NthArgument; - -// Specialization for normal functions -template struct NthArgument { - using type = std::tuple_element_t>; -}; - -// Specialization for member functions (non-const) -template struct NthArgument { - using type = std::tuple_element_t>; -}; - -// Specialization for member functions (const) -template struct NthArgument { - using type = std::tuple_element_t>; -}; - -// Helper alias template -template using NthArgument_t = typename NthArgument::type; namespace rims { @@ -57,6 +38,11 @@ struct unsuported_mode : public error { unsuported_mode() : error{ctrl_Error_UnsupportedMode} {}; }; +struct failed_to_set_power_level : public error { + failed_to_set_power_level() : error{ctrl_Error_PowerLevelFailedToSet} { + } +}; + } // namespace ctrl K_FIFO_DEFINE(ctrlIngress); @@ -85,8 +71,9 @@ static constexpr microseconds_u16_t delay(float percent, microseconds_u16_t full return full - tous(percent, full); } -void PhaseModulation::zeroCrossDetectionTick(rims::microseconds_u16_t usToNext) { +void PhaseModulation::zeroCrossDetectionTick(ZeroCrossDetectionEvent::Data data) { // Disable triac immediately on zero detect to fix flickering at low power + // zero detect event comes ~500us before true ZCD this->off(); constexpr auto minimalGatePulse = microseconds_u16_t{75}; @@ -96,20 +83,29 @@ void PhaseModulation::zeroCrossDetectionTick(rims::microseconds_u16_t usToNext) // 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.setDuration(onDelay); + // _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.start(); - // Schedule triac to turn OFF shortly after firing (e.g., 100 us pulse) - _triacTimerStop.setDuration(onDelay + minimalGatePulse); + // // Schedule triac to turn OFF shortly after firing (e.g., 100 us pulse) + _triacTimerStop.setDuration(onDelay + data.to_zcd + minimalGatePulse); _triacTimerStop.start(); } @@ -117,7 +113,7 @@ GroupModulation::GroupModulation(const gpio_dt_spec &gpio) : PinControlStrategyB ULOG_INFO("GroupModulation started for gpio %d", gpio.pin); } -PhaseModulationOrchestrator::PhaseModulationOrchestrator() : _channelControlStrategy{NoModulation{}, NoModulation{}} { +PhaseModulationOrchestrator::PhaseModulationOrchestrator() : _chPowerControlStrategy{NoModulation{}, NoModulation{}} { ZeroCrossDetectionEventQueue.k_event_init(_events[0]); ctrlIngressQueue.k_event_init(_events[1]); } @@ -137,26 +133,9 @@ void PhaseModulationOrchestrator::loop() { } } -// constexpr float PI = 3.141592f; -// constexpr float FREQUENCY = 0.25f; -// constexpr float MIN_VALUE = 0.0f; -// constexpr float MAX_VALUE = 25.0f; - -// float sinewave(std::chrono::steady_clock::time_point timePoint) { -// using namespace std::chrono; -// static auto startTime = steady_clock::now(); - -// float elapsedSeconds = duration(timePoint - startTime).count(); -// float sineValue = sinf(2.0f * PI * FREQUENCY * elapsedSeconds); - -// return MIN_VALUE + (sineValue + 1.0f) * 0.5f * (MAX_VALUE - MIN_VALUE); -// } - void PhaseModulationOrchestrator::event_zeroCrossDetection() { - // _powerPublisher.notifyPowerUpdate(sinewave(std::chrono::steady_clock::now()), 0); - ZeroCrossDetectionEventQueue.try_consume([&](ZeroCrossDetectionEvent &event) { - std::visit([&event](auto &channel) { channel.zeroCrossDetectionTick(event.timeToNext); }, _channelControlStrategy[event.channel]); + std::visit([&event](auto &channel) { channel.zeroCrossDetectionTick(event.data); }, _chModeOfOperationStorage[event.channel]); }); } @@ -166,8 +145,8 @@ void PhaseModulationOrchestrator::event_ctrlMessageArrived() { ctrl_EgressMessage_power_control_algorithm_response_tag, &PhaseModulationOrchestrator::handler_powerControlAlgorithmRequest, ctrl_PowerControlAlgorithmResponse ctrl_PowerControlAlgorithmResponse_init_zero - ); - + ); + constexpr auto modeOfOperationRequestHandler = std::make_tuple( ctrl_IngressMessage_mode_of_operation_request_tag, ctrl_EgressMessage_mode_of_operation_response_tag, @@ -175,6 +154,13 @@ void PhaseModulationOrchestrator::event_ctrlMessageArrived() { ctrl_ModeOfOperationResponse ctrl_ModeOfOperationResponse_init_zero ); + constexpr auto powerLevelRequestHandler = std::make_tuple( + ctrl_IngressMessage_power_level_request_tag, + ctrl_EgressMessage_power_level_response_tag, + &PhaseModulationOrchestrator::handler_powerLevelRequest, + ctrl_PowerLevelResponse ctrl_PowerLevelResponse_init_zero + ); + constexpr auto groupModulationConfigRequestHandler = std::make_tuple( ctrl_IngressMessage_group_modulation_config_request_tag, ctrl_EgressMessage_group_modulation_config_response_tag, @@ -205,12 +191,12 @@ void PhaseModulationOrchestrator::event_ctrlMessageArrived() { auto genericHandler = [&](const auto &request, auto &response, auto handler) { response.request_id = request.request_id; - + auto fn = std::get<2>(handler); response.which_data = std::get<1>(handler); // cast request and response union to needed type - const auto &req = reinterpret_cast>(response.data); + const auto &req = reinterpret_cast>(request.data); auto &resp = reinterpret_cast>(response.data); try { @@ -240,7 +226,10 @@ void PhaseModulationOrchestrator::event_ctrlMessageArrived() { case std::get<1>(powerControlAlgorithmHandler): genericHandler(req, resp, powerControlAlgorithmHandler); break; - + case std::get<1>(powerLevelRequestHandler): + genericHandler(req, resp, powerLevelRequestHandler); + break; + case std::get<1>(modeOfOperationRequestHandler): genericHandler(req, resp, modeOfOperationRequestHandler); break; @@ -290,7 +279,24 @@ void PhaseModulationOrchestrator::handler_modeOfOperationRequest( // setMode(request.channel_id, request.mode); } - resp.mode = mode(request.mode); + resp.mode = mode(request.channel_id); +} + +void PhaseModulationOrchestrator::handler_powerLevelRequest( // + const PowerLevelRequest &request, + PowerLevelResponse &resp +) { + ULOG_INFO("PowerLevelRequest request handler"); + checkChannel(request.channel_id); + + if (request.has_level) { + if (_powerPublisher.notifyPowerUpdate(request.channel_id, request.level) == 0) { + ULOG_ERROR("there are no subscribers for %d channel", request.channel_id); + throw ctrl::failed_to_set_power_level{}; + } + } + + resp.level = strategy(request.channel_id).power(); } void PhaseModulationOrchestrator::handler_groupModulationConfigRequest( // @@ -357,6 +363,8 @@ void PhaseModulationOrchestrator::checkCurrentMode(const uint8_t channel, ModeOf /// TODO set power control void PhaseModulationOrchestrator::setMode(uint8_t ch, ModeOfOperation mode) { + checkChannel(ch); + switch (mode) { case ctrl_ModeOfOperation_Disabled: _chModeOfOperationStorage[ch].emplace(); @@ -379,6 +387,8 @@ void PhaseModulationOrchestrator::setMode(uint8_t ch, ModeOfOperation mode) { } ModeOfOperation PhaseModulationOrchestrator::mode(uint8_t ch) { + checkChannel(ch); + if (std::holds_alternative(_chModeOfOperationStorage.at(ch))) { return ctrl_ModeOfOperation_Disabled; } @@ -386,22 +396,21 @@ ModeOfOperation PhaseModulationOrchestrator::mode(uint8_t ch) { return ctrl_ModeOfOperation_ManualPower; } - return ctrl_ModeOfOperation_Disabled; } void PhaseModulationOrchestrator::setPowerControlAlgorithm(uint8_t ch, PowerControlAlgorithm alg) { - checkChannel(ch); // throws on bad channel - + checkChannel(ch); + switch (alg) { case ctrl_PowerControlAlgorithm_NoModulation: - _channelControlStrategy[ch].emplace(); + _chPowerControlStrategy[ch].emplace(); break; case ctrl_PowerControlAlgorithm_GroupModulation: - _channelControlStrategy[ch].emplace(pins[ch]); + _chPowerControlStrategy[ch].emplace(pins[ch]); break; case ctrl_PowerControlAlgorithm_PhaseModulation: - _channelControlStrategy[ch].emplace(pins[ch]); + _chPowerControlStrategy[ch].emplace(pins[ch]); break; default: break; @@ -411,13 +420,13 @@ void PhaseModulationOrchestrator::setPowerControlAlgorithm(uint8_t ch, PowerCont PowerControlAlgorithm PhaseModulationOrchestrator::powerControlAlgorithm(uint8_t ch) { checkChannel(ch); - if (std::holds_alternative(_channelControlStrategy.at(ch))) { + if (std::holds_alternative(_chPowerControlStrategy.at(ch))) { return ctrl_PowerControlAlgorithm_GroupModulation; } - if (std::holds_alternative(_channelControlStrategy.at(ch))) { + if (std::holds_alternative(_chPowerControlStrategy.at(ch))) { return ctrl_PowerControlAlgorithm_PhaseModulation; } - + return ctrl_PowerControlAlgorithm_NoModulation; } diff --git a/rims_app/src/power_control.hpp b/rims_app/src/power_control.hpp index 1c6287bf89f..13297e1b673 100644 --- a/rims_app/src/power_control.hpp +++ b/rims_app/src/power_control.hpp @@ -5,6 +5,7 @@ #include "common.hpp" #include "inplace_vector.hpp" #include "proto/ctrl.pb.h" +#include "zero_cross_detection.hpp" #include #include @@ -25,6 +26,9 @@ using PowerControlAlgorithmResponse = ctrl_PowerControlAlgorithmResponse; using ModeOfOperationRequest = ctrl_ModeOfOperationRequest; using ModeOfOperationResponse = ctrl_ModeOfOperationResponse; +using PowerLevelRequest = ctrl_PowerLevelRequest; +using PowerLevelResponse = ctrl_PowerLevelResponse; + using GroupModulationConfigRequest = ctrl_GroupModulationConfigRequest; using GroupModulationConfigResponse = ctrl_GroupModulationConfigResponse; using GroupModulationControlRequest = ctrl_GroupModulationControlRequest; @@ -43,7 +47,7 @@ class PowerControlStrategy { virtual ~PowerControlStrategy() noexcept = default; // function called each AC signal crossed 0 // 100 times a second - virtual void zeroCrossDetectionTick(rims::microseconds_u16_t usToNext) = 0; + virtual void zeroCrossDetectionTick(ZeroCrossDetectionEvent::Data usToNext) = 0; virtual void setPower(float power) { _power = power; }; @@ -81,7 +85,7 @@ class PhaseModulation : public PinControlStrategyBase { public: PhaseModulation(const gpio_dt_spec &gpio); ~PhaseModulation() = default; - void zeroCrossDetectionTick(rims::microseconds_u16_t usToNext) override; + void zeroCrossDetectionTick(ZeroCrossDetectionEvent::Data usToNext) override; private: SingleShootTimer _triacTimerStart, _triacTimerStop; @@ -93,8 +97,24 @@ class GroupModulation : public PinControlStrategyBase { ~GroupModulation() = default; - void zeroCrossDetectionTick(rims::microseconds_u16_t usToNext) override{ + void zeroCrossDetectionTick(ZeroCrossDetectionEvent::Data usToNext) override { + if (_cyclesLeft == 0) { + restart_count(); + } + if (_cyclesOnLeft > 0) { + this->on(); // Fire triac for this cycle + --_cyclesOnLeft; + } else { + this->off(); // Skip this cycle + } + + --_cyclesLeft; + } + + void restart_count(){ + _cyclesLeft = _cyclesMax; + _cyclesOnLeft = _cycles; } void setCyclesMax(uint32_t cycles) { @@ -122,7 +142,7 @@ class GroupModulation : public PinControlStrategyBase { class NoModulation : public PowerControlStrategy { public: - void zeroCrossDetectionTick(rims::microseconds_u16_t) override { + void zeroCrossDetectionTick(ZeroCrossDetectionEvent::Data) override { } void on() const override { } @@ -131,7 +151,7 @@ class NoModulation : public PowerControlStrategy { }; class Linked : public PowerControlStrategy { - void zeroCrossDetectionTick(rims::microseconds_u16_t) override { + void zeroCrossDetectionTick(ZeroCrossDetectionEvent::Data) override { } void on() const override { } @@ -146,7 +166,7 @@ class ModeOfOperationStrategy { virtual ~ModeOfOperationStrategy() { _powerControl.off(); }; - virtual void zeroCrossDetectionTick(microseconds_u16_t usToNext) = 0; + virtual void zeroCrossDetectionTick(ZeroCrossDetectionEvent::Data usToNext) = 0; PowerControlStrategy &_powerControl; }; @@ -155,8 +175,10 @@ class DisabledMode : public ModeOfOperationStrategy { inline static NoModulation _noPowerModulationStrategy{}; public: - DisabledMode() : ModeOfOperationStrategy(_noPowerModulationStrategy){}; - void zeroCrossDetectionTick(microseconds_u16_t usToNext) override { /* nothing */ + DisabledMode() : ModeOfOperationStrategy(_noPowerModulationStrategy) { + _powerControl.off(); + }; + void zeroCrossDetectionTick(ZeroCrossDetectionEvent::Data usToNext) override { /* nothing */ } }; @@ -184,12 +206,15 @@ class PowerPublisher { } } - void notifyPowerUpdate(float newPowerPercent, uint8_t channel) { + int notifyPowerUpdate(uint8_t channel, float newPowerPercent) { + int notifications{}; for (std::size_t i = 0; i < _observers.size(); ++i) { if (_observers[i] && _channels[i] == channel) { _observers[i]->onPowerUpdate(newPowerPercent); + notifications++; } } + return notifications; } private: @@ -227,7 +252,7 @@ class ManualPowerMode : public ModeOfOperationStrategy, public TargetPowerObserv : ModeOfOperationStrategy(pc), _subscription(publisher, this, channel) { } // update state of heat element - void zeroCrossDetectionTick(microseconds_u16_t usToNext) override { /* nothing */ + void zeroCrossDetectionTick(ZeroCrossDetectionEvent::Data usToNext) override { /* nothing */ _powerControl.zeroCrossDetectionTick(usToNext); } @@ -260,12 +285,15 @@ class PhaseModulationOrchestrator { void handler_powerControlAlgorithmRequest(const PowerControlAlgorithmRequest &request, PowerControlAlgorithmResponse &resp); void handler_modeOfOperationRequest(const ModeOfOperationRequest &request, ModeOfOperationResponse &resp); + void handler_powerLevelRequest(const PowerLevelRequest &request, PowerLevelResponse &response); + void handler_groupModulationConfigRequest(const GroupModulationConfigRequest &request, GroupModulationConfigResponse &resp); void handler_groupModulationControlRequest(const GroupModulationControlRequest &request, GroupModulationControlResponse &resp); void handler_phaseModulationConfigRequest(const PhaseModulationConfigRequest &request, PhaseModulationConfigResponse &resp); void handler_phaseModulationControlRequest(const PhaseModulationControlRequest &request, PhaseModulationControlResponse &resp); + /// Helpers void setMode(uint8_t ch, ModeOfOperation mode); ModeOfOperation mode(uint8_t ch); @@ -279,9 +307,13 @@ class PhaseModulationOrchestrator { void checkCurrentMode(const uint8_t channel, ModeOfOperation mode); constexpr PowerControlStrategy &strategy(uint8_t channel) { - return std::visit([](auto &s) -> PowerControlStrategy & { return s; }, _channelControlStrategy[channel]); + return std::visit([](auto &s) -> PowerControlStrategy & { return s; }, _chPowerControlStrategy[channel]); } - std::array _channelControlStrategy; // storage for different strategies, per channal + constexpr const PowerControlStrategy &strategy(uint8_t channel) const { + return std::visit([](auto &s) -> const PowerControlStrategy & { return s; }, _chPowerControlStrategy[channel]); + } + + std::array _chPowerControlStrategy; // storage for different strategies, per channal std::array _chModeOfOperationStorage; PowerPublisher _powerPublisher; diff --git a/rims_app/src/zephyr.hpp b/rims_app/src/zephyr.hpp index 92b2648b361..ed490911c82 100644 --- a/rims_app/src/zephyr.hpp +++ b/rims_app/src/zephyr.hpp @@ -36,6 +36,10 @@ inline void pin_set_dt(const gpio_dt_spec &spec, int value) { } } +inline int pin_get_state(const gpio_dt_spec &spec) { + return gpio_pin_get_dt(&spec); +} + inline void pin_toggle_dt(const gpio_dt_spec &spec) { if (auto ret = gpio_pin_toggle_dt(&spec); ret != 0) { throw io_error{}; diff --git a/rims_app/src/zero_cross_detection.cpp b/rims_app/src/zero_cross_detection.cpp index fa5b9646e4b..fe396a507ee 100644 --- a/rims_app/src/zero_cross_detection.cpp +++ b/rims_app/src/zero_cross_detection.cpp @@ -29,11 +29,16 @@ zephyr_fifo_buffer ZeroCrossDetectionEventQueue{zcdF void ZeroCrossDetection::interrupt_handler(const device *dev, struct gpio_callback *cb, uint32_t pins) { auto _this = CONTAINER_OF(cb, ZeroCrossDetection, cb); int pin_state = gpio_pin_get(dev, gpio_pin_mask_to_index(pins)); - if (pin_state == 0) { // calculate true zero based on statistical data - _this->_trueZeroDetection.setDuration(_this->pulsWidth() / 2); - _this->_trueZeroDetection.start(); + _this->_msgQueue->try_produce([&](ZeroCrossDetectionEvent &event) { + event.data.store_now(); + event.data.to_zcd = _this->pulsWidth() / 2; + event.data.cycle_duration = _this->cycleWidth(); + + event.channel = _this->_channel; + return true; + }); // add sample _this->_cyclesWidths.push_back(std::chrono::duration_cast(clock::now() - _this->_pulseDown)); @@ -45,18 +50,7 @@ void ZeroCrossDetection::interrupt_handler(const device *dev, struct gpio_callba ZeroCrossDetection::ZeroCrossDetection(const gpio_dt_spec &gpio, ZCDFifo_t *queue, uint8_t channel) : _channel{channel}, _msgQueue{queue}, cb{ZeroCrossDetection::interrupt_handler, BIT(gpio.pin)}, // - _intCheckSem{0, 1}, _intCheckTimer{_intCheckSem, std::chrono::seconds{30}}, - _trueZeroDetection{ - [&]() { - this->_msgQueue->try_produce([&](ZeroCrossDetectionEvent &event) { - /// todo add timestamp? - event.channel = this->_channel; - event.timeToNext = this->cycleWidth(); - return true; - }); - }, - std::chrono::microseconds{500} - } { + _intCheckSem{0, 1}, _intCheckTimer{_intCheckSem, std::chrono::seconds{30}} { _intCheckTimer.start(); _pulseDown = clock::now(); diff --git a/rims_app/src/zero_cross_detection.hpp b/rims_app/src/zero_cross_detection.hpp index a97e4947fb3..97b11fa3763 100644 --- a/rims_app/src/zero_cross_detection.hpp +++ b/rims_app/src/zero_cross_detection.hpp @@ -11,7 +11,18 @@ namespace rims { struct ZeroCrossDetectionEvent { - rims::microseconds_u16_t timeToNext; + struct Data { + constexpr clock::time_point restored() const { + 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(); + } + uint64_t time_point; // timepoint + microseconds_u16_t cycle_duration; + microseconds_u16_t to_zcd; + } data; uint8_t channel; }; @@ -40,8 +51,6 @@ class ZeroCrossDetection { zephyr::semaphore::sem _intCheckSem; RecurringSemaphoreTimer _intCheckTimer; - - SingleShootTimer _trueZeroDetection; clock::time_point _pulseDown{}; rims::ring_buffer _pulsWidths; -- 2.45.2 From 75ac69f41ff119e1148f1a0247c0f368bfb272fc Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Tue, 6 May 2025 09:01:30 +0200 Subject: [PATCH 15/18] cleanup --- rims_app/src/app.hpp | 0 rims_app/src/config.cpp | 4 +- rims_app/src/config.hpp | 6 +- rims_app/src/fifo_queue.hpp | 63 ++++++++++ rims_app/src/gpio.cpp | 5 +- rims_app/src/gpio.hpp | 6 +- rims_app/src/log.cpp | 2 +- rims_app/src/log.hpp | 8 +- rims_app/src/main.cpp | 15 +-- rims_app/src/message_pb2.py | 28 ----- rims_app/src/messenger.cpp | 2 +- rims_app/src/messenger.hpp | 7 +- rims_app/src/nanopb_pb2.py | 46 -------- rims_app/src/power_control.cpp | 8 +- rims_app/src/power_control.hpp | 6 +- .../{circular_buffer.hpp => ring_buffer.hpp} | 109 ------------------ rims_app/src/temperature_measurements.cpp | 12 +- rims_app/src/temperature_measurements.hpp | 6 +- rims_app/src/uart.hpp | 2 +- rims_app/src/zero_cross_detection.cpp | 2 +- rims_app/src/zero_cross_detection.hpp | 6 +- 21 files changed, 109 insertions(+), 234 deletions(-) delete mode 100644 rims_app/src/app.hpp create mode 100644 rims_app/src/fifo_queue.hpp delete mode 100644 rims_app/src/message_pb2.py delete mode 100644 rims_app/src/nanopb_pb2.py rename rims_app/src/{circular_buffer.hpp => ring_buffer.hpp} (82%) diff --git a/rims_app/src/app.hpp b/rims_app/src/app.hpp deleted file mode 100644 index e69de29bb2d..00000000000 diff --git a/rims_app/src/config.cpp b/rims_app/src/config.cpp index 071079be662..97aa533515b 100644 --- a/rims_app/src/config.cpp +++ b/rims_app/src/config.cpp @@ -6,7 +6,7 @@ namespace rims { K_FIFO_DEFINE(configIngress); K_FIFO_DEFINE(configEgress); -zephyr_fifo_buffer configIngressQueue{configIngress}; -zephyr_fifo_buffer configEgressQueue{configEgress}; +fifo_queue configIngressQueue{configIngress}; +fifo_queue configEgressQueue{configEgress}; } // namespace rims diff --git a/rims_app/src/config.hpp b/rims_app/src/config.hpp index 77f221642f8..fc209c0fd42 100644 --- a/rims_app/src/config.hpp +++ b/rims_app/src/config.hpp @@ -1,11 +1,11 @@ #pragma once -#include "circular_buffer.hpp" +#include "fifo_queue.hpp" #include "proto/configuration.pb.h" namespace rims { -extern zephyr_fifo_buffer configIngressQueue; -extern zephyr_fifo_buffer configEgressQueue; +extern fifo_queue configIngressQueue; +extern fifo_queue configEgressQueue; } // namespace rims diff --git a/rims_app/src/fifo_queue.hpp b/rims_app/src/fifo_queue.hpp new file mode 100644 index 00000000000..a7e206c57bd --- /dev/null +++ b/rims_app/src/fifo_queue.hpp @@ -0,0 +1,63 @@ +#pragma once + +#include "ring_buffer.hpp" + +namespace rims{ + +template struct ZephyrFifoElement { + void *_reserved; + T item; +}; + +template class fifo_queue { + public: + // 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 + 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 + _elements.pop_front(); // clear first item from queue + } catch (...) { + _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 + { + auto &el = _elements.push_back(std::move(tmp)); + k_fifo_put(&_fifo, &el); // put data into a queue + return true; + } + + return false; + } + + private: + // ZephyrMutex _mutex{}; + ring_buffer, N> _elements{}; + k_fifo &_fifo; +}; +} diff --git a/rims_app/src/gpio.cpp b/rims_app/src/gpio.cpp index 9e31c9e5c14..030234f055a 100644 --- a/rims_app/src/gpio.cpp +++ b/rims_app/src/gpio.cpp @@ -20,11 +20,10 @@ struct bad_id : public error { K_FIFO_DEFINE(gpioIngress); K_FIFO_DEFINE(gpioEggress); -zephyr_fifo_buffer gpioIngressQueue{gpioIngress}; -zephyr_fifo_buffer gpioEgressQueue{gpioEggress}; +fifo_queue gpioIngressQueue{gpioIngress}; +fifo_queue gpioEgressQueue{gpioEggress}; constexpr int channels = 2; - constexpr std::array pins = { gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(gprelay_1_en), gpios), gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(gprelay_2_en), gpios) diff --git a/rims_app/src/gpio.hpp b/rims_app/src/gpio.hpp index e09f3f41918..d5c78475a67 100644 --- a/rims_app/src/gpio.hpp +++ b/rims_app/src/gpio.hpp @@ -1,6 +1,6 @@ #include "common.hpp" -#include "circular_buffer.hpp" +#include "ring_buffer.hpp" #include "proto/gpio.pb.h" #include "zephyr.hpp" @@ -12,8 +12,8 @@ namespace rims { using GpioRequest = gpio_GpioRequest; using GpioResponse = gpio_GpioResponse; -extern zephyr_fifo_buffer gpioIngressQueue; -extern zephyr_fifo_buffer gpioEgressQueue; +extern fifo_queue gpioIngressQueue; +extern fifo_queue gpioEgressQueue; class GPIO { public: diff --git a/rims_app/src/log.cpp b/rims_app/src/log.cpp index 5e86ea189da..7922bd894f7 100644 --- a/rims_app/src/log.cpp +++ b/rims_app/src/log.cpp @@ -6,7 +6,7 @@ namespace rims { K_FIFO_DEFINE(klogFifoQueue); -zephyr_fifo_buffer logEgressQueue{klogFifoQueue}; +fifo_queue logEgressQueue{klogFifoQueue}; static std::size_t g_droppedLogs{0}; diff --git a/rims_app/src/log.hpp b/rims_app/src/log.hpp index bc45f840bbb..c03aa0230bb 100644 --- a/rims_app/src/log.hpp +++ b/rims_app/src/log.hpp @@ -6,7 +6,7 @@ #include #include -#include "circular_buffer.hpp" +#include "fifo_queue.hpp" #include "proto/log.pb.h" namespace rims { @@ -19,11 +19,11 @@ enum class LogLevel : int { // Critical = 4 }; -extern zephyr_fifo_buffer logIngressFifoQueueBuffer; -extern zephyr_fifo_buffer logEgressQueue; +extern fifo_queue logIngressFifoQueueBuffer; +extern fifo_queue logEgressQueue; /// TODO move -static LogLevel g_logLevel = LogLevel::Error; +static LogLevel g_logLevel = LogLevel::Debug; class Log { public: diff --git a/rims_app/src/main.cpp b/rims_app/src/main.cpp index e492e5a49ce..546f10d58f4 100644 --- a/rims_app/src/main.cpp +++ b/rims_app/src/main.cpp @@ -11,6 +11,7 @@ #include "zephyr/debug/thread_analyzer.h" #include "zero_cross_detection.hpp" +#include #include #include #include @@ -75,12 +76,12 @@ gpio_dt_spec ch_1_zcd = GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_zcd), gpios); gpio_dt_spec ch_2_zcd = GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_zcd), gpios); void mythread_analyzer_cb(struct thread_analyzer_info *info) { - // ULOG_INFO( - // "thread name, memfree, cpu : %s, %d, %d", - // info->name, - // info->stack_size - info->stack_used, - // info->utilization // cpu usage in % - // ); + ULOG_INFO( + "thread name, memfree, cpu : %s, %d, %d", + info->name, + info->stack_size - info->stack_used, + info->utilization // cpu usage in % + ); } int main() { @@ -115,7 +116,7 @@ int main() { // zephyr::gpio::pin_toggle_dt(gprelay_2_en); while (1) { - std::this_thread::sleep_for(std::chrono::seconds{2}); + std::this_thread::sleep_for(std::chrono::seconds{5}); thread_analyzer_run(mythread_analyzer_cb, 0); // ULOG_INFO("PING"); // const unsigned char data[] = {'b', 0x55}; diff --git a/rims_app/src/message_pb2.py b/rims_app/src/message_pb2.py deleted file mode 100644 index 5c904f23a21..00000000000 --- a/rims_app/src/message_pb2.py +++ /dev/null @@ -1,28 +0,0 @@ -# -*- coding: utf-8 -*- -# Generated by the protocol buffer compiler. DO NOT EDIT! -# source: message.proto -"""Generated protocol buffer code.""" -from google.protobuf.internal import builder as _builder -from google.protobuf import descriptor as _descriptor -from google.protobuf import descriptor_pool as _descriptor_pool -from google.protobuf import symbol_database as _symbol_database -# @@protoc_insertion_point(imports) - -_sym_db = _symbol_database.Default() - - -import nanopb_pb2 as nanopb__pb2 - - -DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\rmessage.proto\x1a\x0cnanopb.proto\"8\n\x07Message\x12\n\n\x02id\x18\x01 \x01(\x07\x12\x0b\n\x03\x63rc\x18\x03 \x01(\x07\x12\x14\n\x04\x64\x61ta\x18\x02 \x01(\x0c\x42\x06\x92?\x03\x08\xfa\x01\x62\x06proto3') - -_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) -_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'message_pb2', globals()) -if _descriptor._USE_C_DESCRIPTORS == False: - - DESCRIPTOR._options = None - _MESSAGE.fields_by_name['data']._options = None - _MESSAGE.fields_by_name['data']._serialized_options = b'\222?\003\010\372\001' - _MESSAGE._serialized_start=31 - _MESSAGE._serialized_end=87 -# @@protoc_insertion_point(module_scope) diff --git a/rims_app/src/messenger.cpp b/rims_app/src/messenger.cpp index e202a9e60df..f950d26fe3a 100644 --- a/rims_app/src/messenger.cpp +++ b/rims_app/src/messenger.cpp @@ -1,6 +1,6 @@ #include "messenger.hpp" -#include "circular_buffer.hpp" +#include "ring_buffer.hpp" #include "log.hpp" #include "config.hpp" diff --git a/rims_app/src/messenger.hpp b/rims_app/src/messenger.hpp index 2a7f4c5be51..4187047ab00 100644 --- a/rims_app/src/messenger.hpp +++ b/rims_app/src/messenger.hpp @@ -1,18 +1,15 @@ #pragma once #include "common.hpp" -#include "uart.hpp" -#include "zephyr/kernel.h" #include "id.hpp" - #include "proto/message.pb.h" #include -#include -#include namespace rims { +class AsyncUART; + struct buffer { std::span data; AsyncUART *device; diff --git a/rims_app/src/nanopb_pb2.py b/rims_app/src/nanopb_pb2.py deleted file mode 100644 index a85c3de53aa..00000000000 --- a/rims_app/src/nanopb_pb2.py +++ /dev/null @@ -1,46 +0,0 @@ -# -*- coding: utf-8 -*- -# Generated by the protocol buffer compiler. DO NOT EDIT! -# NO CHECKED-IN PROTOBUF GENCODE -# source: nanopb.proto -# Protobuf Python Version: 5.29.0 -"""Generated protocol buffer code.""" -from google.protobuf import descriptor as _descriptor -from google.protobuf import descriptor_pool as _descriptor_pool -from google.protobuf import runtime_version as _runtime_version -from google.protobuf import symbol_database as _symbol_database -from google.protobuf.internal import builder as _builder -_runtime_version.ValidateProtobufRuntimeVersion( - _runtime_version.Domain.PUBLIC, - 5, - 29, - 0, - '', - 'nanopb.proto' -) -# @@protoc_insertion_point(imports) - -_sym_db = _symbol_database.Default() - - -from google.protobuf import descriptor_pb2 as google_dot_protobuf_dot_descriptor__pb2 - - -DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x0cnanopb.proto\x1a google/protobuf/descriptor.proto\"\xfe\x07\n\rNanoPBOptions\x12\x10\n\x08max_size\x18\x01 \x01(\x05\x12\x12\n\nmax_length\x18\x0e \x01(\x05\x12\x11\n\tmax_count\x18\x02 \x01(\x05\x12&\n\x08int_size\x18\x07 \x01(\x0e\x32\x08.IntSize:\nIS_DEFAULT\x12$\n\x04type\x18\x03 \x01(\x0e\x32\n.FieldType:\nFT_DEFAULT\x12\x18\n\nlong_names\x18\x04 \x01(\x08:\x04true\x12\x1c\n\rpacked_struct\x18\x05 \x01(\x08:\x05\x66\x61lse\x12\x1a\n\x0bpacked_enum\x18\n \x01(\x08:\x05\x66\x61lse\x12\x1b\n\x0cskip_message\x18\x06 \x01(\x08:\x05\x66\x61lse\x12\x18\n\tno_unions\x18\x08 \x01(\x08:\x05\x66\x61lse\x12\r\n\x05msgid\x18\t \x01(\r\x12\x1e\n\x0f\x61nonymous_oneof\x18\x0b \x01(\x08:\x05\x66\x61lse\x12\x15\n\x06proto3\x18\x0c \x01(\x08:\x05\x66\x61lse\x12#\n\x14proto3_singular_msgs\x18\x15 \x01(\x08:\x05\x66\x61lse\x12\x1d\n\x0e\x65num_to_string\x18\r \x01(\x08:\x05\x66\x61lse\x12\x1b\n\x0c\x66ixed_length\x18\x0f \x01(\x08:\x05\x66\x61lse\x12\x1a\n\x0b\x66ixed_count\x18\x10 \x01(\x08:\x05\x66\x61lse\x12\x1e\n\x0fsubmsg_callback\x18\x16 \x01(\x08:\x05\x66\x61lse\x12/\n\x0cmangle_names\x18\x11 \x01(\x0e\x32\x11.TypenameMangling:\x06M_NONE\x12(\n\x11\x63\x61llback_datatype\x18\x12 \x01(\t:\rpb_callback_t\x12\x34\n\x11\x63\x61llback_function\x18\x13 \x01(\t:\x19pb_default_field_callback\x12\x30\n\x0e\x64\x65scriptorsize\x18\x14 \x01(\x0e\x32\x0f.DescriptorSize:\x07\x44S_AUTO\x12\x1a\n\x0b\x64\x65\x66\x61ult_has\x18\x17 \x01(\x08:\x05\x66\x61lse\x12\x0f\n\x07include\x18\x18 \x03(\t\x12\x0f\n\x07\x65xclude\x18\x1a \x03(\t\x12\x0f\n\x07package\x18\x19 \x01(\t\x12\x41\n\rtype_override\x18\x1b \x01(\x0e\x32*.google.protobuf.FieldDescriptorProto.Type\x12\x43\n\x0elabel_override\x18\x1f \x01(\x0e\x32+.google.protobuf.FieldDescriptorProto.Label\x12\x19\n\x0bsort_by_tag\x18\x1c \x01(\x08:\x04true\x12.\n\rfallback_type\x18\x1d \x01(\x0e\x32\n.FieldType:\x0b\x46T_CALLBACK\x12\x13\n\x0binitializer\x18\x1e \x01(\t*i\n\tFieldType\x12\x0e\n\nFT_DEFAULT\x10\x00\x12\x0f\n\x0b\x46T_CALLBACK\x10\x01\x12\x0e\n\nFT_POINTER\x10\x04\x12\r\n\tFT_STATIC\x10\x02\x12\r\n\tFT_IGNORE\x10\x03\x12\r\n\tFT_INLINE\x10\x05*D\n\x07IntSize\x12\x0e\n\nIS_DEFAULT\x10\x00\x12\x08\n\x04IS_8\x10\x08\x12\t\n\x05IS_16\x10\x10\x12\t\n\x05IS_32\x10 \x12\t\n\x05IS_64\x10@*Z\n\x10TypenameMangling\x12\n\n\x06M_NONE\x10\x00\x12\x13\n\x0fM_STRIP_PACKAGE\x10\x01\x12\r\n\tM_FLATTEN\x10\x02\x12\x16\n\x12M_PACKAGE_INITIALS\x10\x03*E\n\x0e\x44\x65scriptorSize\x12\x0b\n\x07\x44S_AUTO\x10\x00\x12\x08\n\x04\x44S_1\x10\x01\x12\x08\n\x04\x44S_2\x10\x02\x12\x08\n\x04\x44S_4\x10\x04\x12\x08\n\x04\x44S_8\x10\x08:E\n\x0enanopb_fileopt\x12\x1c.google.protobuf.FileOptions\x18\xf2\x07 \x01(\x0b\x32\x0e.NanoPBOptions:G\n\rnanopb_msgopt\x12\x1f.google.protobuf.MessageOptions\x18\xf2\x07 \x01(\x0b\x32\x0e.NanoPBOptions:E\n\x0enanopb_enumopt\x12\x1c.google.protobuf.EnumOptions\x18\xf2\x07 \x01(\x0b\x32\x0e.NanoPBOptions:>\n\x06nanopb\x12\x1d.google.protobuf.FieldOptions\x18\xf2\x07 \x01(\x0b\x32\x0e.NanoPBOptionsB\x1a\n\x18\x66i.kapsi.koti.jpa.nanopb') - -_globals = globals() -_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals) -_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'nanopb_pb2', _globals) -if not _descriptor._USE_C_DESCRIPTORS: - _globals['DESCRIPTOR']._loaded_options = None - _globals['DESCRIPTOR']._serialized_options = b'\n\030fi.kapsi.koti.jpa.nanopb' - _globals['_FIELDTYPE']._serialized_start=1075 - _globals['_FIELDTYPE']._serialized_end=1180 - _globals['_INTSIZE']._serialized_start=1182 - _globals['_INTSIZE']._serialized_end=1250 - _globals['_TYPENAMEMANGLING']._serialized_start=1252 - _globals['_TYPENAMEMANGLING']._serialized_end=1342 - _globals['_DESCRIPTORSIZE']._serialized_start=1344 - _globals['_DESCRIPTORSIZE']._serialized_end=1413 - _globals['_NANOPBOPTIONS']._serialized_start=51 - _globals['_NANOPBOPTIONS']._serialized_end=1073 -# @@protoc_insertion_point(module_scope) diff --git a/rims_app/src/power_control.cpp b/rims_app/src/power_control.cpp index ad2e2dadc2f..7eea547b342 100644 --- a/rims_app/src/power_control.cpp +++ b/rims_app/src/power_control.cpp @@ -1,6 +1,6 @@ #include "power_control.hpp" -#include "circular_buffer.hpp" +#include "ring_buffer.hpp" #include "common.hpp" #include "log.hpp" #include "proto/ctrl.pb.h" @@ -48,8 +48,8 @@ struct failed_to_set_power_level : public error { K_FIFO_DEFINE(ctrlIngress); K_FIFO_DEFINE(ctrlEggress); -zephyr_fifo_buffer ctrlIngressQueue{ctrlIngress}; -zephyr_fifo_buffer ctrlEgressQueue{ctrlEggress}; +fifo_queue ctrlIngressQueue{ctrlIngress}; +fifo_queue ctrlEgressQueue{ctrlEggress}; constexpr std::array pins = { gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_en), gpios), @@ -360,8 +360,6 @@ void PhaseModulationOrchestrator::checkCurrentMode(const uint8_t channel, ModeOf if (mode(channel) != moo) throw ctrl::wrong_mode{}; } -/// TODO set power control - void PhaseModulationOrchestrator::setMode(uint8_t ch, ModeOfOperation mode) { checkChannel(ch); diff --git a/rims_app/src/power_control.hpp b/rims_app/src/power_control.hpp index 13297e1b673..56dfe507e48 100644 --- a/rims_app/src/power_control.hpp +++ b/rims_app/src/power_control.hpp @@ -1,7 +1,7 @@ #pragma once /* Kernel event for notifying other threads */ -#include "circular_buffer.hpp" +#include "ring_buffer.hpp" #include "common.hpp" #include "inplace_vector.hpp" #include "proto/ctrl.pb.h" @@ -15,8 +15,8 @@ namespace rims { -extern zephyr_fifo_buffer ctrlIngressQueue; -extern zephyr_fifo_buffer ctrlEgressQueue; +extern fifo_queue ctrlIngressQueue; +extern fifo_queue ctrlEgressQueue; using ModeOfOperation = ctrl_ModeOfOperation; using PowerControlAlgorithm = ctrl_PowerControlAlgorithm; diff --git a/rims_app/src/circular_buffer.hpp b/rims_app/src/ring_buffer.hpp similarity index 82% rename from rims_app/src/circular_buffer.hpp rename to rims_app/src/ring_buffer.hpp index 0fb8f98a09d..1e94fa21dd0 100644 --- a/rims_app/src/circular_buffer.hpp +++ b/rims_app/src/ring_buffer.hpp @@ -452,115 +452,6 @@ template class ring_buffer { std::array buf_; RingIndex _index; }; -class ZephyrMutex { - public: - ZephyrMutex() { - k_mutex_init(&mutex_); - } - void lock() { - k_mutex_lock(&mutex_, K_FOREVER); - } - - bool try_lock() { - return k_mutex_lock(&mutex_, K_NO_WAIT) == 0; - } - - void unlock() { - k_mutex_unlock(&mutex_); - } - - private: - struct k_mutex mutex_; - - // Disable copy & move operations (same as std::mutex) - ZephyrMutex(const ZephyrMutex &) = delete; - ZephyrMutex &operator=(const ZephyrMutex &) = delete; -}; - -template class thread_safe_circular_buffer : public ring_buffer { - using base = ring_buffer; - - public: - const T &back() const { - const std::lock_guard _lock{_mutex}; - return base::back(); - }; - - T &back() { - std::lock_guard _lock{_mutex}; - return base::back(); - }; - - T &emplace_back(T &&item) { - std::lock_guard _lock{_mutex}; - return base::emplace_back(std::forward(item)); - }; - - void pop_back() { - std::lock_guard _lock{_mutex}; - return base::pop_back(); - }; - - private: - mutable ZephyrMutex _mutex; -}; - -template struct ZephyrFifoElement { - void *_reserved; - T item; -}; - -template class zephyr_fifo_buffer { - public: - // static_assert(std::is_trivial_v); - zephyr_fifo_buffer(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 - 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 - _elements.pop_front(); // clear first item from queue - } catch (...) { - _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 - { - auto &el = _elements.push_back(std::move(tmp)); - k_fifo_put(&_fifo, &el); // put data into a queue - return true; - } - - return false; - } - - private: - // ZephyrMutex _mutex{}; - ring_buffer, N> _elements{}; - k_fifo &_fifo; -}; } // namespace rims diff --git a/rims_app/src/temperature_measurements.cpp b/rims_app/src/temperature_measurements.cpp index 40ea8bf1c83..f710bc4df55 100644 --- a/rims_app/src/temperature_measurements.cpp +++ b/rims_app/src/temperature_measurements.cpp @@ -45,8 +45,8 @@ struct unknown_channel_id : error { K_FIFO_DEFINE(temperatureIngress); K_FIFO_DEFINE(temperatureEggress); -zephyr_fifo_buffer temperatureIngressQueue{temperatureIngress}; -zephyr_fifo_buffer temperatureEgressQueue{temperatureEggress}; +fifo_queue temperatureIngressQueue{temperatureIngress}; +fifo_queue temperatureEgressQueue{temperatureEggress}; namespace { constexpr gpio_dt_spec rmin_en_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(rmin_en), gpios); @@ -123,10 +123,10 @@ void TemperatureSampler::calibration(bool disableLog) { select_channel(_channel); if (not disableLog) ULOG_INFO("Calibration at channel %d start", _channel); select_rmin(); - auto new_adc_Tmin = takeStableRead(); + auto new_adc_Tmin = takeStableRead(disableLog); select_rmax(); - auto new_adc_Tmax = takeStableRead(); + auto new_adc_Tmax = takeStableRead(disableLog); if (not disableLog) ULOG_INFO( "Calibration at channel %d done, min{%d}/max{%d}, prev min{%d}/max{%d}", _channel, new_adc_Tmin, new_adc_Tmax, _adc_Tmin, _adc_Tmax @@ -251,8 +251,8 @@ float TemperatureSampler::adc2CelciusWeight() const { } adc_sample TemperatureSampler::adcWithCalibration() { - calibration(); - return takeStableRead(); + calibration(true); + return takeStableRead(true); } adc_sample TemperatureSampler::adc() const { diff --git a/rims_app/src/temperature_measurements.hpp b/rims_app/src/temperature_measurements.hpp index f028770f906..ec028d9db22 100644 --- a/rims_app/src/temperature_measurements.hpp +++ b/rims_app/src/temperature_measurements.hpp @@ -1,6 +1,6 @@ #pragma once -#include "circular_buffer.hpp" +#include "ring_buffer.hpp" #include "common.hpp" #include "proto/temperature.pb.h" @@ -13,8 +13,8 @@ namespace rims { -extern zephyr_fifo_buffer temperatureIngressQueue; -extern zephyr_fifo_buffer temperatureEgressQueue; +extern fifo_queue temperatureIngressQueue; +extern fifo_queue temperatureEgressQueue; static constexpr std::size_t MaxSampleSize = 32; static constexpr std::uint8_t MaxOversampling = 16; diff --git a/rims_app/src/uart.hpp b/rims_app/src/uart.hpp index 73396f9ce47..7678b328a12 100644 --- a/rims_app/src/uart.hpp +++ b/rims_app/src/uart.hpp @@ -1,6 +1,6 @@ #pragma once -#include "circular_buffer.hpp" +#include "ring_buffer.hpp" #include "common.hpp" #include "inplace_vector.hpp" diff --git a/rims_app/src/zero_cross_detection.cpp b/rims_app/src/zero_cross_detection.cpp index fe396a507ee..f2320c15b7a 100644 --- a/rims_app/src/zero_cross_detection.cpp +++ b/rims_app/src/zero_cross_detection.cpp @@ -24,7 +24,7 @@ static int gpio_pin_mask_to_index(uint32_t pin_mask) { } } // namespace K_FIFO_DEFINE(zcdFifo); -zephyr_fifo_buffer ZeroCrossDetectionEventQueue{zcdFifo}; +fifo_queue ZeroCrossDetectionEventQueue{zcdFifo}; void ZeroCrossDetection::interrupt_handler(const device *dev, struct gpio_callback *cb, uint32_t pins) { auto _this = CONTAINER_OF(cb, ZeroCrossDetection, cb); diff --git a/rims_app/src/zero_cross_detection.hpp b/rims_app/src/zero_cross_detection.hpp index 97b11fa3763..0afd910c728 100644 --- a/rims_app/src/zero_cross_detection.hpp +++ b/rims_app/src/zero_cross_detection.hpp @@ -1,7 +1,7 @@ #pragma once /* Kernel event for notifying other threads */ -#include "circular_buffer.hpp" +#include "ring_buffer.hpp" #include "common.hpp" #include "zephyr.hpp" @@ -26,8 +26,8 @@ struct ZeroCrossDetectionEvent { uint8_t channel; }; -extern zephyr_fifo_buffer ZeroCrossDetectionEventQueue; -using ZCDFifo_t = zephyr_fifo_buffer; +extern fifo_queue ZeroCrossDetectionEventQueue; +using ZCDFifo_t = fifo_queue; class ZeroCrossDetection { public: -- 2.45.2 From bd5f60512a140b6ce0beafab1bdf1b7f227b84bc Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Tue, 6 May 2025 09:30:06 +0200 Subject: [PATCH 16/18] cleanup + rename of some classes --- rims_app/src/common.hpp | 34 +-- rims_app/src/fifo_queue.hpp | 28 +-- rims_app/src/function.hpp | 249 ++++++++++++++++------ rims_app/src/gpio.cpp | 1 + rims_app/src/gpio.hpp | 4 +- rims_app/src/log.hpp | 4 +- rims_app/src/messenger.cpp | 8 +- rims_app/src/messenger.hpp | 9 +- rims_app/src/power_control.cpp | 21 +- rims_app/src/power_control.hpp | 1 - rims_app/src/ring_buffer.hpp | 5 +- rims_app/src/temperature_measurements.hpp | 4 +- rims_app/src/uart.cpp | 2 +- rims_app/src/uart.hpp | 2 +- rims_app/src/zephyr.hpp | 2 +- rims_app/src/zero_cross_detection.hpp | 19 +- 16 files changed, 249 insertions(+), 144 deletions(-) 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); -- 2.45.2 From efad36a8dbf9ac95e8e3c333b14fb7fb2dbc989f Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Tue, 6 May 2025 09:30:26 +0200 Subject: [PATCH 17/18] add missing files --- rims_app/src/details/function.hpp | 75 +++++++++++++++++++++++++++++++ 1 file changed, 75 insertions(+) create mode 100644 rims_app/src/details/function.hpp diff --git a/rims_app/src/details/function.hpp b/rims_app/src/details/function.hpp new file mode 100644 index 00000000000..b31a48dbc82 --- /dev/null +++ b/rims_app/src/details/function.hpp @@ -0,0 +1,75 @@ +#pragma once +#include +#include + +namespace rims::detail { +template struct functor { + F lambda; + + static functor *cast(void *storage) noexcept { + return static_cast(storage); + } + static const functor *cast(const void *storage) noexcept { + return static_cast(storage); + } + + template static R call(const void *self, A &&...args) { + return cast(self)->lambda(std::forward(args)...); + } + + static void destroy(void *self) { + cast(self)->~functor(); + } + + static void move(void *to, void *from) { + new (to) functor{static_cast(*cast(from))}; + destroy(from); + } + + static void copy(void *to, const void *from) { + new (to) functor{static_cast(*cast(from))}; + } +}; + +struct functor_vtable { + void (*destroy)(void *); + void (*move)(void *, void *); + void (*copy)(void *, const void *); + + template static const functor_vtable *create() noexcept { + static constexpr functor_vtable vtable{functor::destroy, functor::move, functor::copy}; + return &vtable; + } + + template static const functor_vtable *trivial() noexcept { + static constexpr functor_vtable vtable{trivial_destroy, trivial_move, trivial_copy}; + return &vtable; + } + + private: + static void trivial_destroy(void *) { + } + template static void trivial_move(void *to, void *from) { + trivial_copy(to, from); + } + template static void trivial_copy(void *to, const void *from) { + std::memcpy(to, from, N); + } +}; + +template inline constexpr bool is_function_instance = false; + +template struct member_function_signature {}; +template struct member_function_signature { + using type = R(A...); +}; +template struct member_function_signature { + using type = R(A...); +}; +template struct member_function_signature { + using type = R(A...); +}; +template struct member_function_signature { + using type = R(A...); +}; +} // namespace jw::detail -- 2.45.2 From 125268821b95c1be091156c04468ee92a3b948e2 Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Tue, 6 May 2025 09:38:42 +0200 Subject: [PATCH 18/18] cleanup --- rims_app/src/inplace_vector.hpp | 4 +-- rims_app/src/ping.py | 58 --------------------------------- 2 files changed, 2 insertions(+), 60 deletions(-) delete mode 100644 rims_app/src/ping.py diff --git a/rims_app/src/inplace_vector.hpp b/rims_app/src/inplace_vector.hpp index 6afb4193cb0..d772fbbb1d4 100644 --- a/rims_app/src/inplace_vector.hpp +++ b/rims_app/src/inplace_vector.hpp @@ -280,7 +280,7 @@ template struct inplace_vector : private details::inplace_ve constexpr const_reverse_iterator crend() const noexcept { return const_reverse_iterator(cbegin()); } - [[nodiscard]] constexpr bool full() const noexcept{ + [[nodiscard]] constexpr bool full() const noexcept { return size() == capacity(); } [[nodiscard]] constexpr bool empty() const noexcept { @@ -295,7 +295,7 @@ template struct inplace_vector : private details::inplace_ve static constexpr size_type capacity() noexcept { return N; } - + // constexpr void resize(size_type sz); // constexpr void resize(size_type sz, const T& c); constexpr void reserve(size_type n) { diff --git a/rims_app/src/ping.py b/rims_app/src/ping.py deleted file mode 100644 index 7b37a5622e8..00000000000 --- a/rims_app/src/ping.py +++ /dev/null @@ -1,58 +0,0 @@ -import serial -import time -import message_pb2 # Import the generated protobuf module -import struct -import crcmod.predefined - -# UART Configuration -SERIAL_PORT = "/dev/ttyUSB0" # Change to your actual UART port -BAUD_RATE = 921600 -TIMEOUT = 2 # Timeout for UART response - -# Compute CRC32 -def compute_crc32(data: bytes) -> int: - crc32_func = crcmod.predefined.mkPredefinedCrcFun('crc-32') - return crc32_func(data) - -# Create and serialize a Message -def create_message(msg_id: int, payload: bytes): - msg = message_pb2.Message() - msg.id = msg_id - msg.data = payload[:250] # Ensure max 250 bytes - msg.crc = compute_crc32(msg.data) - - return msg.SerializeToString() - -# Send message over UART -def send_message(ser, msg_data): - ser.write(msg_data) - print(f"Sent {len(msg_data)} bytes.") - -# Wait for acknowledgment -def wait_for_ack(ser): - try: - ack = ser.read(19) - if ack == b'ACK': - print("Received ACK: Message successfully received!") - else: - print(f"Unexpected response: {ack}") - except serial.SerialTimeoutException: - print("Timeout: No ACK received.") - -def main(): - # Open serial port - ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=TIMEOUT) - - try: - payload = b"X0000000" # Example payload - message_data = create_message(msg_id=17, payload=payload) - - send_message(ser, message_data) - wait_for_ack(ser) - - finally: - ser.close() - -if __name__ == "__main__": - main() - -- 2.45.2