bartoszek/zephyr
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

exceptions #1

Merged
bartoszek merged 18 commits from exceptions into main 2025-05-06 09:39:21 +00:00
Conversation 0 Commits 18 Files Changed 54 +4848 -2077
54 changed files with 4848 additions and 2077 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -12,6 +12,7 @@
\#*\#
build*/
config_default*/
!doc/build/
!scripts/build
!tests/drivers/build_all

125
boards/bartoszek/rims/rims_h503cbt.dts
View File

@ -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 = <ADC_ACQ_TIME_DEFAULT>;
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 = <ADC_ACQ_TIME_DEFAULT>;
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 = <ADC_ACQ_TIME_DEFAULT>;
@ -143,10 +173,14 @@
status = "okay";
};
&gpioc{
status = "okay";
};
&clk_csi {
status = "okay";
};
// 240886428
&clk_hsi { /* 64MHz, ok*/
clock-frequency = <DT_FREQ_M(64)>;
status = "okay";
@ -157,22 +191,23 @@
};
&clk_hse {
status = "disabled";
status = "okay";
clock-frequency = <DT_FREQ_M(16)>;
};
&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 = <DT_FREQ_M(240)>;
clocks = <&pll1>;
clock-frequency = <DT_FREQ_M(160)>;
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";

3
boards/bartoszek/rims/rims_h503cbt_defconfig
View File

@ -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

15
rims_app/CMakeLists.txt
View File

@ -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})

44
rims_app/CMakeLists.txt.user
View File

@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 15.0.1, 2025-02-26T20:08:18. -->
<!-- Written by QtCreator 15.0.1, 2025-04-22T11:16:22. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
@ -108,14 +108,14 @@
<value type="int" key="CMake.Configure.BaseEnvironment">2</value>
<value type="bool" key="CMake.Configure.ClearSystemEnvironment">false</value>
<valuelist type="QVariantList" key="CMake.Configure.UserEnvironmentChanges"/>
<value type="QString" key="CMake.Initial.Parameters">-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
<value type="QString" key="CMake.Initial.Parameters">-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}</value>
-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</value>
<value type="QString" key="CMake.Source.Directory">/home/bartoszek/zephyrproject/zephyr/rims_app</value>
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/bartoszek/zephyrproject/zephyr/rims_app/build</value>
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
@ -219,14 +219,36 @@
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey"></value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">zephyr_final</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeRunConfiguration.</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey">zephyr_final</value>
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
<value type="bool" key="RunConfiguration.UseLibrarySearchPath">true</value>
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
<value type="QString" key="RunConfiguration.WorkingDirectory.default">/home/bartoszek/zephyrproject/zephyr/rims_app/build/zephyr</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.1">
<value type="bool" key="Analyzer.Perf.Settings.UseGlobalSettings">true</value>
<value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
<value type="int" key="Analyzer.Valgrind.Callgrind.CostFormat">0</value>
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
<value type="QList&lt;int&gt;" key="Analyzer.Valgrind.VisibleErrorKinds"></value>
<valuelist type="QVariantList" key="CustomOutputParsers"/>
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
<value type="bool" key="PE.EnvironmentAspect.PrintOnRun">false</value>
<value type="QString" key="PerfRecordArgsId">-e cpu-cycles --call-graph dwarf,4096 -F 250</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">zephyr_pre0</value>
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeRunConfiguration.</value>
<value type="QString" key="ProjectExplorer.RunConfiguration.BuildKey">zephyr_pre0</value>
<value type="bool" key="ProjectExplorer.RunConfiguration.Customized">false</value>
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
<value type="bool" key="RunConfiguration.UseLibrarySearchPath">true</value>
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
<value type="QString" key="RunConfiguration.WorkingDirectory.default">/home/bartoszek/zephyrproject/zephyr/rims_app/build/zephyr</value>
</valuemap>
<value type="qlonglong" key="ProjectExplorer.Target.RunConfigurationCount">2</value>
</valuemap>
</data>
<data>

18
rims_app/can.cpp
View File

@ -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:

64
rims_app/configuration.proto
View File

@ -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;
}
};

67
rims_app/ctrl.proto
View File

@ -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
}
};

45
rims_app/prj.conf
View File

@ -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

2
rims_app/common.proto → rims_app/proto/common.proto
View File

