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..194eacf0dc3 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,28 @@ 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 = <250000>; - current-speed = <921600>; + 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"; }; +&usart1_tx_pb14 { + slew-rate = "very-high-speed"; +}; + +&usart1_rx_pb15 { + slew-rate = "very-high-speed"; +}; + /* &rng { status = "okay"; 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/CMakeLists.txt b/rims_app/CMakeLists.txt index 8d7099cf169..7e482c9cbaa 100644 --- a/rims_app/CMakeLists.txt +++ b/rims_app/CMakeLists.txt @@ -1,20 +1,15 @@ -# 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/main.cpp 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}) -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/CMakeLists.txt.user b/rims_app/CMakeLists.txt.user index 6b065675621..a558ff55784 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 @@ -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/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/configuration.proto b/rims_app/configuration.proto deleted file mode 100644 index 78127e7a552..00000000000 --- a/rims_app/configuration.proto +++ /dev/null @@ -1,64 +0,0 @@ -syntax = "proto3"; - -package config; -// channel configuration - -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/ctrl.proto b/rims_app/ctrl.proto deleted file mode 100644 index 950b9e12932..00000000000 --- a/rims_app/ctrl.proto +++ /dev/null @@ -1,67 +0,0 @@ -syntax = "proto3"; - -package ctrl; -// channel configuration - -enum Error { - NoError = 0; - - WrongMode = 1; - WrongChannel = 2; -} - -message MaxPower -{ -} - -// Parameters for Phase Modulation -message PhaseModulationParams -{ - /**/ -} - -// Parameters for Group Modulation -message GroupModulationParams -{ - uint32 cycles = 1; // Duration of each group in milliseconds (optional) -} - -message ManualPowerControlRequest -{ - uint32 channelID = 1; // required - oneof modulation - { - PhaseModulationParams phase = 2; - GroupModulationParams group = 3; - } -} - -message ManualPowerControlResponse -{ - oneof modulation - { - PhaseModulationParams phase = 2; - GroupModulationParams group = 3; - Error error = 254; - } -} - -// only those messages are send through interface -message IngressMessages -{ - uint32 requestID = 255; // Unique request ID - oneof data - { - ManualPowerControlRequest manualPowerControlRequest = 1; - } -}; - -message EgressMessages -{ - optional uint32 requestID = 255; // Unique request ID - oneof data - { - ManualPowerControlResponse manualPowerControlResponse = 1; - // also broadcast - } -}; diff --git a/rims_app/prj.conf b/rims_app/prj.conf index ee777d885ea..ebf1a07cb3a 100644 --- a/rims_app/prj.conf +++ b/rims_app/prj.conf @@ -1,55 +1,52 @@ # optimizations - 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 -#CONFIG_MPU_STACK_GUARD=y +CONFIG_HW_STACK_PROTECTION=y +CONFIG_MPU_STACK_GUARD=y +CONFIG_ASSERT=n +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 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_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 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_INTERRUPT_DRIVEN=y +# subsystems 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_CRC=y - -CONFIG_ASSERT=n - -#CONFIG_NUM_PREEMPT_PRIORITIES=0 +CONFIG_MAIN_STACK_SIZE=1500 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/proto/configuration.proto b/rims_app/proto/configuration.proto new file mode 100644 index 00000000000..87a2ba4dd19 --- /dev/null +++ b/rims_app/proto/configuration.proto @@ -0,0 +1,19 @@ +syntax = "proto3"; + +package config; +// channel configuration + +enum Error { + NoError = 0; +} + +// only those messages are send +message IngressMessage +{ + uint32 request_id = 255; +}; + +message EgressMessage +{ + optional uint32 request_id = 255; +}; diff --git a/rims_app/proto/ctrl.proto b/rims_app/proto/ctrl.proto new file mode 100644 index 00000000000..51099dcff86 --- /dev/null +++ b/rims_app/proto/ctrl.proto @@ -0,0 +1,252 @@ +syntax = "proto3"; + +import "nanopb.proto"; +package ctrl; +// channel configuration + +enum Error { + NoError = 0; + + WrongMode = 1; + UnsupportedMode = 2; + + WrongChannel = 3; + + PowerLevelFailedToSet = 4; + + UnknownError = 32; +} + +// 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 = 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 = 2; +} + +// Mode of operation used to control the heating element on a given channel. +enum ModeOfOperation { + Disabled = 0; + + // ConstantTemperature maintains a target temperature using a feedback loop. + // The system adjusts the heating power dynamically to keep the temperature stable. + ConstantTemperature = 3; + + // 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; + + // 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; + + // 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 PowerControlAlgorithm alg = 2; +} + +message PowerControlAlgorithmResponse +{ + uint32 channel_id = 1; + PowerControlAlgorithm alg = 2; + optional Error error = 254; +} + +message ModeOfOperationRequest +{ + uint32 channel_id = 1; + optional ModeOfOperation mode = 2; +} + +message ModeOfOperationResponse +{ + uint32 channel_id = 1; + ModeOfOperation mode = 2; + 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 +{ + // 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; +} + +// Response message for returning the Group Modulation configuration. +message GroupModulationConfigResponse +{ + // 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; +} + +// 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; +// } + +// 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 +{ + uint32 request_id = 255; + oneof data + { + PowerControlAlgorithmRequest power_control_algorithm_request = 1; + ModeOfOperationRequest mode_of_operation_request = 2; + + PowerLevelRequest power_level_request = 3; + + GroupModulationConfigRequest group_modulation_config_request = 10; + GroupModulationControlRequest group_modulation_control_request = 11; + + PhaseModulationConfigRequest phase_modulation_config_request = 12; + PhaseModulationControlRequest phase_modulation_control_request = 13; + } +}; + +message EgressMessage +{ + optional uint32 request_id = 255; // not set for broadcast + oneof data + { + PowerControlAlgorithmResponse power_control_algorithm_response = 1; + ModeOfOperationResponse mode_of_operation_response = 2; + + PowerLevelResponse power_level_response = 3; + + 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/log.proto b/rims_app/proto/log.proto similarity index 90% rename from rims_app/log.proto rename to rims_app/proto/log.proto index 1b051454700..a5760c4d9e3 100644 --- a/rims_app/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/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..4fc07b5fbed 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/proto/temperature.proto b/rims_app/proto/temperature.proto new file mode 100644 index 00000000000..ea5ef06850d --- /dev/null +++ b/rims_app/proto/temperature.proto @@ -0,0 +1,207 @@ +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; + TemperatureSensorDisconnected = 2; + TemperatureSensorBroken = 3; +} + +enum FilterType { + None = 0; + Kalman = 1; + EMA = 2; + Avarage = 3; +} + +/* CONFIGURATION */ +// Only optional values will be set on device +message SamplerConfig +{ + // 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; +} + +message KalmanFilterParams +{ + optional float Q = 1; + optional float R = 2; +} + +message EMAFilterParams +{ + optional float alpha = 1; +} + +message AverageFilterParams +{ + optional uint32 samples = 1; +} + +/* INTERNAL STRUCTURES */ + +// Last temp sample +message TemperatureCurrent +{ + // last update + float temp_c = 1; +} + +// Statictics of temperature on given channel +message TemperatureSummary +{ + // very last update + float temp_c = 1; + + // 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; + + // samples taken + uint32 n_samples = 4; +} + +/* CONFIG API */ +// Read/Write +message SamplerConfigRequest +{ + uint32 channel_id = 1; + SamplerConfig config = 2; +} + +message SamplerConfigResponse +{ + uint32 channel_id = 1; + 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 skip_full_response = 5; + + 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 kalman_filter_params = 2; + EMAFilterParams ema_filter_params = 3; + AverageFilterParams average_filter_params = 4; + + optional Error error = 254; +} + +message FilterRequest +{ + uint32 channel_id = 1; + optional FilterType filter_type = 2; +} +message FilterResponse +{ + uint32 channel_id = 1; + FilterType filter_type = 2; + optional Error error = 254; +} + +// API +// Read +message GetTemperatureRequest +{ + uint32 channel_id = 1; +}; +message GetTemperatureResponse +{ + uint32 channel_id = 1; + TemperatureCurrent temperature_current = 2; + optional Error error = 254; +} + +// Read +message GetTemperatureSummaryRequest +{ + uint32 channel_id = 1; +}; +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 IngressMessage +{ + uint32 request_id = 255; + oneof data + { + // data access + GetTemperatureRequest temperature_request = 1; + GetTemperatureSummaryRequest temperature_summary_request = 2; + + // configuration + SamplerConfigRequest sampler_config_request = 3; + FilterConfigRequest filter_config_request = 4; + FilterRequest filter_request = 5; + } +}; + +message EgressMessage +{ + optional uint32 request_id = 255; + oneof data + { + // data access + GetTemperatureResponse temperature_response = 1; + GetTemperatureSummaryResponse temperature_summary_response = 2; + + // configuration + SamplerConfigResponse sampler_config_response = 3; + FilterConfigResponse filter_config_response = 4; + FilterResponse filter_response = 5; + + // broadcast + BroadcastTemperatureSummary broadcast_temperature_summary = 16; + } +}; diff --git a/rims_app/utilization.proto b/rims_app/proto/utilization.proto similarity index 73% rename from rims_app/utilization.proto rename to rims_app/proto/utilization.proto index 5424bd6cec2..c2cfef7bc74 100644 --- a/rims_app/utilization.proto +++ b/rims_app/proto/utilization.proto @@ -12,17 +12,14 @@ message Utilization }; // only those messages are send through wire at temperature endpoint -message IngressMessages +message IngressMessage { - uint32 requestID = 255; -// oneof data -// { -// } + 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/app.hpp b/rims_app/src/app.hpp deleted file mode 100644 index e69de29bb2d..00000000000 diff --git a/rims_app/src/circular_buffer.hpp b/rims_app/src/circular_buffer.hpp deleted file mode 100644 index ddaf8f6b06f..00000000000 --- a/rims_app/src/circular_buffer.hpp +++ /dev/null @@ -1,404 +0,0 @@ -#pragma once - -#include "zephyr.hpp" -#include "zephyr/sys/__assert.h" -#include -#include -#include - -#include -#include - -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; - std::size_t count = 0; - - public: - constexpr std::size_t size() const noexcept { - return count; - } - - constexpr void clean() noexcept { - 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]; - } -}; - -template class circular_buffer { - public: - explicit circular_buffer() = default; - - 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; - } - - head_ = (head_ + 1) % N; - full_ = head_ == tail_; - return directy_at(at); - } - - 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; - return val; - } - - 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; - } - - 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; - } - - const T &front() const { - assert(!empty()); - return *begin(); - } - T &front() { - int i = head_ - 1; - if (i == -1) i = N - 1; - return *begin(); - } - - const T &back() const { - int i = head_ - 1; - if (i == -1) i = N - 1; - return directy_at(i); - } - T &back() { - int i = head_ - 1; - if (i == -1) i = N - 1; - return directy_at(i); - } - - constexpr void reset() { - head_ = tail_; - full_ = false; - } - - bool empty() const { - return (!full() && (head_ == tail_)); - } - - bool full() const { - return full_; - } - - std::size_t capacity() const { - return N; - } - - 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; - } - - // Iterator class for circular_buffer - class iterator { - public: - using iterator_category = std::forward_iterator_tag; - using value_type = T; - using difference_type = std::ptrdiff_t; - using pointer = T *; - using reference = 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) { - } - - 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; - } - - 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) { - } - - reference operator*() const { - return *reinterpret_cast(std::addressof(buf_[index_])); - } - - pointer operator->() const { - return reinterpret_cast(std::addressof(buf_[index_])); - } - - const_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; - } - - const_iterator operator++(int) { - iterator tmp = *this; - ++(*this); - return tmp; - } - - bool operator==(const const_iterator &other) const { - return index_ == other.index_ && looped_; - } - - bool operator!=(const const_iterator &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_; - }; - - // Begin and end functions to return iterator - iterator begin() { - return iterator(buf_, tail_, tail_, head_); - } - - iterator end() { - return iterator(buf_, head_, tail_, head_); - } - - const_iterator cbegin() const { - return const_iterator(buf_, tail_, tail_, head_); - } - - const_iterator cend() const { - return const_iterator(buf_, head_, tail_, head_); - } - - private: - const T &directy_at(int index) const { - return *reinterpret_cast(std::addressof(buf_[index])); - } - T &directy_at(int index) { - 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}; -}; - -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 circular_buffer { - using base = circular_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: - 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)); - - if (not el) return false; // should be a assert - fn(el->item); // consume item - _elements.pop_back(); // clear item - - return true; - }; - - template // - bool try_produce(Fn fn) { - // 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 - return true; - } - - private: - // ZephyrMutex _mutex{}; - circular_buffer, N> _elements{}; - k_fifo &_fifo; -}; - -} // namespace rims diff --git a/rims_app/src/common.hpp b/rims_app/src/common.hpp index b7c0b2bb5df..f22bda001a8 100644 --- a/rims_app/src/common.hpp +++ b/rims_app/src/common.hpp @@ -1,14 +1,10 @@ #pragma once -#include "log.hpp" -#include "syscalls/kernel.h" -#include "zephyr/sys_clock.h" +#include "function.hpp" #include #include #include - -#include #include #include #include @@ -16,15 +12,43 @@ #include #include #include +#include +#include #include #include 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; } @@ -32,6 +56,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)); @@ -46,16 +71,24 @@ 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); } - virtual ~Timer() = default; + virtual ~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); + k_timer_start(&timer_, _duration, _period); } void stop() { @@ -63,32 +96,37 @@ 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; } - 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(timer_.timeout.dticks)}; + 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(timer_.period.ticks)}; + return std::chrono::microseconds{k_ticks_to_us_floor64(_period.ticks)}; } public: 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 @@ -102,7 +140,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; @@ -113,6 +151,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); } @@ -122,26 +161,36 @@ class RecurringSemaphoreTimer : public Timer { class SingleShootTimer : public Timer { public: - SingleShootTimer(std::function cb, std::chrono::milliseconds interval) : Timer(interval), _cb{std::move(cb)} { + SingleShootTimer(rims::function cb, std::chrono::microseconds duration) : Timer(duration), _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(); } - std::function _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(); + } + + 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; @@ -201,6 +250,7 @@ class ZephyrThread { // Optionally start the thread if delayed or suspended void start() { k_thread_start(&_threadData); + k_yield(); } protected: @@ -233,9 +283,4 @@ 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 short_microseconds = std::chrono::duration; // max ~65ms -using adc_sample = uint16_t; - } // namespace rims diff --git a/rims_app/src/config.cpp b/rims_app/src/config.cpp index 7f136c6da70..97aa533515b 100644 --- a/rims_app/src/config.cpp +++ b/rims_app/src/config.cpp @@ -1,12 +1,12 @@ #include "config.hpp" -#include "configuration.pb.h" +#include "proto/configuration.pb.h" 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 5814870c629..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 "configuration.pb.h" +#include "fifo_queue.hpp" +#include "proto/configuration.pb.h" -namespace rims{ +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/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 diff --git a/rims_app/src/fifo_queue.hpp b/rims_app/src/fifo_queue.hpp new file mode 100644 index 00000000000..1361af96bcc --- /dev/null +++ b/rims_app/src/fifo_queue.hpp @@ -0,0 +1,65 @@ +#pragma once + +#include "ring_buffer.hpp" + +#include + +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{}; + 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 new file mode 100644 index 00000000000..baa18f8c33a --- /dev/null +++ b/rims_app/src/gpio.cpp @@ -0,0 +1,145 @@ +#include "gpio.hpp" +#include "proto/gpio.pb.h" +#include "zephyr.hpp" +#include "log.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); + +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) +}; + +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..3d13c5bc221 --- /dev/null +++ b/rims_app/src/gpio.hpp @@ -0,0 +1,39 @@ +#include "common.hpp" + +#include "fifo_queue.hpp" + +#include "proto/gpio.pb.h" +#include + +namespace rims { + +using GpioRequest = gpio_GpioRequest; +using GpioResponse = gpio_GpioResponse; + +extern fifo_queue gpioIngressQueue; +extern fifo_queue 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/id.hpp b/rims_app/src/id.hpp new file mode 100644 index 00000000000..4082382f0b3 --- /dev/null +++ b/rims_app/src/id.hpp @@ -0,0 +1,128 @@ +#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 : 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 & 0x3FF) << 4; + raw |= (receiver & 0x3FF) << 14; + 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 & 0x3FF) << 4; + raw |= (receiver & 0x3FF) << 14; + 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 >> 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) + 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)); +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/inplace_vector.hpp b/rims_app/src/inplace_vector.hpp new file mode 100644 index 00000000000..d772fbbb1d4 --- /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/log.cpp b/rims_app/src/log.cpp index 03fc8e72a0f..7922bd894f7 100644 --- a/rims_app/src/log.cpp +++ b/rims_app/src/log.cpp @@ -1,14 +1,12 @@ #include "log.hpp" -#include "log.pb.h" #include "zephyr/kernel.h" #include -#include #include #include namespace rims { K_FIFO_DEFINE(klogFifoQueue); -zephyr_fifo_buffer logEgressFifoQueueBuffer{klogFifoQueue}; +fifo_queue logEgressQueue{klogFifoQueue}; static std::size_t g_droppedLogs{0}; @@ -68,12 +66,13 @@ 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 = logEgressQueue.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); logmsg.data.logEntry.lineNumber = this->_sl.line(); diff --git a/rims_app/src/log.hpp b/rims_app/src/log.hpp index a8760d47ce7..fcdda173022 100644 --- a/rims_app/src/log.hpp +++ b/rims_app/src/log.hpp @@ -2,12 +2,12 @@ #include #include -#include #include #include -#include "circular_buffer.hpp" -#include "log.pb.h" +#include "fifo_queue.hpp" +#include "function.hpp" +#include "proto/log.pb.h" namespace rims { @@ -19,8 +19,8 @@ enum class LogLevel : int { // Critical = 4 }; -extern zephyr_fifo_buffer logIngressFifoQueueBuffer; -extern zephyr_fifo_buffer logEgressFifoQueueBuffer; +extern fifo_queue logIngressFifoQueueBuffer; +extern fifo_queue logEgressQueue; /// TODO move static LogLevel g_logLevel = LogLevel::Debug; @@ -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/main.cpp b/rims_app/src/main.cpp index 143f6510648..546f10d58f4 100644 --- a/rims_app/src/main.cpp +++ b/rims_app/src/main.cpp @@ -1,24 +1,31 @@ #include "common.hpp" -#include "message_decode.hpp" -#include "phase_modulation.hpp" +#include "gpio.hpp" +#include "log.hpp" +#include "messenger.hpp" +#include "placement_unique_ptr.hpp" +#include "power_control.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 -#include +#include +#include #include #include +#include + 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); @@ -27,23 +34,20 @@ 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)}; +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 /// 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; - template class LazyInit { alignas(T) std::byte _buf[sizeof(T)]; unique_placed_ptr obj; @@ -65,6 +69,20 @@ 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 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 % + ); +} int main() { using namespace rims; @@ -74,6 +92,7 @@ int main() { temperatureSampler.init(temperatureSamplerStack); zcd.init(zeroCrossDetectionStack); phaseModulation.init(phaseModulationStack); + gpio.init(gpioStack); messengerTread->init_hw(); messengerTread->start(); @@ -90,46 +109,17 @@ int main() { phaseModulation->init_hw(); phaseModulation->start(); + gpio->init_hw(); + gpio->start(); + + zephyr::gpio::pin_configure(status, GPIO_OUTPUT_INACTIVE); + + // zephyr::gpio::pin_toggle_dt(gprelay_2_en); while (1) { - std::this_thread::sleep_for(std::chrono::milliseconds{50}); + 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}; + // uart_fifo_fill(defaultUart()->_dev, data, 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/message_decode.cpp b/rims_app/src/message_decode.cpp deleted file mode 100644 index d705953d558..00000000000 --- a/rims_app/src/message_decode.cpp +++ /dev/null @@ -1,318 +0,0 @@ -#include "message_decode.hpp" - -#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 -#include -#include -#include - -#include -#include -#include - -#include "cobs.h" -#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); - -/* -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 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) - -// // 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 -// } - -// 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 -// } - -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); }); -} - -void rims::MessengerThread::event_ctrlEgress() -{ - ctrlEgressQueue.try_consume([&](ctrl_EgressMessages &in) { ipcPush(in, ctrl_EgressMessages_msg, 2); }); -} - -void rims::MessengerThread::event_configEgress() -{ - configEgressQueue.try_consume([&](config_EgressMessages &in) { ipcPush(in, config_EgressMessages_msg, 3); }); -} - -void rims::MessengerThread::event_logEgress() -{ - logEgressFifoQueueBuffer.try_consume([&](log_EgressMessages &in) { ipcPush(in, log_EgressMessages_msg, 4); }); -} - -void MessengerThread::handle_badCRC() -{ - /// TODO -} - -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) { - 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)); - } - } -} - -void MessengerThread::event_dataArrived() { - // decode - // put on queue - 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); - - // pb_istream_t datastream; - uint32_t datasize{}; - pb_byte_t *databegin{}; - uint32_t id{}, crc{}; - bool hasId{false}, hasCrc{false}, hasData{false}; - - auto decode = [&]() { - uint32_t tag; - bool eof; - pb_wire_type_t wire_t; - - pb_decode_tag(&stream, &wire_t, &tag, &eof); - - 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); - 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; - } - } -} - -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); - } - return true; - }); -} - -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); - } - return true; - }); -} - -void MessengerThread::handle_configIngressMsg(pb_istream_t &stream, AsyncUART *cb) { - // 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); - } - 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); - - // set IF od sender - message.id = id; /// TODO make response ID - message.data.size = ostream.bytes_written; - 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); - if (cb == nullptr) { - cb = defaultUart(); - } - encode(message, cb); - } else { - encode(message, defaultUart()); - } -} - -bool MessengerThread::decode(pb_istream_t &stream, const pb_msgdesc_t *fields, void *dest) const { - return pb_decode(&stream, fields, &dest); -} - -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); - } - - return true; -} - -void MessengerThread::putActiveRequest(int type, int id, AsyncUART *cb) { - for (auto &req : _activeRequests) { - if (req.type == -1) { // Find an empty slot - req.type = type; - req.requestId = id; - req.cb = cb; - return; - } - } -} - -AsyncUART *MessengerThread::takeActiveRequest(int type, int id) { - for (auto &req : _activeRequests) { - if (req.type == type && req.requestId == id) { - req.type = -1; // Mark as available - return req.cb; - } - } - return {}; -} - -} // namespace rims diff --git a/rims_app/src/message_decode.hpp b/rims_app/src/message_decode.hpp deleted file mode 100644 index 548f92578ae..00000000000 --- a/rims_app/src/message_decode.hpp +++ /dev/null @@ -1,60 +0,0 @@ -#pragma once - -#include "common.hpp" -#include "message.pb.h" -#include "uart.hpp" -#include "zephyr/kernel.h" - -#include -#include - -namespace rims { -struct buffer { - std::byte *data; - std::size_t size; - AsyncUART *device; -}; - -extern k_msgq messenger_buffer_arrived_queue; - -class MessengerThread : public ZephyrThread { - public: - MessengerThread(TStackBase &stack); - - static void send() { - } - - protected: - void threadMain() override; - - private: - struct requestData { - int type; - int requestId; - AsyncUART *cb; - }; - - void event_dataArrived(); - void event_temperatureEgress(); - void event_ctrlEgress(); - 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 putActiveRequest(int type, int id, AsyncUART *cb); - AsyncUART *takeActiveRequest(int type, int id); - - std::array _events; - std::array _activeRequests; -}; -} // namespace rims 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 new file mode 100644 index 00000000000..3a59a50b8e0 --- /dev/null +++ b/rims_app/src/messenger.cpp @@ -0,0 +1,510 @@ +#include "messenger.hpp" + +#include "ring_buffer.hpp" +#include "log.hpp" + +#include "config.hpp" +#include "gpio.hpp" +#include "power_control.hpp" +#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/gpio.pb.h" +#include "proto/log.pb.h" +#include "proto/message.pb.h" +#include "proto/temperature.pb.h" + +#include "id.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "cobs.h" +#include "zephyr.hpp" + +namespace rims { +K_MSGQ_DEFINE(messenger_buffer_arrived_queue, sizeof(rims::BufferView), 2, 1); +namespace { + +bool crcIsInvalid(std::span data, std::uint32_t crc) { + return zephyr::crc::crc32_ieee(data) != crc; +} + +} // namespace +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 + +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, 9, 0, "Messenger"} { + zephyr::event_pool::k_init(_events.at(0), messenger_buffer_arrived_queue); + + for (auto &ar : _activeRequests) { + ar.wireId = {}; + ar.wireId.reserved = -1; // maark unused slot + ar.cb = {}; + ar.requestId = {}; + } +} + +void MessengerThread::threadMain() { + temperatureEgressQueue.k_event_init(_events.at(1)); + ctrlEgressQueue.k_event_init(_events.at(2)); + configEgressQueue.k_event_init(_events.at(3)); + 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 { + 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)); + zephyr::event_pool::k_poll_handle(_events[5], std::bind(&MessengerThread::event_gpioEgress, this)); + } + } catch (const std::exception &e) { + /// TODO + } + } +} + +void MessengerThread::event_dataArrived() { + std::span submessage{}; + WireFormatID id{}; + uint32_t crc{}; + BufferView buf; + + try { + 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); + + const auto decode_id = [&]() { + try { + id.fromWire(pb::decode_fixed32(stream)); + } catch (const pb::error &e) { + throw messenger::no_id{}; + } + }; + + const auto decode_crc = [&]() { + try { + crc = pb::decode_fixed32(stream); + } catch (const pb::error &e) { + throw messenger::no_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 (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 + case 0x01: { /// TODO temperature endpoint ID + handle_temperatureIngressMsg(id, stream, buf.device); + break; + } + case 0x02: /// TODO configuration endpoint ID + { + handle_ctrlIngressMsg(id, stream, buf.device); + break; + } + case 0x03: /// TODO phase controll endpoint ID + { + handle_configIngressMsg(id, stream, buf.device); + break; + } + case 0x04: { + // logIngressFifoQueueBuffer.try_produce() + break; + } + case 0x05: { + handle_gpioIngressMsg(id, stream, buf.device); + break; + } + default: { + AsyncUART::transmit(buf.device, buf.data); + break; + } + } + } 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::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; + egressPush(message, 16, std::nullopt); + } +} + +void MessengerThread::event_temperatureEgress() { + 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_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_EgressMessage &out) { + egressPush(&out, config_EgressMessage_msg, 3, out.has_request_id ? std::optional{out.request_id} : std::nullopt); + }); +} + +void MessengerThread::event_logEgress() { + 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::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) { + 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) { + 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) { + // 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(wireId_receiver, 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); + } + // if no request was sent before, just drop the message + } else { + transmit(message, defaultUart()); + } +} + +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; + + 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; + + egressPush(message, wireId_receiver, requestId); +} + +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)); + 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}; + 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; +} + +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.wireId.reserved != 0) { // Find an empty slot + req.wireId = wireId; + req.requestId = id; + req.cb = cb; + return; + } + } + + throw messenger::request_queue_full{}; +} + +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) { + ret = req; // copy to return variable + req.wireId.reserved = -1; // mark as empty slot + break; + } + } + + return ret; +} + +} // namespace rims diff --git a/rims_app/src/messenger.hpp b/rims_app/src/messenger.hpp new file mode 100644 index 00000000000..1f584e807e0 --- /dev/null +++ b/rims_app/src/messenger.hpp @@ -0,0 +1,63 @@ +#pragma once + +#include "common.hpp" + +#include "id.hpp" +#include "proto/message.pb.h" + +#include +#include + +namespace rims { +class AsyncUART; + +struct BufferView { + std::span data; + AsyncUART *device; +}; + +extern k_msgq messenger_buffer_arrived_queue; + +class MessengerThread : public ZephyrThread { + public: + MessengerThread(TStackBase &stack); + + static void send() { + } + + protected: + void threadMain() override; + + private: + struct RequestData { + WireFormatID wireId; // original ID of IngressMessage + uint32_t requestId; // requestID from message + AsyncUART *cb; + }; + + void event_dataArrived(); + void event_temperatureEgress(); + 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); + + void egressPush(Message &message, int id, std::optional requestId); + 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; + uint8_t _activeRequestsNr; +}; +} // namespace rims 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/phase_modulation.cpp b/rims_app/src/phase_modulation.cpp deleted file mode 100644 index 956a881b035..00000000000 --- a/rims_app/src/phase_modulation.cpp +++ /dev/null @@ -1,155 +0,0 @@ -#include "phase_modulation.hpp" - -#include "circular_buffer.hpp" -#include "ctrl.pb.h" -#include "log.hpp" -#include "pb_encode.h" -#include "zephyr.hpp" -#include "zero_cross_detection.hpp" - -#include -#include -#include -#include -#include - -namespace rims { - -K_FIFO_DEFINE(ctrlIngress); -K_FIFO_DEFINE(ctrlEggress); - -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) -}; - -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::action_triacEnable() const { - gpio_pin_set_dt(&_gpio, 1); -} - -void PhaseControlBase::action_triacDisable() const { - 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}} { - ULOG_INFO("Started"); -} - -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 - - // 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(); -} - -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]); - - ZeroCrossDetectionEventQueue.k_event_init(_events[0]); - ctrlIngressQueue.k_event_init(_events[1]); -} - -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(); }); - } - } -} - -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 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 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 setPower = [](auto &channel) { channel->setPower(sinewave(std::chrono::steady_clock::now())); }; - 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) { - std::visit(tickRisingEdge, _channel[event.channel]); - } else { - std::visit(tickFallingEdge, _channel[event.channel]); - } - }); -} - -void PhaseModulationOrchestrator::event_ctrlMessageArrived() { - ctrlIngressQueue.try_consume([&](const ctrl_IngressMessages &request) { - ULOG_INFO("control request message handler"); - ctrlEgressQueue.try_produce([&](ctrl_EgressMessages &response) { - 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; - break; - } - return true; - }); - }); -} - -void PhaseModulationOrchestrator::handler_manualPowerControlResponse(const ManualPowerControlRequest &req, ManualPowerControlResponse &resp) { -} - -void PhaseModulationThread::threadMain() { - ULOG_INFO("PhaseModulationThread start"); - /// runs in context of new thread, on new thread stack etc. - PhaseModulationOrchestrator thread{}; - thread.loop(); -} - -} // namespace rims diff --git a/rims_app/src/phase_modulation.hpp b/rims_app/src/phase_modulation.hpp deleted file mode 100644 index 12506fc926c..00000000000 --- a/rims_app/src/phase_modulation.hpp +++ /dev/null @@ -1,98 +0,0 @@ -#pragma once - -/* Kernel event for notifying other threads */ -#include "circular_buffer.hpp" -#include "placement_unique_ptr.hpp" -#include "common.hpp" -#include "ctrl.pb.h" - -#include -#include -#include - -namespace rims { - -extern zephyr_fifo_buffer ctrlIngressQueue; -extern zephyr_fifo_buffer ctrlEgressQueue; - -using ManualPowerControlRequest = ctrl_ManualPowerControlRequest; -using ManualPowerControlResponse = ctrl_ManualPowerControlResponse; - -constexpr int Channels = 2; - -class PhaseControlBase { - public: - PhaseControlBase(const gpio_dt_spec &gpio); - void action_triacDisable() const; - void action_triacEnable() const; - - const gpio_dt_spec &_gpio; -}; - -class PhaseModulation : public PhaseControlBase { - public: - PhaseModulation(const gpio_dt_spec &gpio); - - // function triggered 100 times / sec - void setPower(float percent){ - _power = percent; - } - void tickFallingEdge(); - void tickRisingEdge() {}; - - private: - SingleShootTimer _triacTimer, _off; - float _power; -}; - -class GroupModulation : public PhaseControlBase { - public: - void setPower(float percent){} - void tickFallingEdge() {}; - void tickRisingEdge() {}; -}; - -class NoneModulation { - public: - void setPower(float percent){} - void tickFallingEdge() {}; - void tickRisingEdge() {}; -}; - -/// manages all phase controll messages -class PhaseModulationOrchestrator { - using Variant = std::variant< // - unique_placed_ptr, - unique_placed_ptr, - unique_placed_ptr>; - - public: - PhaseModulationOrchestrator(); - void loop(); - - private: - void event_zeroCrossDetection(); - void event_ctrlMessageArrived(); - - void handler_manualPowerControlResponse(const ManualPowerControlRequest &req, ManualPowerControlResponse &resp); - - bool setup(); - int gpio_init(uint_fast8_t channel); - - std::byte _buffer[Channels][std::max(sizeof(PhaseModulation), sizeof(GroupModulation))]; - Variant _channel[Channels]; - - 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/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() - diff --git a/rims_app/src/power_control.cpp b/rims_app/src/power_control.cpp new file mode 100644 index 00000000000..ab099c8f5b4 --- /dev/null +++ b/rims_app/src/power_control.cpp @@ -0,0 +1,441 @@ +#include "power_control.hpp" + +#include "common.hpp" +#include "log.hpp" +#include "proto/ctrl.pb.h" +#include "zephyr.hpp" +#include "zero_cross_detection.hpp" + +#include +#include +#include +#include +#include +#include +#include + +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} {}; +}; + +struct failed_to_set_power_level : public error { + failed_to_set_power_level() : error{ctrl_Error_PowerLevelFailedToSet} { + } +}; + +} // namespace ctrl + +K_FIFO_DEFINE(ctrlIngress); +K_FIFO_DEFINE(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), + gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_en), gpios) +}; + +PhaseModulation::PhaseModulation(const gpio_dt_spec &gpio) + : PinControlStrategyBase(gpio), // + _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); +} + +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); +} + +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}; + 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 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.start(); + + // // Schedule triac to turn OFF shortly after firing (e.g., 100 us pulse) + _triacTimerStop.setDuration(onDelay + data.to_zcd + minimalGatePulse); + _triacTimerStop.start(); +} + +GroupModulation::GroupModulation(const gpio_dt_spec &gpio) : PinControlStrategyBase(gpio) { + ULOG_INFO("GroupModulation started for gpio %d", gpio.pin); +} + +PhaseModulationOrchestrator::PhaseModulationOrchestrator() : _chPowerControlStrategy{NoModulation{}, NoModulation{}} { + ZeroCrossDetectionEventQueue.k_event_init(_events[0]); + ctrlIngressQueue.k_event_init(_events[1]); +} + +// thread main +void PhaseModulationOrchestrator::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_zeroCrossDetection(); }); + zephyr::event_pool::k_poll_handle(_events[1], [&]() { event_ctrlMessageArrived(); }); + } + } catch (const std::exception &e) { + ULOG_ERROR("EXCEPTION %s", e.what()); + } + } +} + +void PhaseModulationOrchestrator::event_zeroCrossDetection() { + ZeroCrossDetectionEventQueue.try_consume([&](ZeroCrossDetectionEvent &event) { + std::visit([&event](auto &channel) { channel.zeroCrossDetectionTick(event.data); }, _chModeOfOperationStorage[event.channel]); + }); +} + +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, + &PhaseModulationOrchestrator::handler_modeOfOperationRequest, + 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, + &PhaseModulationOrchestrator::handler_groupModulationConfigRequest, + ctrl_GroupModulationConfigResponse ctrl_GroupModulationConfigResponse_init_zero + ); + + constexpr auto groupModulationControlRequestHandler = std::make_tuple( + 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_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_phase_modulation_control_request_tag, + ctrl_EgressMessage_phase_modulation_control_response_tag, + &PhaseModulationOrchestrator::handler_phaseModulationControlRequest, + ctrl_PhaseModulationControlResponse ctrl_PhaseModulationControlResponse_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 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_IngressMessage &req) { + ULOG_INFO("control request message handler"); + ctrlEgressQueue.try_produce([&](ctrl_EgressMessage &resp) { + ULOG_INFO("control response message handler"); + resp.request_id = req.request_id; + resp.has_request_id = true; + + switch (req.which_data) { + 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; + + 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; + } + + ); + }); +} + +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 +) { + ULOG_INFO("ModeOfOperationRequest request handler"); + checkChannel(request.channel_id); + + if (request.has_mode) { + setMode(request.channel_id, 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( // + 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 >= MaxChannels) throw ctrl::wrong_channel{}; +} + +void PhaseModulationOrchestrator::checkCurrentMode(const uint8_t channel, ModeOfOperation moo) { + if (mode(channel) != moo) throw ctrl::wrong_mode{}; +} + +void PhaseModulationOrchestrator::setMode(uint8_t ch, ModeOfOperation mode) { + checkChannel(ch); + + switch (mode) { + case ctrl_ModeOfOperation_Disabled: + _chModeOfOperationStorage[ch].emplace(); + break; + case ctrl_ModeOfOperation_ConstantTemperature: + throw ctrl::unsuported_mode{}; + 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: + throw ctrl::wrong_mode{}; + break; + } +} + +ModeOfOperation PhaseModulationOrchestrator::mode(uint8_t ch) { + checkChannel(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); + + switch (alg) { + case ctrl_PowerControlAlgorithm_NoModulation: + _chPowerControlStrategy[ch].emplace(); + break; + case ctrl_PowerControlAlgorithm_GroupModulation: + _chPowerControlStrategy[ch].emplace(pins[ch]); + break; + case ctrl_PowerControlAlgorithm_PhaseModulation: + _chPowerControlStrategy[ch].emplace(pins[ch]); + break; + default: + break; + } +} + +PowerControlAlgorithm PhaseModulationOrchestrator::powerControlAlgorithm(uint8_t ch) { + checkChannel(ch); + + if (std::holds_alternative(_chPowerControlStrategy.at(ch))) { + return ctrl_PowerControlAlgorithm_GroupModulation; + } + if (std::holds_alternative(_chPowerControlStrategy.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); + return 0; +} + +void PhaseModulationThread::threadMain() { + ULOG_INFO("PhaseModulationThread start"); + /// runs in context of new thread, on new thread stack etc. + PhaseModulationOrchestrator thread{}; + thread.loop(); +} + +} // namespace rims diff --git a/rims_app/src/power_control.hpp b/rims_app/src/power_control.hpp new file mode 100644 index 00000000000..76ecad92263 --- /dev/null +++ b/rims_app/src/power_control.hpp @@ -0,0 +1,333 @@ +#pragma once + +/* Kernel event for notifying other threads */ +#include "common.hpp" +#include "inplace_vector.hpp" +#include "proto/ctrl.pb.h" +#include "zero_cross_detection.hpp" + +#include +#include +#include + +#include + +namespace rims { + +extern fifo_queue ctrlIngressQueue; +extern fifo_queue ctrlEgressQueue; + +using ModeOfOperation = ctrl_ModeOfOperation; +using PowerControlAlgorithm = ctrl_PowerControlAlgorithm; + +using PowerControlAlgorithmRequest = ctrl_PowerControlAlgorithmRequest; +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; +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(ZeroCrossDetectionEvent::Data 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(ZeroCrossDetectionEvent::Data usToNext) override; + + private: + SingleShootTimer _triacTimerStart, _triacTimerStop; +}; + +class GroupModulation : public PinControlStrategyBase { + public: + GroupModulation(const gpio_dt_spec &gpio); + + ~GroupModulation() = default; + + 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) { + _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(ZeroCrossDetectionEvent::Data) override { + } + void on() const override { + } + void off() const override { + } +}; + +class Linked : public PowerControlStrategy { + void zeroCrossDetectionTick(ZeroCrossDetectionEvent::Data) override { + } + void on() const override { + } + void off() const override { + } +}; + +class ModeOfOperationStrategy { + public: + ModeOfOperationStrategy(PowerControlStrategy &pc) : _powerControl{pc} { + } + virtual ~ModeOfOperationStrategy() { + _powerControl.off(); + }; + virtual void zeroCrossDetectionTick(ZeroCrossDetectionEvent::Data usToNext) = 0; + + PowerControlStrategy &_powerControl; +}; + +class DisabledMode : public ModeOfOperationStrategy { + inline static NoModulation _noPowerModulationStrategy{}; + + public: + DisabledMode() : ModeOfOperationStrategy(_noPowerModulationStrategy) { + _powerControl.off(); + }; + void zeroCrossDetectionTick(ZeroCrossDetectionEvent::Data 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); + } + } + + 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: + 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(ZeroCrossDetectionEvent::Data 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_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); + + void setPowerControlAlgorithm(uint8_t ch, PowerControlAlgorithm alg); + PowerControlAlgorithm powerControlAlgorithm(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; }, _chPowerControlStrategy[channel]); + } + 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; + + 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/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/ring_buffer.hpp b/rims_app/src/ring_buffer.hpp new file mode 100644 index 00000000000..c0c343ced5c --- /dev/null +++ b/rims_app/src/ring_buffer.hpp @@ -0,0 +1,456 @@ +#pragma once + +#include "zephyr.hpp" +#include +#include +#include +#include + +#include +#include + +namespace rims { + +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 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 RingBuffer() : _index{N} { + } + + T &push_back(const T &item) { + if (full()) { + pop_front(); + } + + 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_[_index.tail()]))); + destroy_at(_index.increment_tail()); + + return val; + } + + // removes the last (youngest) element + void pop_back() { + __ASSERT_NO_MSG(not empty()); + + destroy_at(_index.decrement_head()); + } + + // removes the first (oldest) element + void pop_front() { + __ASSERT_NO_MSG(not empty()); + + destroy_at(_index.increment_tail()); + } + + // oldest element + const T &front() const { + __ASSERT_NO_MSG(not empty()); + + return *cbegin(); + } + T &front() { + __ASSERT_NO_MSG(not empty()); + + return *begin(); + } + + const T &back() const { + __ASSERT_NO_MSG(not empty()); + + return *std::prev(cend()); + } + T &back() { + __ASSERT_NO_MSG(not empty()); + + return *(std::prev(end())); + } + + constexpr void reset() { + // destroy elements? :| + _index.reset(); + } + + constexpr bool empty() const { + return _index.empty(); + } + + constexpr bool full() const { + return _index.full(); + } + + constexpr std::size_t capacity() const { + return _index.capacity(); + } + + constexpr std::size_t size() const { + return _index.size(); + } + + constexpr std::size_t free() const { + return _index.free(); + } + + // Iterator class for ring_buffer + template class iterator_base { + public: + using iterator_category = std::random_access_iterator_tag; + using value_type = T; + using difference_type = std::ptrdiff_t; + using pointer = std::conditional_t; + using reference = std::conditional_t; + + 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) { + } + + 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_)[current_])); + } + + pointer operator->() const { + return std::addressof(operator*()); + } + + constexpr iterator_t &operator++() { + current_ = (current_ + 1) % N; + // Detect if we have looped over the circular buffer + if (current_ == tail_ && looped_) { + current_ = head_; // Move iterator to end + } + if (current_ == head_) { + looped_ = true; + } + return *this; + } + + constexpr iterator_t operator++(int) { + iterator_t tmp = *this; + ++(*this); + return tmp; + } + + 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; + } + + 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; + } + + constexpr iterator_t &operator+=(const iterator_t &n) { + current_ = (current_ + n.current_) % N; + return *this; + } + + constexpr iterator_t operator+(difference_type n) const { + std::size_t new_index = (current_ + n) % N; + return iterator_t(*buf_, new_index, tail_, head_); + } + + 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: + 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 {buf_, _index.tail(), _index.tail(), _index.head()}; + } + const_iterator begin() const { + return {buf_, _index.tail(), _index.tail(), _index.head()}; + } + + const_iterator cbegin() const { + 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 {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])); + } + T &directy_at(int index) { + return *reinterpret_cast(std::addressof(buf_[index])); + } + + std::array buf_; + RingIndex _index; +}; + + +} // namespace rims diff --git a/rims_app/src/temperature_measurements.cpp b/rims_app/src/temperature_measurements.cpp index c9e1af4ea99..f710bc4df55 100644 --- a/rims_app/src/temperature_measurements.cpp +++ b/rims_app/src/temperature_measurements.cpp @@ -1,308 +1,499 @@ #include "temperature_measurements.hpp" +#include #include #include #include #include #include +#include +#include #include +#include +#include #include #include "common.hpp" #include "log.hpp" -#include "temperature.pb.h" +#include "proto/temperature.pb.h" #include #include +#include "temperature_measurements.hpp" #include "zephyr.hpp" -#define ADC_NODE DT_NODELABEL(adc1) -#define DT_ADC_TEMP_NODELABEL DT_NODELABEL(adc_temp) - 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); -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); +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); + +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() { + 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); +} + +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) { - 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; - } else if constexpr (std::is_same_v) { + 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; }); } - -void TemperatureSampler::take_sample() { - _samples.emplace_front(adc_take_sample()); +} // namespace +TemperatureSampler::TemperatureSampler(uint8_t channel) : _channel{channel} { + calibration(); + _broadcastTimer.start(); + _samplerTimer.start(); } -TemperatureStatistics TemperatureSampler::temperature_statistics() const { - const auto &samples = this->_samples; +void TemperatureSampler::take_sample() { + auto adc = adcWithCalibration(); + _samples.push_back(adc); +} - 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::broadcastTemperature() const { + PB_encode_egress(temperature_statistics()); +} + +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(disableLog); + + select_rmax(); + 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 + ); + + _adc_Tmax = new_adc_Tmax; + _adc_Tmin = new_adc_Tmin; + + select_rpt(); +} + +void TemperatureSampler::tick() { + // executed every 1ms by the orchestrator + 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 { + 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_temperature_summary = true; + + 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; + } + + // Use only the last N samples or all if fewer + std::size_t count = std::min(this->_samplesNumber, total_samples); + auto start_it = samples.cend() - count; + + auto sum = std::accumulate(start_it, samples.cend(), uint32_t{}); + auto mean = sum / count; + + auto sq_sum = std::accumulate(start_it, samples.cend(), uint32_t{}, [&](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; - temp.channel_id = _channel; + BroadcastTemperatureStatistics btemp; + btemp.channel_id = this->_channel; + btemp.has_temperature_summary = true; - return temp; -}; + 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; -TemperatureCurrent TemperatureSampler::temperature() const { - return TemperatureCurrent{.channel_id = _channel, .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); + return btemp; } -TemperatureSampler::Sample TemperatureSampler::adc_take_sample() 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); +TemperatureCurrent TemperatureSampler::temperature() const { + /// TODO rise error on empty buffer? + if (_samples.empty()) return TemperatureCurrent{.temp_c = 0}; + return TemperatureCurrent{.temp_c = adcToTemperature(_samples.back())}; +} - const adc_sequence_options options = { - .interval_us = 0, - .callback = nullptr, - .user_data = nullptr, - .extra_samplings = 3, +void TemperatureSampler::setSamplerConfig(const temperature_SamplerConfig &config) noexcept(false) { + 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 sampling period from %lld to %d ms", + std::chrono::duration_cast(_samplerTimer.interval()).count(), + config.period_ms + ); + this->_samplerTimer.setDuration(std::chrono::milliseconds{config.period_ms}); + } +} + +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 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) * adc2CelciusWeight(); + + return temperature; +} + +float TemperatureSampler::adc2CelciusWeight() 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::adcWithCalibration() { + calibration(true); + return takeStableRead(true); +} + +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, }; - // 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); 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 to_temperature(avg); + return avg; } template constexpr bool unknownChannelError(const Req &req, Resp &resp) { if (req.channel_id > ChannelNumber) { - resp.data.error = temperature_Error_UnknownChannel; + resp.channel_id = req.channel_id; + + resp.error = temperature_Error_UnknownChannel; + resp.has_error = true; return true; } return false; } TemperatureSamplerOrchestrator::TemperatureSamplerOrchestrator() - : // - _current_channel{0}, // - _channel{TemperatureSampler{1}, TemperatureSampler{2}}, // - _samplerSem{0, 1}, // - _samplerTimer{_samplerSem, std::chrono::milliseconds{125}}, // - _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}} - } { + : // + _temperatureSamplerChannels{TemperatureSampler{1}, TemperatureSampler{2}} // +{ 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); }; void TemperatureSamplerOrchestrator::loop() { - ULOG_INFO("Starting TemperatureSampler loop"); - - _samplerTimer.start(); - _broadcastTimer[0].start(); - _broadcastTimer[1].start(); + ULOG_INFO("Starting TemperatureSamplerOrchestrator loop"); + _samplerTick.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_samplersTick(); }); + } + } catch (const std::exception &e) { + ULOG_ERROR("EXCEPTION %s", e.what()); } } } void TemperatureSamplerOrchestrator::event_messageArrived() { - temperatureEgressQueue.try_produce([&](temperature_EgressMessages &response) { - ULOG_INFO("Producing temperature_EgressMessages response"); - auto ok = temperatureIngressQueue.try_consume([&](temperature_IngressMessages &request) { - 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 + 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_IngressMessage_sampler_config_request_tag, + temperature_EgressMessage_sampler_config_response_tag, + &TemperatureSamplerOrchestrator::handle_samplerConfigRequest + ); + + constexpr auto filterConfigHandler = std::make_tuple( + temperature_IngressMessage_filter_config_request_tag, + temperature_EgressMessage_filter_config_response_tag, + &TemperatureSamplerOrchestrator::handle_filterConfigRequest + ); + + constexpr auto filterHandler = std::make_tuple( + temperature_IngressMessage_filter_request_tag, + temperature_EgressMessage_filter_response_tag, + &TemperatureSamplerOrchestrator::handle_filterRequest + ); + + 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_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>(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>(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 temperature_IngressMessages_temperatureRequest_tag: - response.which_data = temperature_EgressMessages_temperatureResponse_tag; - handle_getTemperatureRequest( // - request.data.temperatureRequest, - response.data.temperatureResponse - ); + case std::get<1>(samplerConfigHandler): + resp.which_data = std::get<1>(samplerConfigHandler); + genericHandler(req.data.sampler_config_request, resp.data.sampler_config_response, samplerConfigHandler); 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>(filterConfigHandler): + resp.which_data = std::get<1>(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.filter_request, resp.data.filter_response, filterHandler); 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"); + 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.data.temperatureStats = _channel[req.channel_id].temperature_statistics(); -} - -void TemperatureSamplerOrchestrator::handle_getCurrentTemperatureRequest( - const GetCurrentTemperatureRequest &req, - GetCurrentTemperatureResponse &resp -) const { - if (unknownChannelError(req, resp)) return; - - resp.data.temperatureCurrent = _channel[req.channel_id].temperature(); + resp.has_temperature_current = true; + resp.temperature_current = _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 + } + + resp.has_config = true; + resp.config = _temperatureSamplerChannels[req.channel_id].samplerConfig(); +} + +void TemperatureSamplerOrchestrator::handle_filterConfigRequest( // + const FilterConfigRequest &req, + FilterConfigResponse &resp +) const { + if (unknownChannelError(req, resp)) return; + + if (req.has_ema_filter_params) { + /// TODO set + } + + if (req.has_kalman_pilter_params) { + /// TODO set + } + + if (req.has_average_filter_params) { + /// TODO set + } + + if (not req.skip_full_response) { } } -void TemperatureSamplerOrchestrator::action_takeSample() { - zephyr::semaphore::k_sem_take_now(_samplerSem); - _current_channel = (_current_channel + 1) % channelNumber; - _channel[_current_channel].take_sample(); - /// TODO change channel +void TemperatureSamplerOrchestrator::handle_filterRequest( // + const FilterRequest &req, + FilterResponse &resp +) const { + if (unknownChannelError(req, resp)) return; } -void TemperatureSamplerOrchestrator::action_sendTemperature(uint8_t ch) { - zephyr::semaphore::k_sem_take_now(_broadcastSem[ch]); - PB_encode_egress(_channel[ch].temperature_statistics()); +void TemperatureSamplerOrchestrator::action_samplersTick() { + zephyr::semaphore::k_sem_take_now(_samplerTickSem); + for (auto &sampler : _temperatureSamplerChannels) { + sampler.tick(); + } } 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); + 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); - 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); - - return ret; + zephyr::adc::channel_setup(adc, adc_temp); + return 0; } void TemperatureSamplerThread::threadMain() { @@ -312,4 +503,38 @@ void TemperatureSamplerThread::threadMain() { thread.loop(); } +uint32_t TemperatureSampler::takeStableRead(bool disableLog) { + const auto start = std::chrono::steady_clock::now(); + if (not disableLog) ULOG_INFO("Starting ADC stable read"); + + int maxIterations = 100; + const int STABILITY_COUNT_REQUIRED = 6; + const int STABILITY_THRESHOLD = 5; + + int prevAdc = 0; + int stableCount = 0; + + while (stableCount < STABILITY_COUNT_REQUIRED && maxIterations > 0) { + // 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) { + stableCount++; + } else { + stableCount = 0; + } + + prevAdc = adcraw; + maxIterations--; + } + + const auto stop = std::chrono::steady_clock::now(); + auto ms = (stop - start).count() / 1000 / 1000; + auto itleft = (100 - maxIterations); + if (not disableLog) 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 c4d00de66d0..c24590357a7 100644 --- a/rims_app/src/temperature_measurements.hpp +++ b/rims_app/src/temperature_measurements.hpp @@ -1,9 +1,9 @@ #pragma once -#include "circular_buffer.hpp" +#include "fifo_queue.hpp" #include "common.hpp" -#include "temperature.pb.h" +#include "proto/temperature.pb.h" #include "zephyr.hpp" #include @@ -13,27 +13,34 @@ 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 = 16; -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 TemperatureStatistics = temperature_TemperatureStatistics; -using TemperatureCurrent = temperature_TemperatureCurrent; +using FilterConfigRequest = temperature_FilterConfigRequest; +using FilterConfigResponse = temperature_FilterConfigResponse; +using FilterRequest = temperature_FilterRequest; +using FilterResponse = temperature_FilterResponse; + +using SampleConfig = temperature_SamplerConfig; +using TemperatureSummary = temperature_TemperatureSummary; +using TemperatureCurrent = temperature_TemperatureCurrent; +using BroadcastTemperatureStatistics = temperature_BroadcastTemperatureSummary; 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; @@ -41,16 +48,45 @@ class TemperatureSampler { TemperatureSampler(TemperatureSampler &&) = delete; TemperatureSampler &operator=(TemperatureSampler &&) = delete; - void take_sample(); + void tick(); + void calibration(bool disableLog = false); - TemperatureStatistics temperature_statistics() const; - TemperatureCurrent temperature() const; + void setSamplerConfig(const SampleConfig &config) noexcept(false); + SampleConfig samplerConfig() const; + + BroadcastTemperatureStatistics temperature_statistics() const; + TemperatureCurrent temperature() const; protected: - Sample to_temperature(int adc_value) const; - Sample adc_take_sample() const; + bool _running{false}; - circular_buffer _samples; + void take_sample(); + void broadcastTemperature() const; + + uint32_t takeStableRead(bool disableLog = false); + + float adc2CelciusWeight() const; + Sample adcToTemperature(uint32_t adc_value) const; + + adc_sample adcWithCalibration(); + adc_sample adc() 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{_samplerSem, std::chrono::milliseconds{100}}; + + zephyr::semaphore::sem _broadcastSem{0, 1}; + RecurringSemaphoreTimer _broadcastTimer{_broadcastSem, std::chrono::seconds{60}}; + + RingBuffer _samples; + // 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{}; }; @@ -65,28 +101,25 @@ 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) 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_takeSample(); - void action_sendTemperature(uint8_t ch); + void action_samplersTick(); - uint8_t _current_channel{0}; - std::array _channel; + std::array _temperatureSamplerChannels; - zephyr::semaphore::sem _samplerSem{0, 1}; - RecurringSemaphoreTimer _samplerTimer; + zephyr::semaphore::sem _samplerTickSem{0, 1}; + RecurringSemaphoreTimer _samplerTick{_samplerTickSem, std::chrono::milliseconds{1}}; - std::array _broadcastSem; - std::array _broadcastTimer; - - std::array _events; // event from timer and from Messenger + std::array _events; // event from timer and from Messenger }; class TemperatureSamplerThread : public ZephyrThread { public: - TemperatureSamplerThread(TStackBase &stack) : ZephyrThread(stack, 10, 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 c1b614fa4ea..4ec064887fb 100644 --- a/rims_app/src/uart.cpp +++ b/rims_app/src/uart.cpp @@ -1,8 +1,12 @@ #include "uart.hpp" #include "log.hpp" -#include "message_decode.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 @@ -56,49 +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 } } // 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) { +#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"); - bool first = true; - for (auto byte : bytes) { - if (first) { - if (dev->tx_buffer.empty()) { - dev->tx_buffer.emplace_front((uint8_t)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((uint8_t)byte); + 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) { @@ -108,12 +136,14 @@ void AsyncUART::uartCallback(const device *dev, void *user_data) { void AsyncUART::uartISR() { try { - while (uart_irq_update(_dev) && uart_irq_is_pending(_dev)) { - if (rxHasByte()) { + if (uart_irq_update(_dev)) { + bool rxready = uart_irq_rx_ready(_dev); + if (rxready) { readByteUart(); } - if (uart_irq_tx_ready(_dev)) { - putByteUart(); + bool txready = uart_irq_tx_ready(_dev); + if (txready) { + writeByteUart(); } } @@ -128,21 +158,27 @@ void AsyncUART::uartISR() { } } -bool AsyncUART::rxHasByte() const { - return uart_irq_rx_ready(_dev); -} - void AsyncUART::readByteUart() { - if (faultFlag) { - if (rxByte() == 0) faultFlag = false; - } - // throw on buffer overflow - else if (tx_buffer.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(); + 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().clear(); + _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().clear(); + } + } } } @@ -160,9 +196,9 @@ uint8_t AsyncUART::rxByte() { } void AsyncUART::processMessage() { - buffer buf{.data = reinterpret_cast(rxBuffer().data()), .size = rxBuffer().size(), .device = this}; + BufferView 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() { @@ -171,28 +207,24 @@ AsyncUART::rx_buffer_t &AsyncUART::rxBuffer() { void AsyncUART::switchRxBuffer() { _currentBufferIndex = _currentBufferIndex == 1 ? 0 : 1; - rxBuffer().clean(); + rxBuffer().clear(); } 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) { @@ -202,10 +234,10 @@ 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; + _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 8da04004cf1..54a8a498cb4 100644 --- a/rims_app/src/uart.hpp +++ b/rims_app/src/uart.hpp @@ -1,9 +1,9 @@ #pragma once -#include "circular_buffer.hpp" +#include "ring_buffer.hpp" #include "common.hpp" +#include "inplace_vector.hpp" -#include #include #include #include @@ -18,28 +18,30 @@ class uart_rx_buffer_overflow; class uart_rx_not_ready_error; class AsyncUART { - using rx_buffer_t = static_vector; - using tx_buffer_t = circular_buffer; + using rx_buffer_t = beman::inplace_vector; + using tx_buffer_t = RingBuffer; public: AsyncUART(); void loop(); - static void transmit(AsyncUART *dev, std::span bytes); - static void workHandler(k_work *work); + static void transmit(AsyncUART *dev, std::span bytes); static void uartCallback(const struct device *dev, void *user_data); - - void uartISR(); - const device *_dev; + + void uartISR(); private: + const device *_dev; 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 +49,34 @@ 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 + + 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); @@ -59,7 +86,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: diff --git a/rims_app/src/zephyr.cpp b/rims_app/src/zephyr.cpp index dcbe1a48b7b..80a22be1f35 100644 --- a/rims_app/src/zephyr.cpp +++ b/rims_app/src/zephyr.cpp @@ -1,13 +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) { - if (gpio_pin_configure_dt(&pin, flags) != 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 99077a6082c..9ea3533288e 100644 --- a/rims_app/src/zephyr.hpp +++ b/rims_app/src/zephyr.hpp @@ -6,6 +6,7 @@ #include #include +#include #include #include #include @@ -24,6 +25,41 @@ 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 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{}; + } +} + +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) { @@ -48,7 +84,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)); } @@ -81,7 +116,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) { @@ -89,15 +124,27 @@ 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::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 { @@ -105,5 +152,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 d47838e66bf..f2320c15b7a 100644 --- a/rims_app/src/zero_cross_detection.cpp +++ b/rims_app/src/zero_cross_detection.cpp @@ -1,72 +1,67 @@ #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(zcd_state_1), gpios); -gpio_dt_spec ch2ZCD_pin_spec = GPIO_DT_SPEC_GET(DT_NODELABEL(zcd_state_2), 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}; +fifo_queue 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); + if (pin_state == 0) { + // calculate true zero based on statistical data + _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)); + _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(); + _pulseDown = clock::now(); - 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"); + 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,11 +71,10 @@ 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));} ); - 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)); }); } } } @@ -90,14 +84,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 +103,23 @@ 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..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 "circular_buffer.hpp" #include "common.hpp" +#include "fifo_queue.hpp" #include "zephyr.hpp" #include @@ -11,16 +11,27 @@ namespace rims { struct ZeroCrossDetectionEvent { - bool state : 1; - int channel : 4; // max 2^16 channels + 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; }; -extern zephyr_fifo_buffer ZeroCrossDetectionEventQueue; -using ZCDFifo_t = zephyr_fifo_buffer; +extern fifo_queue ZeroCrossDetectionEventQueue; +using ZCDFifo_t = fifo_queue; 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,24 +39,34 @@ 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::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); diff --git a/rims_app/temperature.proto b/rims_app/temperature.proto deleted file mode 100644 index f67d9abbc8d..00000000000 --- a/rims_app/temperature.proto +++ /dev/null @@ -1,124 +0,0 @@ -syntax = "proto3"; - -package temperature; - -enum Error { - NoError = 0; - UnknownChannel = 1; - TemperatureSensorDisconnected = 2; - TemperatureSensorBroken = 3; -} - -/* CONFIGURATION */ -message SamplerConfig -{ - optional uint32 samples = 3; // optional, if ommited return current value - optional uint32 period_ms = 4; // optional -} - -/* INTERNAL STRUCTURES */ - -// Last temp sample -message TemperatureCurrent -{ - uint32 channel_id = 1; - - // last update - float temp_c = 2; -} - -// Statictics of temperature on given channel -message TemperatureStatistics -{ - uint32 channel_id = 1; - - // last update - float temp_c = 2; - - // avarage from last nsamples - float temp_avg_c = 3; - - // standard deviation of temperature on given channel from last nsamples - float temp_stddev_c = 4; - - // samples taken - uint32 n_samples = 5; -} - -/* CONFIG API */ - -message SamplerConfigRequest -{ - uint32 channel_id = 1; - SamplerConfig config = 2; -} - -message SamplerConfigResponse -{ - oneof data - { - SamplerConfig config = 2; - Error error = 254; - } -} - -// API -message GetCurrentTemperatureRequest -{ - uint32 channel_id = 1; -}; - -message GetCurrentTemperatureResponse -{ - oneof data - { - TemperatureCurrent temperatureCurrent = 1; - Error error = 254; - } -} - -message GetTemperatureRequest -{ - uint32 channel_id = 1; -}; - -message GetTemperatureResponse -{ - oneof data - { - TemperatureStatistics temperatureStats = 1; - Error error = 254; - } -} - -// only those messages are send through wire at temperature endpoint -message IngressMessages -{ - uint32 requestID = 255; - oneof data - { - // data access - GetCurrentTemperatureRequest currentTemperatureRequest = 1; - GetTemperatureRequest temperatureRequest = 2; - - // configuration - SamplerConfigRequest samplerConfigRequest = 3; - } -}; - -message EgressMessages -{ - optional uint32 requestID = 255; - oneof data - { - // data access - GetCurrentTemperatureResponse currentTemperatureResponse = 1; - GetTemperatureResponse temperatureResponse = 2; - - // configuration - SamplerConfigResponse samplerConfigResponse = 3; - - // broadcast - TemperatureStatistics broadcastTemperatureStatistics = 16; - } -};