From fd2e07f3003329400bccda4d64b08801970b802f Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Wed, 30 Apr 2025 08:53:46 +0200 Subject: [PATCH] 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);