@ -1,3 +1,5 @@
syntax = "proto3";
/*
Request/Response Architecture

19
rims_app/proto/configuration.proto Normal file
View File

@ -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;
};

252
rims_app/proto/ctrl.proto Normal file
View File

@ -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
}
};

46
rims_app/proto/gpio.proto Normal file
View File

@ -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;
}
};

8
rims_app/log.proto → rims_app/proto/log.proto
View File

@ -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;

10
rims_app/message.proto → rims_app/proto/message.proto
View File

@ -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

207
rims_app/proto/temperature.proto Normal file
View File

@ -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;
}
};

11
rims_app/utilization.proto → rims_app/proto/utilization.proto
View File

@ -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

0
rims_app/src/app.hpp
View File

404
rims_app/src/circular_buffer.hpp
View File

@ -1,404 +0,0 @@
#pragma once
#include "zephyr.hpp"
#include "zephyr/sys/__assert.h"
#include <array>
#include <cstddef>
#include <cstring>
#include <mutex>
#include <utility>
namespace rims {
// template <typename T> 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 <typename T, std::size_t Capacity> class static_vector {
private:
std::array<T, Capacity> 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 T, std::size_t N> 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<T *>(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<T *>(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<std::byte[sizeof(T)], N> &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<T *>(std::addressof(buf_[index_]));
}
pointer operator->() {
return reinterpret_cast<T *>(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<std::byte[sizeof(T)], N> &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<std::byte[sizeof(T)], N> &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<const T *>(std::addressof(buf_[index_]));
}
pointer operator->() const {
return reinterpret_cast<const T *>(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<std::byte[sizeof(T)], N> &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<const T *>(std::addressof(buf_[index]));
}
T &directy_at(int index) {
return *reinterpret_cast<T *>(std::addressof(buf_[index]));
}
// std::mutex mutex_;
std::array<std::byte[sizeof(T)], N> 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 <typename T, size_t N> class thread_safe_circular_buffer : public circular_buffer<T, N> {
using base = circular_buffer<T, N>;
public:
const T &back() const {
const std::lock_guard<ZephyrMutex> _lock{_mutex};
return base::back();
};
T &back() {
std::lock_guard<ZephyrMutex> _lock{_mutex};
return base::back();
};
T &emplace_back(T &&item) {
std::lock_guard<ZephyrMutex> _lock{_mutex};
return base::emplace_back(std::forward<T>(item));
};
void pop_back() {
std::lock_guard<ZephyrMutex> _lock{_mutex};
return base::pop_back();
};
private:
mutable ZephyrMutex _mutex;
};
template <typename T> struct ZephyrFifoElement {
void *_reserved;
T item;
};
template <typename T, size_t N> 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 <typename Fn> //
bool try_consume(Fn fn) {
// std::lock_guard<ZephyrMutex> _lock{_mutex};
if (_elements.empty()) return false;
/// read from queue
ZephyrFifoElement<T> *el = reinterpret_cast<ZephyrFifoElement<T> *>(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 <typename Fn> //
bool try_produce(Fn fn) {
// std::lock_guard<ZephyrMutex> _lock{_mutex};
if (_elements.full()) return false;
auto &el = _elements.emplace_front(ZephyrFifoElement<T>{}); // 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<ZephyrFifoElement<T>, N> _elements{};
k_fifo &_fifo;
};
} // namespace rims

117
rims_app/src/common.hpp
View File

@ -1,14 +1,10 @@
#pragma once
#include "log.hpp"
#include "syscalls/kernel.h"
#include "zephyr/sys_clock.h"
#include "function.hpp"
#include <chrono>
#include <cstddef>
#include <cstring>
#include <functional>
#include <optional>
#include <ratio>
#include <string_view>
@ -16,15 +12,43 @@
#include <zephyr/devicetree.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>
#include <zephyr/sys/time_units.h>
#include <zephyr/sys_clock.h>
#include <zephyr/drivers/adc.h>
#include <zephyr/drivers/gpio.h>
namespace rims {
// Primary template (for general case)
template <std::size_t N, typename T> struct NthArgument;
// Specialization for normal functions
template <std::size_t N, typename R, typename... Args> struct NthArgument<N, R(Args...)> {
using type = std::tuple_element_t<N, std::tuple<Args...>>;
};
// Specialization for member functions (non-const)
template <std::size_t N, typename R, typename C, typename... Args> struct NthArgument<N, R (C::*)(Args...)> {
using type = std::tuple_element_t<N, std::tuple<Args...>>;
};
// Specialization for member functions (const)
template <std::size_t N, typename R, typename C, typename... Args> struct NthArgument<N, R (C::*)(Args...) const> {
using type = std::tuple_element_t<N, std::tuple<Args...>>;
};
// Helper alias template
template <std::size_t N, typename T> using NthArgument_t = typename NthArgument<N, T>::type;
using clock = std::chrono::steady_clock;
using time_point = clock::time_point;
using microseconds_u16_t = std::chrono::duration<uint16_t, std::micro>; // max ~65ms
using microseconds_u32_t = std::chrono::duration<uint32_t, std::micro>; // 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<std::chrono::microseconds> period = std::nullopt)
: _interval(chronoToKTimeout(interval)), _period{period.has_value() ? chronoToKTimeout(*period) : K_NO_WAIT} {
Timer(std::chrono::microseconds duration, std::optional<std::chrono::microseconds> 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<void()> cb, std::chrono::milliseconds interval) : Timer(interval), _cb{std::move(cb)} {
SingleShootTimer(rims::function<void()> 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<void()> _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<std::chrono::microseconds>(target - now);
_duration = chronoToKTimeout(delay);
}
_period = K_NO_WAIT; // Ensure it's a one-shot
start();
}
rims::function<void()> _cb;
};
// class RecurringTimer : public Timer {
// public:
// RecurringTimer(std::function<void()> cb, std::chrono::milliseconds interval) : Timer(interval, interval), _cb{std::move(cb)} {
// }
// void expiry_cb() override {
// _cb();
// }
// std::function<void()> _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<uint16_t, std::micro>; // max ~65ms
using adc_sample = uint16_t;
} // namespace rims

6
rims_app/src/config.cpp
View File

@ -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<config_IngressMessages, 2> configIngressQueue{configIngress};
zephyr_fifo_buffer<config_EgressMessages, 2> configEgressQueue{configEgress};
fifo_queue<config_IngressMessage, 2> configIngressQueue{configIngress};
fifo_queue<config_EgressMessage, 2> configEgressQueue{configEgress};
} // namespace rims

12
rims_app/src/config.hpp
View File

@ -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<config_IngressMessages,2 > configIngressQueue;
extern zephyr_fifo_buffer<config_EgressMessages, 2> configEgressQueue;
extern fifo_queue<config_IngressMessage, 2> configIngressQueue;
extern fifo_queue<config_EgressMessage, 2> configEgressQueue;
}
} // namespace rims

75
rims_app/src/details/function.hpp Normal file
View File

@ -0,0 +1,75 @@
#pragma once
#include <cstring>
#include <utility>
namespace rims::detail {
template <typename F> struct functor {
F lambda;
static functor *cast(void *storage) noexcept {
return static_cast<functor *>(storage);
}
static const functor *cast(const void *storage) noexcept {
return static_cast<const functor *>(storage);
}
template <typename R, typename... A> static R call(const void *self, A &&...args) {
return cast(self)->lambda(std::forward<A>(args)...);
}
static void destroy(void *self) {
cast(self)->~functor();
}
static void move(void *to, void *from) {
new (to) functor{static_cast<functor &&>(*cast(from))};
destroy(from);
}
static void copy(void *to, const void *from) {
new (to) functor{static_cast<const functor &>(*cast(from))};
}
};
struct functor_vtable {
void (*destroy)(void *);
void (*move)(void *, void *);
void (*copy)(void *, const void *);
template <typename F> static const functor_vtable *create() noexcept {
static constexpr functor_vtable vtable{functor<F>::destroy, functor<F>::move, functor<F>::copy};
return &vtable;
}
template <std::size_t N> static const functor_vtable *trivial() noexcept {
static constexpr functor_vtable vtable{trivial_destroy, trivial_move<N>, trivial_copy<N>};
return &vtable;
}
private:
static void trivial_destroy(void *) {
}
template <std::size_t N> static void trivial_move(void *to, void *from) {
trivial_copy<N>(to, from);
}
template <std::size_t N> static void trivial_copy(void *to, const void *from) {
std::memcpy(to, from, N);
}
};
template <typename T> inline constexpr bool is_function_instance = false;
template <typename> struct member_function_signature {};
template <typename R, typename T, bool Nx, typename... A> struct member_function_signature<R (T::*)(A...) noexcept(Nx)> {
using type = R(A...);
};
template <typename R, typename T, bool Nx, typename... A> struct member_function_signature<R (T::*)(A...) & noexcept(Nx)> {
using type = R(A...);
};
template <typename R, typename T, bool Nx, typename... A> struct member_function_signature<R (T::*)(A...) const noexcept(Nx)> {
using type = R(A...);
};
template <typename R, typename T, bool Nx, typename... A> struct member_function_signature<R (T::*)(A...) const & noexcept(Nx)> {
using type = R(A...);
};
} // namespace jw::detail

65
rims_app/src/fifo_queue.hpp Normal file
View File

@ -0,0 +1,65 @@
#pragma once
#include "ring_buffer.hpp"
#include <zephyr/kernel.h>
namespace rims {
template <typename T> struct ZephyrFifoElement {
void *_reserved;
T item;
};
template <typename T, size_t N> class fifo_queue {
public:
// static_assert(std::is_trivial_v<T>);
fifo_queue(k_fifo &fifo) : _fifo{fifo} {
}
void k_event_init(k_poll_event &event) {
zephyr::event_pool::k_init(event, _fifo);
}
template <typename Fn> //
bool try_consume(Fn fn) {
// std::lock_guard<ZephyrMutex> _lock{_mutex};
if (_elements.empty()) return false;
/// read from queue
ZephyrFifoElement<T> *el = reinterpret_cast<ZephyrFifoElement<T> *>(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 <typename Fn> //
bool try_produce(Fn fn) {
// std::lock_guard<ZephyrMutex> _lock{_mutex};
if (_elements.full()) return false;
auto tmp = ZephyrFifoElement<T>{};
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<ZephyrFifoElement<T>, N> _elements{};
k_fifo &_fifo;
};
} // namespace rims

249
rims_app/src/function.hpp
View File

@ -1,81 +1,198 @@
#pragma once
#include <cassert>
#include <cstddef>
#include "details/function.hpp"
#include <cstring>
#include <functional>
#include <tuple>
#include <type_traits>
#include <utility>
#include <cstring> // For std::memset
namespace rims{
template <typename>
class Function;
namespace rims {
template <typename, unsigned = 1> struct trivial_function;
template <typename, unsigned = 1> 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 <typename R, typename... A, unsigned N> struct trivial_function<R(A...), N> {
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 <typename T, unsigned M> trivial_function(function<T, M> &&) = delete;
template <typename T, unsigned M> trivial_function(const function<T, M> &) = delete;
constexpr trivial_function(std::nullptr_t) noexcept : trivial_function{} {
}
template <typename F>
requires(not detail::is_function_instance<std::remove_cvref_t<F>>)
explicit trivial_function(F &&func) : trivial_function{create(std::forward<F>(func))} {
}
template <typename F> trivial_function &operator=(F &&func) noexcept {
return *this = trivial_function{std::forward<F>(func)};
}
template <unsigned M>
requires(M < N)
trivial_function(const trivial_function<R(A...), M> &other) noexcept : call{other.call} {
std::memcpy(storage, &other.storage, sizeof(other.storage));
}
R operator()(A... args) const {
return call(&storage, std::forward<A>(args)...);
}
constexpr bool valid() const noexcept {
return call != nullptr;
}
explicit constexpr operator bool() const noexcept {
return valid();
}
template <typename Ret, typename... Args>
class Function<Ret(Args...)> {
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 <typename Callable>
static Ret invokeImpl(void* callable, Args&&... args) {
return (*reinterpret_cast<Callable*>(callable))(std::forward<Args>(args)...);
template <typename F> static trivial_function create(F &&func) {
using functor = detail::functor<std::remove_cvref_t<F>>;
static_assert(std::is_trivially_destructible_v<functor>);
static_assert(sizeof(functor) <= sizeof(dummy));
static_assert(alignof(functor) <= alignof(dummy));
trivial_function f;
new (&f.storage) functor{std::forward<F>(func)};
f.call = functor::template call<R, A...>;
return f;
}
template <typename Callable>
void storeCallable(Callable&& callable) {
using CallableType = std::decay_t<Callable>;
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>(callable));
invokeFn = &invokeImpl<CallableType>;
template <typename, unsigned> friend struct trivial_function;
template <typename, unsigned> friend struct function;
using dummy = detail::functor<decltype([x = std::declval<std::array<void *, N>>()](A...) {})>;
union {
struct {
} nothing{};
alignas(dummy) std::byte storage[sizeof(dummy)];
};
R (*call)(const void *, A &&...){nullptr};
};
template <typename F, typename Signature = typename detail::member_function_signature<decltype(&F::operator())>::type>
trivial_function(F) -> trivial_function<Signature, (sizeof(F) - 1) / sizeof(void *) + 1>;
// 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 <typename R, typename... A, unsigned N> struct function<R(A...), N> {
constexpr function() noexcept = default;
constexpr ~function() {
if (call != nullptr) vtable->destroy(&storage);
}
public:
Function() = default;
template <typename Callable>
Function(Callable&& callable) {
storeCallable(std::forward<Callable>(callable));
template <typename F> function &operator=(F &&func) {
return assign(std::forward<F>(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 <typename F>
requires(not detail::is_function_instance<std::remove_cvref_t<F>>)
explicit function(F &&func) : function{create(std::forward<F>(func))} {
}
~Function() {
if (invokeFn) {
reinterpret_cast<void(*)(void*)>(buffer)(buffer);
}
template <unsigned M>
requires(M <= N)
function(function<R(A...), M> &&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<void*>(reinterpret_cast<const void*>(buffer)), std::forward<Args>(args)...);
template <unsigned M>
requires(M <= N)
function(const function<R(A...), M> &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 <unsigned M>
requires(M <= N)
function(const trivial_function<R(A...), M> &other) noexcept
: vtable{detail::functor_vtable::trivial<sizeof(other.storage)>()}, call{other.call} {
std::memcpy(&storage, &other.storage, sizeof(storage));
}
};}
R operator()(A... args) const {
return call(&storage, std::forward<A>(args)...);
}
constexpr bool valid() const noexcept {
return call != nullptr;
}
explicit constexpr operator bool() const noexcept {
return valid();
}
private:
template <typename F> static function create(F &&func) {
using functor = detail::functor<std::remove_cvref_t<F>>;
static_assert(sizeof(functor) <= sizeof(dummy));
static_assert(alignof(functor) <= alignof(dummy));
function f;
new (&f.storage) functor{std::forward<F>(func)};
f.call = functor::template call<R, A...>;
f.vtable = detail::functor_vtable::create<std::remove_cvref_t<F>>();
return f;
}
template <typename F> function &assign(F &&other) {
this->~function();
return *new (this) function{std::forward<F>(other)};
}
template <typename, unsigned> friend struct function;
using dummy = trivial_function<R(A...), N>::dummy;
union {
struct {
} nothing{};
struct {
alignas(dummy) std::byte storage[sizeof(dummy)];
const detail::functor_vtable *vtable;
};
};
R (*call)(const void *, A &&...){nullptr};
};
template <typename F, typename Signature = typename detail::member_function_signature<decltype(&F::operator())>::type>
function(F) -> function<Signature, (sizeof(F) - 1) / sizeof(void *) + 1>;
// A single-use function object with stored arguments.
template <typename T> struct callable_tuple {
template <typename... E> constexpr callable_tuple(E &&...elements) : tuple{std::forward<E>(elements)...} {
}
template <typename... A> decltype(auto) operator()(A &&...args) {
return call(std::make_index_sequence<std::tuple_size_v<T>>{}, std::forward<A>(args)...);
}
private:
template <std::size_t... I, typename... A> decltype(auto) call(std::index_sequence<I...>, A &&...args) {
return std::invoke(std::get<I>(std::move(tuple))..., std::forward<A>(args)...);
}
T tuple;
};
template <typename... E> callable_tuple(E...) -> callable_tuple<std::tuple<std::decay_t<E>...>>;
} // namespace rims
namespace rims::detail {
template <typename Sig, unsigned N> inline constexpr bool is_function_instance<trivial_function<Sig, N>> = true;
template <typename Sig, unsigned N> inline constexpr bool is_function_instance<function<Sig, N>> = true;
} // namespace rims::detail

145
rims_app/src/gpio.cpp Normal file
View File

@ -0,0 +1,145 @@
#include "gpio.hpp"
#include "proto/gpio.pb.h"
#include "zephyr.hpp"
#include "log.hpp"
#include <exception>
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<gpio_IngressMessage, 2> gpioIngressQueue{gpioIngress};
fifo_queue<gpio_EgressMessage, 2> gpioEgressQueue{gpioEggress};
constexpr int channels = 2;
constexpr std::array<gpio_dt_spec, channels> 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<NthArgument_t<0, decltype(fn)>>(request.data);
auto &resp = reinterpret_cast<NthArgument_t<1, decltype(fn)>>(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

39
rims_app/src/gpio.hpp Normal file
View File

@ -0,0 +1,39 @@
#include "common.hpp"
#include "fifo_queue.hpp"
#include "proto/gpio.pb.h"
#include <array>
namespace rims {
using GpioRequest = gpio_GpioRequest;
using GpioResponse = gpio_GpioResponse;
extern fifo_queue<gpio_IngressMessage, 2> gpioIngressQueue;
extern fifo_queue<gpio_EgressMessage, 2> gpioEgressQueue;
class GPIO {
public:
GPIO();
void loop();
private:
void handle_gpioRequest(const GpioRequest &req, GpioResponse &resp);
void event_gpioMessageArrived();
std::array<k_poll_event, 1> _events;
};
class GpioThread : public ZephyrThread {
public:
GpioThread(TStackBase &stack) : ZephyrThread(stack, 5, 0, "Gpio"){};
void threadMain() override;
protected:
int do_hardwarenInit() override;
};
} // namespace rims

128
rims_app/src/id.hpp Normal file
View File

@ -0,0 +1,128 @@
#pragma once
#include <cstdint>
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<uint32_t>(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<uint32_t>(prior);
return *this;
}
constexpr WireFormatID &setType(Type t) {
type = static_cast<uint32_t>(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

769
rims_app/src/inplace_vector.hpp Normal file
View File

@ -0,0 +1,769 @@
#pragma once
#pragma GCC diagnostic ignored "-Wunused-parameter"
#include <algorithm> // for rotate...
#include <array>
#include <concepts> // for lots...
#include <cstddef> // for size_t
#include <cstdint> // for fixed-width integer types
#include <cstdio> // for assertion diagnostics
#include <cstdlib> // for abort
// #include <functional> // for less and equal_to
#include <iterator> // for reverse_iterator and iterator traits
#include <limits> // for numeric_limits
#include <memory> // for destroy
#include <new> // for operator new
#include <ranges>
#include <stdexcept> // for length_error
#include <type_traits> // 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 <size_t N>
using smallest_size_t
= std::conditional_t<(N < std::numeric_limits<uint8_t>::max()), uint8_t,
std::conditional_t<(N < std::numeric_limits<uint16_t>::max()), uint16_t,
std::conditional_t<(N < std::numeric_limits<uint32_t>::max()), uint32_t,
std::conditional_t<(N < std::numeric_limits<uint64_t>::max()), uint64_t,
size_t>>>>;
// clang-format on
// Index a random-access and sized range doing bound checks in debug builds
template <std::ranges::random_access_range Rng, std::integral Index>
static constexpr decltype(auto) index(Rng &&rng, Index i) noexcept
requires(std::ranges::sized_range<Rng>)
{
IV_EXPECT(static_cast<ptrdiff_t>(i) < std::ranges::size(rng));
return std::begin(std::forward<Rng>(rng))[std::forward<Index>(i)];
}
// http://eel.is/c++draft/container.requirements.general#container.intro.reqmts-2
template <class Rng, class T>
concept container_compatible_range = std::ranges::input_range<Rng> && std::convertible_to<std::ranges::range_reference_t<Rng>, T>;
template <typename T, std::size_t N>
concept satify_constexpr = N == 0 || std::is_trivial_v<T>;
} // namespace beman::details::inplace_vector
// Types implementing the `inplace_vector`'s storage
namespace beman::details::inplace_vector::storage {
// Storage for zero elements.
template <class T> 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 <class T, size_t N> struct trivial {
static_assert(std::is_trivial_v<T>, "storage::trivial<T, C> requires Trivial<T>");
static_assert(N != size_t{0}, "N == 0, use zero_sized");
protected:
using size_type = smallest_size_t<N>;
private:
// If value_type is const, then const std::array of non-const elements:
using array_based_storage = std::conditional_t<!std::is_const_v<T>, std::array<T, N>, const std::array<std::remove_const_t<T>, 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 <class T, size_t N> 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<T *>(_d) + i;
}
constexpr const T *storage_data(size_t i) const noexcept {
IV_EXPECT(i < N);
return reinterpret_cast<const T *>(_d) + i;
}
};
/// Storage for non-trivial elements.
template <class T, size_t N> struct non_trivial {
static_assert(!std::is_trivial_v<T>, "use storage::trivial for Trivial<T> elements");
static_assert(N != size_t{0}, "use storage::zero for N==0");
protected:
using size_type = smallest_size_t<N>;
private:
using byte_based_storage =
std::conditional_t<!std::is_const_v<T>, raw_byte_based_storage<T, N>, const raw_byte_based_storage<std::remove_const_t<T>, 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<T>)
= default;
constexpr ~non_trivial() {
std::destroy(storage_data(), storage_data() + storage_size());
}
};
// Selects the vector storage.
template <class T, size_t N>
using storage_for = std::conditional_t<!satify_constexpr<T, N>, non_trivial<T, N>, std::conditional_t<N == 0, zero_sized<T>, trivial<T, N>>>;
} // namespace beman::details::inplace_vector::storage
namespace beman {
template <typename IV>
concept has_constexpr_support = details::inplace_vector::satify_constexpr<typename IV::value_type, IV::capacity()>;
/// Dynamically-resizable fixed-N vector with inplace storage.
template <class T, size_t N> struct inplace_vector : private details::inplace_vector::storage::storage_for<T, N> {
private:
static_assert(std::is_nothrow_destructible_v<T>, "T must be nothrow destructible");
using base_t = details::inplace_vector::storage::storage_for<T, N>;
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<iterator>;
using const_reverse_iterator = std::reverse_iterator<const_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 <class InputIterator> // BUGBUG: why not model input_iterator?
// constexpr inplace_vector(InputIterator first, InputIterator
// last);
// template <details::inplace_vector::container_compatible_range<T> R>
// constexpr inplace_vector(from_range_t, R&& rg);
// from base-class, trivial if is_trivially_copy_constructible_v<T>:
// constexpr inplace_vector(const inplace_vector&);
// from base-class, trivial if is_trivially_move_constructible_v<T>
// constexpr inplace_vector(inplace_vector&&) noexcept(N == 0 ||
// std::is_nothrow_move_constructible_v<T>);
// constexpr inplace_vector(std::initializer_list<T> il);
// from base-class, trivial if is_trivially_destructible_v<T>
// constexpr ~inplace_vector();
// from base-class, trivial if is_trivially_destructible_v<T> &&
// is_trivially_copy_assignable_v<T>
// constexpr inplace_vector& operator=(const inplace_vector& other);
// from base-class, trivial if is_trivially_destructible_v<T> &&
// is_trivially_copy_assignable_v<T>
// constexpr inplace_vector& operator=(inplace_vector&& other)
// noexcept(N == 0 || is_nothrow_move_assignable_v<T>);
// template <class InputIterator> // BUGBUG: why not model input_iterator
// constexpr void assign(InputIterator first, InputIterator last);
// template<details::inplace_vector::container_compatible_range<T> R>
// constexpr void assign_range(R&& rg);
// constexpr void assign(size_type n, const T& u);
// constexpr void assign(std::initializer_list<T> 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 <class... Args>
// constexpr T& emplace_back(Args&&... args);
// constexpr T& push_back(const T& x);
// constexpr T& push_back(T&& x);
// template<details::inplace_vector::container_compatible_range<T> R>
// constexpr void append_range(R&& rg);
// constexpr void pop_back();
// template<class... Args>
// constexpr T* try_emplace_back(Args&&... args);
// constexpr T* try_push_back(const T& value);
// constexpr T* try_push_back(T&& value);
// template<class... Args>
// constexpr T& unchecked_emplace_back(Args&&... args);
// constexpr T& unchecked_push_back(const T& value);
// constexpr T& unchecked_push_back(T&& value);
// template <class... Args>
// 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 <class InputIterator>
// constexpr iterator insert(const_iterator position, InputIterator
// first, InputIterator last);
// template<details::inplace_vector::container_compatible_range<T> R>
// constexpr iterator insert_range(const_iterator position, R&& rg);
// constexpr iterator insert(const_iterator position,
// std::initializer_list<T>
// 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<T> &&
// std::is_nothrow_move_constructible_v<T>));
// 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<T>*/
// 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<T> && std::is_nothrow_move_constructible_v<T>)) {
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<T>) {
assert_iterator_pair_in_range(first, last);
if constexpr (N > 0 && !std::is_trivial_v<T>) {
for (; first != last; ++first)
first->~T();
}
}
public:
// Implementation
// [containers.sequences.inplace_vector.modifiers], modifiers
template <class... Args>
constexpr T &unchecked_emplace_back(Args &&...args)
requires(std::constructible_from<T, Args...>)
{
IV_EXPECT(size() < capacity() && "inplace_vector out-of-memory");
std::construct_at(end(), std::forward<Args>(args)...);
unsafe_set_size(size() + size_type(1));
return back();
}
template <class... Args> constexpr T *try_emplace_back(Args &&...args) {
if (size() == capacity()) [[unlikely]]
return nullptr;
return &unchecked_emplace_back(std::forward<Args>(args)...);
}
template <class... Args>
constexpr T &emplace_back(Args &&...args)
requires(std::constructible_from<T, Args...>)
{
if (!try_emplace_back(std::forward<Args>(args)...)) [[unlikely]]
throw std::bad_alloc();
return back();
}
constexpr T &push_back(const T &x)
requires(std::constructible_from<T, const T &>)
{
emplace_back(x);
return back();
}
constexpr T &push_back(T &&x)
requires(std::constructible_from<T, T &&>)
{
emplace_back(std::forward<T &&>(x));
return back();
}
constexpr T *try_push_back(const T &x)
requires(std::constructible_from<T, const T &>)
{
return try_emplace_back(x);
}
constexpr T *try_push_back(T &&x)
requires(std::constructible_from<T, T &&>)
{
return try_emplace_back(std::forward<T &&>(x));
}
constexpr T &unchecked_push_back(const T &x)
requires(std::constructible_from<T, const T &>)
{
return unchecked_emplace_back(x);
}
constexpr T &unchecked_push_back(T &&x)
requires(std::constructible_from<T, T &&>)
{
return unchecked_emplace_back(std::forward<T &&>(x));
}
template <details::inplace_vector::container_compatible_range<T> R>
constexpr void append_range(R &&rg)
requires(std::constructible_from<T, std::ranges::range_reference_t<R>>)
{
if constexpr (std::ranges::sized_range<R>) {
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<decltype(e)>(e));
}
}
template <class... Args>
constexpr iterator emplace(const_iterator position, Args &&...args)
requires(std::constructible_from<T, Args...> && std::movable<T>)
{
assert_iterator_in_range(position);
auto b = end();
emplace_back(std::forward<Args>(args)...);
auto pos = begin() + (position - begin());
std::rotate(pos, b, end());
return pos;
}
template <class InputIterator>
constexpr iterator insert(const_iterator position, InputIterator first, InputIterator last)
requires(std::constructible_from<T, std::iter_reference_t<InputIterator>> && std::movable<T>)
{
assert_iterator_in_range(position);
if constexpr (std::random_access_iterator<InputIterator>) {
if (size() + static_cast<size_type>(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 <details::inplace_vector::container_compatible_range<T> R>
constexpr iterator insert_range(const_iterator position, R &&rg)
requires(std::constructible_from<T, std::ranges::range_reference_t<R>> && std::movable<T>)
{
return insert(position, std::begin(rg), std::end(rg));
}
constexpr iterator insert(const_iterator position, std::initializer_list<T> il)
requires(std::constructible_from<T, std::ranges::range_reference_t<std::initializer_list<T>>> && std::movable<T>)
{
return insert_range(position, il);
}
constexpr iterator insert(const_iterator position, size_type n, const T &x)
requires(std::constructible_from<T, const T &> && std::copyable<T>)
{
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<T, const T &> && std::copyable<T>)
{
return insert(position, 1, x);
}
constexpr iterator insert(const_iterator position, T &&x)
requires(std::constructible_from<T, T &&> && std::movable<T>)
{
return emplace(position, std::move(x));
}
constexpr inplace_vector(std::initializer_list<T> il)
requires(std::constructible_from<T, std::ranges::range_reference_t<std::initializer_list<T>>> && std::movable<T>)
{
insert(begin(), il);
}
constexpr inplace_vector(size_type n, const T &value)
requires(std::constructible_from<T, const T &> && std::copyable<T>)
{
insert(begin(), n, value);
}
constexpr explicit inplace_vector(size_type n)
requires(std::constructible_from<T, T &&> && std::default_initializable<T>)
{
for (size_type i = 0; i < n; ++i)
emplace_back(T{});
}
template <class InputIterator> // BUGBUG: why not std::ranges::input_iterator?
constexpr inplace_vector(InputIterator first, InputIterator last)
requires(std::constructible_from<T, std::iter_reference_t<InputIterator>> && std::movable<T>)
{
insert(begin(), first, last);
}
template <details::inplace_vector::container_compatible_range<T> R>
constexpr inplace_vector(beman::from_range_t, R &&rg)
requires(std::constructible_from<T, std::ranges::range_reference_t<R>> && std::movable<T>)
{
insert_range(begin(), std::forward<R &&>(rg));
}
constexpr iterator erase(const_iterator first, const_iterator last)
requires(std::movable<T>)
{
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<size_type>(last - first));
}
return f;
}
constexpr iterator erase(const_iterator position)
requires(std::movable<T>)
{
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<T, const T &> && std::copyable<T>)
{
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<T, T &&> && std::default_initializable<T>)
{
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<T>)
= default;
constexpr inplace_vector(const inplace_vector &x)
requires(N != 0 && !std::is_trivially_copy_constructible_v<T> && std::copyable<T>)
{
for (auto &&e : x)
emplace_back(e);
}
constexpr inplace_vector(inplace_vector &&x)
requires(N == 0 || std::is_trivially_move_constructible_v<T>)
= default;
constexpr inplace_vector(inplace_vector &&x)
requires(N != 0 && !std::is_trivially_move_constructible_v<T> && std::movable<T>)
{
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<T> && std::is_trivially_copy_constructible_v<T> &&
std::is_trivially_copy_assignable_v<T>))
= default;
constexpr inplace_vector &operator=(const inplace_vector &x)
requires(N != 0 && !(std::is_trivially_destructible_v<T> && std::is_trivially_copy_constructible_v<T> && std::is_trivially_copy_assignable_v<T>) && std::copyable<T>)
{
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<T> && std::is_trivially_move_constructible_v<T> &&
std::is_trivially_move_assignable_v<T>))
= default;
constexpr inplace_vector &operator=(inplace_vector &&x)
requires(N != 0 && !(std::is_trivially_destructible_v<T> && std::is_trivially_move_constructible_v<T> && std::is_trivially_move_assignable_v<T>) && std::movable<T>)
{
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<T> && std::is_nothrow_move_constructible_v<T>))
requires(std::movable<T>)
{
auto tmp = std::move(x);
x = std::move(*this);
(*this) = std::move(tmp);
}
template <class InputIterator>
constexpr void assign(InputIterator first, InputIterator last)
requires(std::constructible_from<T, std::iter_reference_t<InputIterator>> && std::movable<T>)
{
clear();
insert(begin(), first, last);
}
template <details::inplace_vector::container_compatible_range<T> R>
constexpr void assign_range(R &&rg)
requires(std::constructible_from<T, std::ranges::range_reference_t<R>> && std::movable<T>)
{
assign(std::begin(rg), std::end(rg));
}
constexpr void assign(size_type n, const T &u)
requires(std::constructible_from<T, const T &> && std::movable<T>)
{
clear();
insert(begin(), n, u);
}
constexpr void assign(std::initializer_list<T> il)
requires(std::constructible_from<T, std::ranges::range_reference_t<std::initializer_list<T>>> && std::movable<T>)
{
clear();
insert_range(begin(), il);
}
constexpr friend int /*synth-three-way-result<T>*/
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 <typename T, std::size_t N, typename U = T> constexpr std::size_t erase(inplace_vector<T, N> &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 <typename T, std::size_t N, typename Predicate> constexpr std::size_t erase_if(inplace_vector<T, N> &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

13
rims_app/src/log.cpp
View File

@ -1,14 +1,12 @@
#include "log.hpp"
#include "log.pb.h"
#include "zephyr/kernel.h"
#include <cassert>
#include <chrono>
#include <cstring>
#include <thread>
namespace rims {
K_FIFO_DEFINE(klogFifoQueue);
zephyr_fifo_buffer<log_EgressMessages, 10> logEgressFifoQueueBuffer{klogFifoQueue};
fifo_queue<log_EgressMessage, 16> 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();

12
rims_app/src/log.hpp
View File

@ -2,12 +2,12 @@
#include <cstdarg>
#include <cstdio>
#include <functional>
#include <source_location>
#include <string_view>
#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<log_IngressMessages, 2> logIngressFifoQueueBuffer;
extern zephyr_fifo_buffer<log_EgressMessages, 10> logEgressFifoQueueBuffer;
extern fifo_queue<log_IngressMessage, 2> logIngressFifoQueueBuffer;
extern fifo_queue<log_EgressMessage, 16> 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<void(Level level, std::string_view usermsg, const std::source_location &sl)>;
using Sink = rims::function<void(Level level, std::string_view usermsg, const std::source_location &sl)>;
// static void registerSink(std::string_view name, Sink sink);
// static void unregisterSink(std::string_view name);

98
rims_app/src/main.cpp
View File

@ -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 <array>
#include <chrono>
#include <cstddef>
#include <cstdio>
#include <cstdlib>
#include <thread>
#include <zephyr/kernel.h>
#include <thread>
#include <zephyr/kernel.h>
#include <zephyr/kernel/thread.h>
#include <zephyr/kernel/thread_stack.h>
#include <zephyr/drivers/gpio.h>
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 <typename T> class LazyInit {
alignas(T) std::byte _buf[sizeof(T)];
unique_placed_ptr<T> obj;
@ -65,6 +69,20 @@ LazyInit<UARTThread> uartThread;
LazyInit<TemperatureSamplerThread> temperatureSampler;
LazyInit<ZeroCrossDetectionThread> zcd;
LazyInit<PhaseModulationThread> phaseModulation;
LazyInit<GpioThread> 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");
// }
}

318
rims_app/src/message_decode.cpp
View File

@ -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 <array>
#include <cstring>
#include <functional>
#include <zephyr/kernel.h>
#include <cassert>
#include <cstddef>
#include <cstdint>
#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<const pb_byte_t *>(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 <typename Msg> 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<std::byte *>(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

60
rims_app/src/message_decode.hpp
View File

@ -1,60 +0,0 @@
#pragma once
#include "common.hpp"
#include "message.pb.h"
#include "uart.hpp"
#include "zephyr/kernel.h"
#include <cstddef>
#include <pb_decode.h>
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 <typename Msg> 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<k_poll_event, 5> _events;
std::array<requestData, 8> _activeRequests;
};
} // namespace rims

28
rims_app/src/message_pb2.py
View File

@ -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)

510
rims_app/src/messenger.cpp Normal file
View File

@ -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 <array>
#include <cstring>
#include <exception>
#include <functional>
#include <optional>
#include <span>
#include <cassert>
#include <cstddef>
#include <cstdint>
#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<uint8_t> 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<uint8_t> in, std::span<uint8_t> 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<uint8_t> in, std::span<uint8_t> 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<uint32_t, pb_wire_type_t> 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<uint8_t> bytes) {
return pb_istream_from_buffer(bytes.data(), bytes.size_bytes());
}
inline pb_ostream_t ostream_from_span(std::span<uint8_t> 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<uint8_t> 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<uint32_t> 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<uint32_t> 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::RequestData> MessengerThread::takeActiveRequest(int wireId_receiver, uint32_t id) {
std::optional<MessengerThread::RequestData> 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

63
rims_app/src/messenger.hpp Normal file
View File

@ -0,0 +1,63 @@
#pragma once
#include "common.hpp"
#include "id.hpp"
#include "proto/message.pb.h"
#include <span>
#include <cstdint>
namespace rims {
class AsyncUART;
struct BufferView {
std::span<uint8_t> 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<uint32_t> requestId);
void egressPush(void *submessage, const pb_msgdesc_t &fields, int id, std::optional<uint32_t> requestId);
void putActiveRequest(WireFormatID wireId, uint32_t id, AsyncUART *cb);
std::optional<RequestData> takeActiveRequest(int type, uint32_t id);
std::array<k_poll_event, 6> _events;
std::array<RequestData, 8> _activeRequests;
uint8_t _activeRequestsNr;
};
} // namespace rims

46
rims_app/src/nanopb_pb2.py
View File

@ -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)

155
rims_app/src/phase_modulation.cpp
View File

@ -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 <algorithm>
#include <chrono>
#include <functional>
#include <zephyr/drivers/gpio.h>
#include <zephyr/kernel.h>
namespace rims {
K_FIFO_DEFINE(ctrlIngress);
K_FIFO_DEFINE(ctrlEggress);
zephyr_fifo_buffer<ctrl_IngressMessages, 2> ctrlIngressQueue{ctrlIngress};
zephyr_fifo_buffer<ctrl_EgressMessages, 2> ctrlEgressQueue{ctrlEggress};
std::array<gpio_dt_spec, Channels> 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<NoneModulation>(_buffer[0])(), placement_unique<NoneModulation>(_buffer[1])()} {
_channel[0] = placement_unique<PhaseModulation>(_buffer[0])(pins[0]);
_channel[1] = placement_unique<PhaseModulation>(_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<float>(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

98
rims_app/src/phase_modulation.hpp
View File

@ -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 <algorithm>
#include <variant>
#include <zephyr/drivers/gpio.h>
namespace rims {
extern zephyr_fifo_buffer<ctrl_IngressMessages, 2> ctrlIngressQueue;
extern zephyr_fifo_buffer<ctrl_EgressMessages, 2> 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<NoneModulation>,
unique_placed_ptr<PhaseModulation>,
unique_placed_ptr<GroupModulation>>;
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<k_poll_event, 2> _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

58
rims_app/src/ping.py
View File

@ -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()

441
rims_app/src/power_control.cpp Normal file
View File

@ -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 <algorithm>
#include <chrono>
#include <cstdint>
#include <exception>
#include <functional>
#include <tuple>
#include <variant>
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<ctrl_IngressMessage, 2> ctrlIngressQueue{ctrlIngress};
fifo_queue<ctrl_EgressMessage, 2> ctrlEgressQueue{ctrlEggress};
constexpr std::array<gpio_dt_spec, MaxChannels> 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<void()>{[this]() { this->on(); }}, std::chrono::milliseconds{0}},
_triacTimerStop{rims::function<void()>{[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<uint16_t>(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<NthArgument_t<0, decltype(fn)>>(request.data);
auto &resp = reinterpret_cast<NthArgument_t<1, decltype(fn)>>(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<GroupModulation>(_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<GroupModulation>(_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<DisabledMode>();
break;
case ctrl_ModeOfOperation_ConstantTemperature:
throw ctrl::unsuported_mode{};
case ctrl_ModeOfOperation_TemperatureSlope:
throw ctrl::unsuported_mode{};
case ctrl_ModeOfOperation_ManualPower:
_chModeOfOperationStorage[ch].emplace<ManualPowerMode>(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<DisabledMode>(_chModeOfOperationStorage.at(ch))) {
return ctrl_ModeOfOperation_Disabled;
}
if (std::holds_alternative<ManualPowerMode>(_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<NoModulation>();
break;
case ctrl_PowerControlAlgorithm_GroupModulation:
_chPowerControlStrategy[ch].emplace<GroupModulation>(pins[ch]);
break;
case ctrl_PowerControlAlgorithm_PhaseModulation:
_chPowerControlStrategy[ch].emplace<PhaseModulation>(pins[ch]);
break;
default:
break;
}
}
PowerControlAlgorithm PhaseModulationOrchestrator::powerControlAlgorithm(uint8_t ch) {
checkChannel(ch);
if (std::holds_alternative<GroupModulation>(_chPowerControlStrategy.at(ch))) {
return ctrl_PowerControlAlgorithm_GroupModulation;
}
if (std::holds_alternative<PhaseModulation>(_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

333
rims_app/src/power_control.hpp Normal file
View File

@ -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 <algorithm>
#include <cstdint>
#include <variant>
#include <zephyr/drivers/gpio.h>
namespace rims {
extern fifo_queue<ctrl_IngressMessage, 2> ctrlIngressQueue;
extern fifo_queue<ctrl_EgressMessage, 2> 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<TargetPowerObserver *, MaxChannels> _observers;
beman::inplace_vector<uint8_t, MaxChannels> _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<DisabledMode, ManualPowerMode>;
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<ControlStrategy, MaxChannels> _chPowerControlStrategy; // storage for different strategies, per channal
std::array<OperationStrategy, MaxChannels> _chModeOfOperationStorage;
PowerPublisher _powerPublisher;
std::array<k_poll_event, 2> _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

8
rims_app/src/power_measurements.cpp
View File

@ -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<adc_sample> current_measurements, std::span<short_microseconds> offsets) {
float fit_sine_wave_amplitude(std::span<adc_sample> current_measurements, std::span<microseconds_u16_t> 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<adc_sample> current_measurements, std::s
}
// Function to calculate power based on current measurements, resistance, and zero-crossing information
float calculate_power(std::span<adc_sample> current_measurements, std::span<short_microseconds> time_intervals, float resistance) {
float calculate_power(std::span<adc_sample> current_measurements, std::span<microseconds_u16_t> 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<adc_sample> measurements{&br};
std::pmr::vector<short_microseconds> time_points{&br};
std::pmr::vector<microseconds_u16_t> time_points{&br};
measurements.reserve(100);
time_points.reserve(100);
measurements.emplace_back(32.0f);
time_points.emplace_back(std::chrono::duration_cast<short_microseconds>(clock::now() - clock::now()));
time_points.emplace_back(std::chrono::duration_cast<microseconds_u16_t>(clock::now() - clock::now()));
while (1) {
///TODO fix

456
rims_app/src/ring_buffer.hpp Normal file
View File

@ -0,0 +1,456 @@
#pragma once
#include "zephyr.hpp"
#include <array>
#include <cassert>
#include <cstddef>
#include <cstring>
#include <type_traits>
#include <utility>
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<index_t, std::size_t>;
constexpr std::pair<area, area> 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 T, std::size_t N> class RingBuffer {
public:
static_assert(std::is_trivial_v<T>);
static_assert(std::is_trivially_destructible_v<T>);
static_assert(std::is_trivially_copyable_v<T>);
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<T *>(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<T>);
__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<T *>(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 <bool CONST> 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<CONST, const value_type *, value_type *>;
using reference = std::conditional_t<CONST, const value_type &, value_type &>;
using iterator_t = iterator_base<CONST>;
using buf_t = std::conditional_t<CONST, const std::array<std::byte[sizeof(T)], N>, std::array<std::byte[sizeof(T)], N>>;
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<pointer>(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<difference_type>(lhs.current_ - rhs.current_);
} else {
return static_cast<difference_type>(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<false>;
using const_iterator = iterator_base<true>;
static_assert(std::bidirectional_iterator<iterator>);
// static_assert(std::random_access_iterator< iterator >);
static_assert(std::bidirectional_iterator<const_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<const T *>(std::addressof(buf_[index]));
}
T &directy_at(int index) {
return *reinterpret_cast<T *>(std::addressof(buf_[index]));
}
std::array<std::byte[sizeof(T)], N> buf_;
RingIndex _index;
};
} // namespace rims

579
rims_app/src/temperature_measurements.cpp
View File

@ -1,308 +1,499 @@
#include "temperature_measurements.hpp"
#include <algorithm>
#include <array>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <cstring>
#include <exception>
#include <functional>
#include <numeric>
#include <thread>
#include <tuple>
#include <type_traits>
#include "common.hpp"
#include "log.hpp"
#include "temperature.pb.h"
#include "proto/temperature.pb.h"
#include <zephyr/drivers/adc.h>
#include <zephyr/drivers/gpio.h>
#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<temperature_IngressMessages, 2> temperatureIngressQueue{temperatureIngress};
zephyr_fifo_buffer<temperature_EgressMessages, 2> temperatureEgressQueue{temperatureEggress};
fifo_queue<temperature_IngressMessage, 2> temperatureIngressQueue{temperatureIngress};
fifo_queue<temperature_EgressMessage, 2> 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 <typename T> void PB_encode_egress(const T &tempresp) {
/// TODO repeat on fail
temperatureEgressQueue.try_produce([&](temperature_EgressMessages &egress) {
if constexpr (std::is_same_v<T, GetTemperatureResponse>) {
temperatureEgressQueue.try_produce([&](temperature_EgressMessage &egress) {
if constexpr (std::is_same_v<T, GetTemperatureSummaryResponse>) {
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<T, SamplerConfigResponse>) {
ULOG_DEBUG("Sending SamplerConfigResponse message");
egress.data.samplerConfigResponse = tempresp;
egress.which_data = temperature_EgressMessages_samplerConfigResponse_tag;
} else if constexpr (std::is_same_v<T, TemperatureStatistics>) {
egress.data.sampler_config_response = tempresp;
egress.which_data = temperature_EgressMessage_sampler_config_response_tag;
} else if constexpr (std::is_same_v<T, BroadcastTemperatureStatistics>) {
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<int16_t, 4> 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<std::chrono::milliseconds>(_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<std::chrono::milliseconds>(_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<float>(adcMin - adcMax);
}
adc_sample TemperatureSampler::adcWithCalibration() {
calibration(true);
return takeStableRead(true);
}
adc_sample TemperatureSampler::adc() const {
std::array<adc_sample, 16> 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 <typename Req, typename Resp> 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

95
rims_app/src/temperature_measurements.hpp
View File

@ -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 <cstddef>
@ -13,27 +13,34 @@
namespace rims {
extern zephyr_fifo_buffer<temperature_IngressMessages, 2> temperatureIngressQueue;
extern zephyr_fifo_buffer<temperature_EgressMessages, 2> temperatureEgressQueue;
extern fifo_queue<temperature_IngressMessage, 2> temperatureIngressQueue;
extern fifo_queue<temperature_EgressMessage, 2> 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<Sample, MaxSampleSize> _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<adc_sample, MaxSampleSize> _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<TemperatureSampler, temperatureChannelsNr> _channel;
std::array<TemperatureSampler, temperatureChannelsNr> _temperatureSamplerChannels;
zephyr::semaphore::sem _samplerSem{0, 1};
RecurringSemaphoreTimer _samplerTimer;
zephyr::semaphore::sem _samplerTickSem{0, 1};
RecurringSemaphoreTimer _samplerTick{_samplerTickSem, std::chrono::milliseconds{1}};
std::array<zephyr::semaphore::sem, temperatureChannelsNr> _broadcastSem;
std::array<RecurringSemaphoreTimer, temperatureChannelsNr> _broadcastTimer;
std::array<k_poll_event, 2 + temperatureChannelsNr> _events; // event from timer and from Messenger
std::array<k_poll_event, 2> _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;
};

160
rims_app/src/uart.cpp
View File

@ -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 <chrono>
#include <cstddef>
@ -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<std::byte> bytes) {
void AsyncUART::transmit(AsyncUART *dev, std::span<std::uint8_t> 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<std::byte *>(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) {

57
rims_app/src/uart.hpp
View File

@ -1,9 +1,9 @@
#pragma once
#include "circular_buffer.hpp"
#include "ring_buffer.hpp"
#include "common.hpp"
#include "inplace_vector.hpp"
#include <cstddef>
#include <cstdint>
#include <span>
#include <zephyr/drivers/uart.h>
@ -18,28 +18,30 @@ class uart_rx_buffer_overflow;
class uart_rx_not_ready_error;
class AsyncUART {
using rx_buffer_t = static_vector<uint8_t, 256>;
using tx_buffer_t = circular_buffer<uint8_t, 256>;
using rx_buffer_t = beman::inplace_vector<uint8_t, 256>;
using tx_buffer_t = RingBuffer<uint8_t, 256>;
public:
AsyncUART();
void loop();
static void transmit(AsyncUART *dev, std::span<std::byte> bytes);
static void workHandler(k_work *work);
static void transmit(AsyncUART *dev, std::span<uint8_t> 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_buffer_t, 2> 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:

42
rims_app/src/zephyr.cpp
View File

@ -1,13 +1,45 @@
#include "zephyr.hpp"
#include "log.hpp"
#include "zephyr/sys/__assert.h"
#include <source_location>
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);
}
}

66
rims_app/src/zephyr.hpp
View File

@ -6,6 +6,7 @@
#include <source_location>
#include <span>
#include <zephyr/drivers/adc.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/crc.h>
@ -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<uint8_t> 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<k_poll_event> events, std::chrono::nanoseconds timeout = std::chrono::nanoseconds{0}) {
return ::k_poll(events.data(), events.size(), chronoToKTimeout(timeout));
}
@ -81,7 +116,7 @@ template <typename Fn> 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

108
rims_app/src/zero_cross_detection.cpp
View File

@ -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 <chrono>
#include <cstdint>
#include <numeric>
#include <zephyr/device.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/kernel.h>
#include <bitset>
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<ZeroCrossDetectionEvent, 2> ZeroCrossDetectionEventQueue{zcdFifo};
fifo_queue<ZeroCrossDetectionEvent, 2> 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<bool>(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<microseconds_u16_t>(clock::now() - _this->_pulseDown));
_this->_pulseDown = clock::now();
} else {
_this->_pulsWidths.push_back(std::chrono::duration_cast<microseconds_u16_t>(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<std::chrono::duration<int32_t>>(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<std::chrono::duration<int32_t>>(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

51
rims_app/src/zero_cross_detection.hpp
View File

@ -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 <chrono>
@ -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<std::chrono::nanoseconds>(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<ZeroCrossDetectionEvent, 2> ZeroCrossDetectionEventQueue;
using ZCDFifo_t = zephyr_fifo_buffer<ZeroCrossDetectionEvent, 2>;
extern fifo_queue<ZeroCrossDetectionEvent, 2> ZeroCrossDetectionEventQueue;
using ZCDFifo_t = fifo_queue<ZeroCrossDetectionEvent, 2>;
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<microseconds_u16_t, 20> _pulsWidths;
rims::RingBuffer<microseconds_u16_t, 20> _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);

124
rims_app/temperature.proto
View File

@ -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;
}
};