From f5be4c4e18dbcbb8dd8ef4d7e2779a6ff7e1101b Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Fri, 18 Apr 2025 14:44:09 +0200 Subject: [PATCH] fix temperature readings --- .gitignore | 1 + boards/bartoszek/rims/rims_h503cbt.dts | 121 +++++++---- rims_app/CMakeLists.txt.user | 14 +- rims_app/src/circular_buffer.hpp | 76 +++++++ rims_app/src/common.hpp | 1 + rims_app/src/log.cpp | 2 +- rims_app/src/log.hpp | 4 +- rims_app/src/main.cpp | 39 ++-- rims_app/src/messenger.cpp | 2 +- rims_app/src/phase_modulation.cpp | 30 +-- rims_app/src/phase_modulation.hpp | 3 + rims_app/src/temperature_measurements.cpp | 238 ++++++++++++++++------ rims_app/src/temperature_measurements.hpp | 37 +++- rims_app/src/zephyr.cpp | 3 +- rims_app/src/zephyr.hpp | 35 ++-- rims_app/src/zero_cross_detection.cpp | 12 +- 16 files changed, 447 insertions(+), 171 deletions(-) diff --git a/.gitignore b/.gitignore index aab3981f52f..1a0c5ba8a9e 100644 --- a/.gitignore +++ b/.gitignore @@ -12,6 +12,7 @@ \#*\# build*/ +config_default*/ !doc/build/ !scripts/build !tests/drivers/build_all diff --git a/boards/bartoszek/rims/rims_h503cbt.dts b/boards/bartoszek/rims/rims_h503cbt.dts index 08238b99154..56c3d194b2d 100644 --- a/boards/bartoszek/rims/rims_h503cbt.dts +++ b/boards/bartoszek/rims/rims_h503cbt.dts @@ -60,38 +60,68 @@ compatible= "gpio-pin"; temp_channel_sel: temp_channel_sel { - gpios = <&gpioa 1 GPIO_ACTIVE_LOW>; + gpios = <&gpioc 13 GPIO_ACTIVE_HIGH>; label = "CHSEL1"; }; - triac_enable_ch1: triac_enable_ch1 { + rmin_en: rmin_en { + gpios = <&gpioc 14 GPIO_ACTIVE_HIGH>; + label = "Rmin_EN"; + }; + + rmax_en: rmax_en { + gpios = <&gpioc 15 GPIO_ACTIVE_HIGH>; + label = "Rmax_EN"; + }; + + rpt_en: rpt_en { gpios = <&gpiob 6 GPIO_ACTIVE_HIGH>; - label = "POWER_ENABLE_1"; + label = "Rpt_EN"; }; - triac_enable_ch2: triac_enable_ch2 { + gprelay_1_en: gprelay_1_en { + gpios = <&gpiob 1 GPIO_ACTIVE_HIGH>; + label = "GPRelay_1_EN"; + }; + + gprelay_2_en: gprelay_2_en { + gpios = <&gpiob 0 GPIO_ACTIVE_HIGH>; + label = "GPRelay_2_EN"; + }; + + power_led: power_led { + gpios = <&gpioa 6 GPIO_ACTIVE_HIGH>; + label = "POWER_LED"; + }; + + status_led: status_led { + gpios = <&gpioa 7 GPIO_ACTIVE_HIGH>; + label = "STATUS_LED"; + }; + + can_stby: can_stby { + gpios = <&gpioa 9 GPIO_ACTIVE_HIGH>; + label = "CAN_STBY"; + }; + + ch1_zcd: ch1_zcd { gpios = <&gpiob 4 GPIO_ACTIVE_HIGH>; - label = "POWER_ENABLE_2"; + label = "CH1_ZCD"; }; - //line_enable_ch1: line_enable_ch1 { - // gpios = <&gpiob 3 GPIO_ACTIVE_LOW>; - // label = "POWER_ENABLE_1"; - //}; - - //line_enable_ch2: line_enable_ch2 { - // gpios = <&gpiob 4 GPIO_ACTIVE_LOW>; - // label = "POWER_ENABLE_2"; - //}; - - zcd_state_1: zcd_state_1 { - gpios = <&gpiob 5 GPIO_ACTIVE_LOW>; - label = "ZCD_IN_1"; + ch1_en: ch1_en { + gpios = <&gpiob 5 GPIO_ACTIVE_HIGH>; + label = "CH1_EN"; }; - zcd_state_2: zcd_state_2 { - gpios = <&gpiob 3 GPIO_ACTIVE_LOW>; - label = "ZCD_IN_2"; + ch2_zcd: ch2_zcd { + gpios = <&gpioa 15 GPIO_ACTIVE_HIGH>; + label = "CH2_ZCD"; + }; + + ch2_en: ch2_en { + gpios = <&gpiob 3 GPIO_ACTIVE_HIGH>; + label = "CH2_EN"; }; }; @@ -119,15 +149,15 @@ zephyr,acquisition-time = ; zephyr,resolution = <12>; }; - adc_current_ch1: channel@18 { // @pinctrl.dtsi as adc1_inp18_pa4: adc1_inp18_pa4 - reg = <18>; + adc_current_ch1: channel@14 { // @pinctrl.dtsi as adc1_inp14_pa2: adc1_inp14_pa2 + reg = <14>; zephyr,gain = "ADC_GAIN_1"; zephyr,reference = "ADC_REF_INTERNAL"; zephyr,acquisition-time = ; zephyr,resolution = <12>; }; - adc_current_ch2: channel@19 { // @pinctrl.dtsi as adc1_inp19_pa5: adc1_inp19_pa5 - reg = <19>; + adc_current_ch2: channel@18 { // @pinctrl.dtsi as adc1_inn18_pa5: adc1_inn18_pa5 + reg = <18>; zephyr,gain = "ADC_GAIN_1"; zephyr,reference = "ADC_REF_INTERNAL"; zephyr,acquisition-time = ; @@ -143,10 +173,14 @@ status = "okay"; }; +&gpioc{ + status = "okay"; +}; + &clk_csi { status = "okay"; }; -// 240886428 + &clk_hsi { /* 64MHz, ok*/ clock-frequency = ; status = "okay"; @@ -157,22 +191,23 @@ }; &clk_hse { - status = "disabled"; + status = "okay"; + clock-frequency = ; }; -&pll { - div-m = <4>; - mul-n = <30>; - div-r = <6>; +&pll1 { + div-m = <1>; + mul-n = <20>; + div-r = <2>; div-p = <2>; div-q = <2>; - clocks = <&clk_hsi>; + clocks = <&clk_hse>; status = "okay"; }; &rcc { - clocks = <&pll>; - clock-frequency = ; + clocks = <&pll1>; + clock-frequency = ; ahb-prescaler = <1>; apb1-prescaler = <1>; apb2-prescaler = <1>; @@ -180,22 +215,24 @@ status = "okay"; }; -//&gpdma1 { -// status = "okay"; -//}; - &usart1 { pinctrl-0 = <&usart1_tx_pb14 &usart1_rx_pb15>; pinctrl-names = "default"; -// dmas = <&gpdma1 7 22 (STM32_DMA_PERIPH_TX | STM32_DMA_PRIORITY_LOW) // GPDMA1_REQUEST_USART1_TX -// &gpdma1 2 21 (STM32_DMA_PERIPH_RX | STM32_DMA_PRIORITY_LOW)>; // GPDMA1_REQUEST_USART1_RX -// dma-names = "tx", "rx"; + current-speed = <3000000>; - current-speed = <921600>; +// clocks = <&rcc STM32_CLOCK(APB2, 14)>, <&rcc STM32_SRC_PLL2_Q USART1_SEL(1)>; status = "okay"; }; +&usart1_tx_pb14 { + slew-rate = "very-high-speed"; +}; + +&usart1_rx_pb15 { + slew-rate = "very-high-speed"; +}; + /* &rng { status = "okay"; diff --git a/rims_app/CMakeLists.txt.user b/rims_app/CMakeLists.txt.user index 4e9a5f03d5a..f68b96de663 100644 --- a/rims_app/CMakeLists.txt.user +++ b/rims_app/CMakeLists.txt.user @@ -1,6 +1,6 @@ - + EnvironmentId @@ -108,14 +108,14 @@ 2 false - -DCMAKE_COLOR_DIAGNOSTICS:BOOL=ON --DCMAKE_PREFIX_PATH:PATH=%{Qt:QT_INSTALL_PREFIX} --DCMAKE_BUILD_TYPE:STRING=Build --DCMAKE_PROJECT_INCLUDE_BEFORE:FILEPATH=%{BuildConfig:BuildDirectory:NativeFilePath}/.qtc/package-manager/auto-setup.cmake + -DCMAKE_BUILD_TYPE:STRING=Build +-DCMAKE_COLOR_DIAGNOSTICS:BOOL=ON +-DCMAKE_CXX_COMPILER:FILEPATH=%{Compiler:Executable:Cxx} -DCMAKE_C_COMPILER:FILEPATH=%{Compiler:Executable:C} --DQT_QMAKE_EXECUTABLE:FILEPATH=%{Qt:qmakeExecutable} -DCMAKE_GENERATOR:STRING=Ninja --DCMAKE_CXX_COMPILER:FILEPATH=%{Compiler:Executable:Cxx} +-DQT_QMAKE_EXECUTABLE:FILEPATH=%{Qt:qmakeExecutable} +-DCMAKE_PREFIX_PATH:PATH=%{Qt:QT_INSTALL_PREFIX} +-DCMAKE_PROJECT_INCLUDE_BEFORE:FILEPATH=%{BuildConfig:BuildDirectory:NativeFilePath}/.qtc/package-manager/auto-setup.cmake /home/bartoszek/zephyrproject/zephyr/rims_app /home/bartoszek/zephyrproject/zephyr/rims_app/build diff --git a/rims_app/src/circular_buffer.hpp b/rims_app/src/circular_buffer.hpp index e71daf20845..d3a9d8af05e 100644 --- a/rims_app/src/circular_buffer.hpp +++ b/rims_app/src/circular_buffer.hpp @@ -197,6 +197,44 @@ template class circular_buffer { ++(*this); return tmp; } + + iterator operator+(difference_type n) const { + std::size_t new_index = (index_ + n) % N; + return iterator(buf_, new_index, tail_, head_); + } + + iterator operator-(difference_type n) const { + std::size_t new_index = (index_ + N - (n % N)) % N; + return iterator(buf_, new_index, tail_, head_); + } + + iterator& operator+=(difference_type n) { + index_ = (index_ + n) % N; + return *this; + } + + iterator& operator-=(difference_type n) { + index_ = (index_ + N - (n % N)) % N; + return *this; + } + + reference operator[](difference_type n) { + std::size_t idx = (index_ + n) % N; + return *reinterpret_cast(std::addressof(buf_[idx])); + } + + bool operator<(const iterator& other) const { + return (*this - other) < 0; + } + bool operator>(const iterator& other) const { + return other < *this; + } + bool operator<=(const iterator& other) const { + return !(*this > other); + } + bool operator>=(const iterator& other) const { + return !(*this < other); + } bool operator==(const iterator &other) const { return index_ == other.index_ && looped_; @@ -251,6 +289,44 @@ template class circular_buffer { ++(*this); return tmp; } + + const_iterator operator+(difference_type n) const { + std::size_t new_index = (index_ + n) % N; + return {buf_, new_index, tail_, head_}; + } + + const_iterator operator-(difference_type n) const { + std::size_t new_index = (index_ + N - (n % N)) % N; + return {buf_, new_index, tail_, head_}; + } + + const_iterator& operator+=(difference_type n) { + index_ = (index_ + n) % N; + return *this; + } + + const_iterator& operator-=(difference_type n) { + index_ = (index_ + N - (n % N)) % N; + return *this; + } + + reference operator[](difference_type n) { + std::size_t idx = (index_ + n) % N; + return *reinterpret_cast(std::addressof(buf_[idx])); + } + + bool operator<(const iterator& other) const { + return (*this - other) < 0; + } + bool operator>(const iterator& other) const { + return other < *this; + } + bool operator<=(const iterator& other) const { + return !(*this > other); + } + bool operator>=(const iterator& other) const { + return !(*this < other); + } bool operator==(const const_iterator &other) const { return index_ == other.index_ && looped_; diff --git a/rims_app/src/common.hpp b/rims_app/src/common.hpp index 39c5934f31a..20c3dcab401 100644 --- a/rims_app/src/common.hpp +++ b/rims_app/src/common.hpp @@ -203,6 +203,7 @@ class ZephyrThread { // Optionally start the thread if delayed or suspended void start() { k_thread_start(&_threadData); + k_yield(); } protected: diff --git a/rims_app/src/log.cpp b/rims_app/src/log.cpp index bbe10001e82..834181ae02d 100644 --- a/rims_app/src/log.cpp +++ b/rims_app/src/log.cpp @@ -6,7 +6,7 @@ namespace rims { K_FIFO_DEFINE(klogFifoQueue); -zephyr_fifo_buffer logEgressFifoQueueBuffer{klogFifoQueue}; +zephyr_fifo_buffer logEgressFifoQueueBuffer{klogFifoQueue}; static std::size_t g_droppedLogs{0}; diff --git a/rims_app/src/log.hpp b/rims_app/src/log.hpp index d1fa996281b..993345bf50f 100644 --- a/rims_app/src/log.hpp +++ b/rims_app/src/log.hpp @@ -20,10 +20,10 @@ enum class LogLevel : int { // }; extern zephyr_fifo_buffer logIngressFifoQueueBuffer; -extern zephyr_fifo_buffer logEgressFifoQueueBuffer; +extern zephyr_fifo_buffer logEgressFifoQueueBuffer; /// TODO move -static LogLevel g_logLevel = LogLevel::Critical; +static LogLevel g_logLevel = LogLevel::Debug; class Log { public: diff --git a/rims_app/src/main.cpp b/rims_app/src/main.cpp index 54ef0c90435..6b785b435a0 100644 --- a/rims_app/src/main.cpp +++ b/rims_app/src/main.cpp @@ -5,29 +5,21 @@ #include "placement_unique_ptr.hpp" #include "temperature_measurements.hpp" #include "uart.hpp" +#include "zephyr.hpp" #include "zero_cross_detection.hpp" +#include #include #include #include + #include #include #include #include -extern "C" void _exit() { -} - -[[noreturn]] void Terminate() noexcept { - // insert your own termination handler code here... - while (true) { - } -} - -namespace __cxxabiv1 { -std::terminate_handler __terminate_handler = Terminate; -} +#include using namespace rims; @@ -37,7 +29,7 @@ TStack messengerStack{k_messengerStack, K_THREAD_STACK_SIZEOF(k_messengerStack)} static K_THREAD_STACK_DEFINE(k_uartStack, 2048); TStack uartStack{k_uartStack, K_THREAD_STACK_SIZEOF(k_uartStack)}; -static K_THREAD_STACK_DEFINE(k_temperatureSamplerStack, 2048); +static K_THREAD_STACK_DEFINE(k_temperatureSamplerStack, 4096); TStack temperatureSamplerStack{k_temperatureSamplerStack, K_THREAD_STACK_SIZEOF(k_temperatureSamplerStack)}; static K_THREAD_STACK_DEFINE(k_zeroCrossDetectionStack, 2048); @@ -73,9 +65,15 @@ LazyInit temperatureSampler; LazyInit zcd; LazyInit phaseModulation; +gpio_dt_spec status = GPIO_DT_SPEC_GET(DT_NODELABEL(status_led), gpios); +gpio_dt_spec gprelay_1_en = GPIO_DT_SPEC_GET(DT_NODELABEL(gprelay_1_en), gpios); +gpio_dt_spec gprelay_2_en = GPIO_DT_SPEC_GET(DT_NODELABEL(gprelay_2_en), gpios); // pompka + int main() { using namespace rims; + const auto wait = []() { std::this_thread::sleep_for(std::chrono::milliseconds{100}); }; + messengerTread.init(messengerStack); uartThread.init(uartStack); temperatureSampler.init(temperatureSamplerStack); @@ -84,20 +82,33 @@ int main() { messengerTread->init_hw(); messengerTread->start(); + wait(); uartThread->init_hw(); uartThread->start(); + wait(); temperatureSampler->init_hw(); temperatureSampler->start(); + wait(); zcd->init_hw(); zcd->start(); + wait(); phaseModulation->init_hw(); phaseModulation->start(); + wait(); + + zephyr::gpio::pin_configure(status, GPIO_OUTPUT_INACTIVE); + zephyr::gpio::pin_configure(gprelay_1_en, GPIO_OUTPUT_INACTIVE); + zephyr::gpio::pin_configure(gprelay_2_en, GPIO_OUTPUT_INACTIVE); while (1) { - std::this_thread::sleep_for(std::chrono::seconds{2}); + std::this_thread::sleep_for(std::chrono::seconds{5}); + // zephyr::gpio::pin_toggle_dt(gprelay_2_en); + + // const unsigned char data[] = {'b', 0x55}; + // uart_fifo_fill(defaultUart()->_dev, data, 1); } } diff --git a/rims_app/src/messenger.cpp b/rims_app/src/messenger.cpp index b0fd7f364e6..0fd053f386e 100644 --- a/rims_app/src/messenger.cpp +++ b/rims_app/src/messenger.cpp @@ -213,7 +213,7 @@ struct request_queue_full : public error { } // namespace messenger -MessengerThread::MessengerThread(TStackBase &stack) : ZephyrThread{stack, 10, 0, "Messenger"} { +MessengerThread::MessengerThread(TStackBase &stack) : ZephyrThread{stack, 9, 0, "Messenger"} { zephyr::event_pool::k_init(_events.at(0), messenger_buffer_arrived_queue); for (auto &ar : _activeRequests) { diff --git a/rims_app/src/phase_modulation.cpp b/rims_app/src/phase_modulation.cpp index fa4e105d127..c9e0c848a1a 100644 --- a/rims_app/src/phase_modulation.cpp +++ b/rims_app/src/phase_modulation.cpp @@ -67,8 +67,8 @@ zephyr_fifo_buffer ctrlIngressQueue{ctrlIngress}; zephyr_fifo_buffer ctrlEgressQueue{ctrlEggress}; std::array pins = { - gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(triac_enable_ch1), gpios), - gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(triac_enable_ch2), gpios) + gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_en), gpios), + gpio_dt_spec GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_en), gpios) }; struct PhaseControl { @@ -140,26 +140,28 @@ int PhaseModulationThread::do_hardwarenInit() { return 0; } -// constexpr float PI = 3.141592f; -// constexpr float FREQUENCY = 0.5f; // 2 Hz -// constexpr float MIN_VALUE = 10.0f; -// constexpr float MAX_VALUE = 25.0f; +constexpr float PI = 3.141592f; +constexpr float FREQUENCY = 0.5f; // 2 Hz +constexpr float MIN_VALUE = 10.0f; +constexpr float MAX_VALUE = 25.0f; -// float sinewave(std::chrono::steady_clock::time_point timePoint) { -// using namespace std::chrono; -// static auto startTime = steady_clock::now(); +float sinewave(std::chrono::steady_clock::time_point timePoint) { + using namespace std::chrono; + static auto startTime = steady_clock::now(); -// float elapsedSeconds = duration(timePoint - startTime).count(); -// float sineValue = sinf(2.0f * PI * FREQUENCY * elapsedSeconds); + float elapsedSeconds = duration(timePoint - startTime).count(); + float sineValue = sinf(2.0f * PI * FREQUENCY * elapsedSeconds); -// // return std::sin(2.0 * PI * FREQUENCY * elapsedSeconds); -// return MIN_VALUE + (sineValue + 1.0f) * 0.5f * (MAX_VALUE - MIN_VALUE); -// } + // return std::sin(2.0 * PI * FREQUENCY * elapsedSeconds); + return MIN_VALUE + (sineValue + 1.0f) * 0.5f * (MAX_VALUE - MIN_VALUE); +} void PhaseModulationOrchestrator::event_zeroCrossDetection() { auto tickFallingEdge = [](auto &channel) { channel.tickFallingEdge(); }; auto tickRisingEdge = [](auto &channel) { channel.tickRisingEdge(); }; + auto setPower = [](auto &channel) { channel.setPower(sinewave(std::chrono::steady_clock::now())); }; + std::visit(setPower, _channel[0]); /// TODO check proper channel ZeroCrossDetectionEventQueue.try_consume([&](ZeroCrossDetectionEvent &event) { if (event.state) { diff --git a/rims_app/src/phase_modulation.hpp b/rims_app/src/phase_modulation.hpp index 47c2843738a..52a3cb5a405 100644 --- a/rims_app/src/phase_modulation.hpp +++ b/rims_app/src/phase_modulation.hpp @@ -68,6 +68,9 @@ class GroupModulation : public PhaseControlBase { void tickFallingEdge() {}; void tickRisingEdge() {}; + + void setPower(float percent) { + } void setCyclesMax(uint32_t cycles) { _cyclesMax = cycles; diff --git a/rims_app/src/temperature_measurements.cpp b/rims_app/src/temperature_measurements.cpp index c226445497c..b645701a8ae 100644 --- a/rims_app/src/temperature_measurements.cpp +++ b/rims_app/src/temperature_measurements.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -48,6 +49,29 @@ K_FIFO_DEFINE(temperatureEggress); zephyr_fifo_buffer temperatureIngressQueue{temperatureIngress}; zephyr_fifo_buffer temperatureEgressQueue{temperatureEggress}; +constexpr gpio_dt_spec rmin_en_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(rmin_en), gpios); +constexpr gpio_dt_spec rmax_en_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(rmax_en), gpios); +constexpr gpio_dt_spec rpt_en_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(rpt_en), gpios); +constexpr gpio_dt_spec temp_channel_sel_gpio_dt = GPIO_DT_SPEC_GET(DT_NODELABEL(temp_channel_sel), gpios); + +void select_rmin(){ + zephyr::gpio::pin_set_low(rmax_en_gpio_dt); + zephyr::gpio::pin_set_high(rmin_en_gpio_dt); + zephyr::gpio::pin_set_low(rpt_en_gpio_dt); +} + +void select_rmax(){ + zephyr::gpio::pin_set_high(rmax_en_gpio_dt); + zephyr::gpio::pin_set_low(rmin_en_gpio_dt); + zephyr::gpio::pin_set_low(rpt_en_gpio_dt); +} + +void select_rpt(){ + zephyr::gpio::pin_set_low(rmax_en_gpio_dt); + zephyr::gpio::pin_set_low(rmin_en_gpio_dt); + zephyr::gpio::pin_set_high(rpt_en_gpio_dt); +} + template void PB_encode_egress(const T &tempresp) { /// TODO repeat on fail temperatureEgressQueue.try_produce([&](temperature_EgressMessages &egress) { @@ -69,64 +93,109 @@ template void PB_encode_egress(const T &tempresp) { } void TemperatureSampler::take_sample() { + ULOG_DEBUG("Samples on ch %d, size %d", this->_channel, this->_samples.size()); _samples.emplace_front(adc_take_sample()); } -TemperatureStatistics TemperatureSampler::temperature_statistics() const { - const auto &samples = this->_samples; +TemperatureSampler::TemperatureSampler(uint8_t channel) + : /*_samplerSem{0, 1}, _samplerTimer{_samplerSem, std::chrono::milliseconds{125}}, */ _channel{channel} { + calibration(); +} - auto sum = std::accumulate(samples.cbegin(), samples.cend(), Sample{}); - auto mean = sum / samples.size(); - auto sq_sum = std::accumulate(samples.cbegin(), samples.cend(), Sample{}, [&](auto currentSum, auto sample) { +void TemperatureSampler::calibration() { + /// TODO Change to current channal + ULOG_INFO("Starting temp adc calibration"); + select_rmin(); + this->_adc_Tmin = takeStableRead(); + + select_rmax(); + this->_adc_Tmax = takeStableRead(); + ULOG_INFO("Calibration done, min{%d}/max{%d}", _adc_Tmin, _adc_Tmax); + + select_rpt(); +} + +void TemperatureSampler::tick() { + // if (zephyr::semaphore::k_sem_take_now(_samplerSem) == 0) { + // take_sample(); + // } +} + +BroadcastTemperatureStatistics TemperatureSampler::temperature_statistics() const { + const auto &samples = this->_samples; + std::size_t total_samples = samples.size(); + + // Use only the last N samples or all if fewer + std::size_t count = std::max(std::min(this->_samplesNumber, total_samples), 1U); + auto start_it = samples.cend() - count; + + auto sum = std::accumulate(start_it, samples.cend(), Sample{}); + auto mean = sum / count; + + auto sq_sum = std::accumulate(start_it, samples.cend(), Sample{}, [&](auto currentSum, auto sample) { return currentSum + ((sample - mean) * (sample - mean)); }); - auto stddev = std::sqrt(sq_sum / samples.size()); + auto stddev = std::sqrt(sq_sum / count); - TemperatureStatistics temp = temperature_TemperatureStatistics_init_zero; - temp.temp_c = samples.back(); - temp.temp_avg_c = mean; - temp.n_samples = samples.size(); - temp.temp_stddev_c = stddev; + BroadcastTemperatureStatistics btemp; + btemp.channel_id = this->_channel; + btemp.has_temperatureStats = true; - return temp; -}; + TemperatureStatistics &temp = btemp.temperatureStats; + temp.temp_c = samples.back(); + temp.temp_avg_c = mean; + temp.n_samples = count; + temp.temp_stddev_c = stddev; + + return btemp; +} TemperatureCurrent TemperatureSampler::temperature() const { return TemperatureCurrent{.temp_c = this->_samples.back()}; -}; - -TemperatureSampler::Sample TemperatureSampler::to_temperature(int adc_value) const { - // Constants for ADC calculation - /// TODO procedure to get VREF - const float V_REF = 3.27f; // Reference voltage - const int ADC_RESOLUTION = 4096; // 12-bit ADC resolution - - // Convert ADC value to voltage - const float voltage = (adc_value * V_REF) / (ADC_RESOLUTION - 1); - - // Linear interpolation coefficients - const float V1 = 0.0389f; // Voltage corresponding to T1 - const float T1 = 103.945f; // Temperature at V1 - const float V2 = 0.634f; // Voltage corresponding to T2 - const float T2 = 22.0f; // Temperature at V2 - - // Calculate temperature using linear interpolation - return T1 + (T2 - T1) * (voltage - V1) / (V2 - V1); } -TemperatureSampler::Sample TemperatureSampler::adc_take_sample() const { +void TemperatureSampler::setSamplerConfig(const temperature_SamplerConfig &config) noexcept(false) { + if (config.has_samples) { + ULOG_INFO("change samples num from %d to %d", this->_samplesNumber, config.samples); + this->_samplesNumber = config.samples; + } + + // if (config.has_period_ms) { + // ULOG_INFO("change period from %lld to %d", this->_samplerTimer.interval().count(), config.samples); + // this->_samplerTimer.setInterval(std::chrono::milliseconds{config.period_ms}); + // } +} + +TemperatureSampler::Sample TemperatureSampler::to_temperature(uint32_t adcReading) const { + const auto tempMin = this->_Tmin; + const auto tempMax = this->_Tmax; + const auto adcMax = this->_adc_Tmax; + const auto adcMin = this->_adc_Tmin; + + // Clamp adcReading within adcMin and adcMax range + if (adcReading < adcMax) adcReading = adcMax; + if (adcReading > adcMin) adcReading = adcMin; + + // Inverse linear interpolation + float temperature = tempMin + (float)(adcMin - adcReading) * (tempMax - tempMin) / (float)(adcMin - adcMax); + + return temperature; +} + +uint32_t TemperatureSampler::adc() const +{ std::array samples{}; // TODO add emssage for samples fer measurement constexpr auto ADC_RESOLUTION = 12; const adc_dt_spec adc_spec = ADC_DT_SPEC_GET_BY_IDX(DT_PATH(zephyr_user), 0); - + const adc_sequence_options options = { .interval_us = 0, .callback = nullptr, .user_data = nullptr, .extra_samplings = 3, }; - + adc_sequence sequence = { .options = &options, .channels = BIT(15), @@ -135,24 +204,27 @@ TemperatureSampler::Sample TemperatureSampler::adc_take_sample() const { .resolution = ADC_RESOLUTION, .oversampling = 0, }; - + auto err = adc_read_dt(&adc_spec, &sequence); if (err != 0) { ULOG_ERROR("ADC got error %d at sequence.cahnnelid %d", err, sequence.channels); return 0; } - + auto sum = std::accumulate(samples.begin(), samples.end(), 0); auto avg = sum / samples.size(); + + return avg; +} - return to_temperature(avg); +TemperatureSampler::Sample TemperatureSampler::adc_take_sample() const { + return to_temperature(adc()); } template constexpr bool unknownChannelError(const Req &req, Resp &resp) { if (req.channel_id > ChannelNumber) { - /// fixme - // resp.which_data = 254; // error_tag - // resp.data.error = temperature_Error_UnknownChannel; + resp.error = temperature_Error_UnknownChannel; + resp.has_error = true; return true; } return false; @@ -160,10 +232,10 @@ template constexpr bool unknownChannelError(const TemperatureSamplerOrchestrator::TemperatureSamplerOrchestrator() : // - _temperatureSamplerChannel{0}, // - _temperatureSamplerChannels{TemperatureSampler{1}, TemperatureSampler{2}}, // + _temperatureSamplerChannel{0}, // + _temperatureSamplerChannels{TemperatureSampler{1}, TemperatureSampler{2}}, // _samplerSem{0, 1}, // - _samplerTimer{_samplerSem, std::chrono::milliseconds{125}}, // + _samplerTimer{_samplerSem, std::chrono::milliseconds{500}}, // _broadcastSem{zephyr::semaphore::sem{0, 1}, zephyr::semaphore::sem{0, 1}}, // _broadcastTimer{ // RecurringSemaphoreTimer{_broadcastSem.at(0), std::chrono::seconds{4}}, @@ -260,12 +332,12 @@ void TemperatureSamplerOrchestrator::event_messageArrived() { resp.which_data = std::get<1>(samplerConfigHandler); genericHandler(req.data.samplerConfigRequest, resp.data.samplerConfigResponse, samplerConfigHandler); break; - + case std::get<1>(filterConfigHandler): resp.which_data = std::get<1>(filterConfigHandler); genericHandler(req.data.filterConfigRequest, resp.data.filterConfigResponse, filterConfigHandler); break; - + case std::get<1>(filterHandler): resp.which_data = std::get<1>(filterHandler); genericHandler(req.data.filterRequest, resp.data.filterResponse, filterHandler); @@ -291,7 +363,7 @@ void TemperatureSamplerOrchestrator::handle_getTemperatureRequest( // ) const { if (unknownChannelError(req, resp)) return; - resp.temperatureStats = _temperatureSamplerChannels[req.channel_id].temperature_statistics(); + resp.temperatureStats = _temperatureSamplerChannels[req.channel_id].temperature_statistics().temperatureStats; } void TemperatureSamplerOrchestrator::handle_getCurrentTemperatureRequest( @@ -299,29 +371,49 @@ void TemperatureSamplerOrchestrator::handle_getCurrentTemperatureRequest( GetCurrentTemperatureResponse &resp ) const { if (unknownChannelError(req, resp)) return; - - resp.channel_id = req.channel_id; + + resp.channel_id = req.channel_id; resp.temperatureCurrent = _temperatureSamplerChannels[req.channel_id].temperature(); } void TemperatureSamplerOrchestrator::handle_samplerConfigRequest( // const SamplerConfigRequest &req, SamplerConfigResponse &resp -) const { +) { if (unknownChannelError(req, resp)) return; if (req.has_config) { + _temperatureSamplerChannels[req.channel_id].setSamplerConfig(req.config); /// TODO apply configuration } else { /// TODO get configuration only } } +void TemperatureSamplerOrchestrator::handle_filterConfigRequest(const FilterConfigRequest &req, FilterConfigResponse &resp) const { + if (unknownChannelError(req, resp)) return; + + if (req.has_emaFilterParams) { + } + + if (req.has_kalmanFilterParams) { + } + + if (req.has_averageFilterParams) { + } + + if (not req.skipFullResponse) { + } +} + +void TemperatureSamplerOrchestrator::handle_filterRequest(const FilterRequest &req, FilterResponse &resp) const { + if (unknownChannelError(req, resp)) return; +} + void TemperatureSamplerOrchestrator::action_takeSample() { zephyr::semaphore::k_sem_take_now(_samplerSem); - _temperatureSamplerChannel = (_temperatureSamplerChannel + 1) % channelNumber; _temperatureSamplerChannels[_temperatureSamplerChannel].take_sample(); - /// TODO change channel + _temperatureSamplerChannel = (_temperatureSamplerChannel + 1) % channelNumber; } void TemperatureSamplerOrchestrator::action_sendTemperature(uint8_t ch) { @@ -365,6 +457,11 @@ int TemperatureSamplerThread::do_hardwarenInit() { auto ret = configurechannel(adc_temp); // configurechannel(adc_current_ch1); // configurechannel(adc_current_ch2); + + zephyr::gpio::pin_configure(rmax_en_gpio_dt, GPIO_OUTPUT_INACTIVE); + zephyr::gpio::pin_configure(rmin_en_gpio_dt, GPIO_OUTPUT_INACTIVE); + zephyr::gpio::pin_configure(rpt_en_gpio_dt, GPIO_OUTPUT_INACTIVE); + zephyr::gpio::pin_configure(temp_channel_sel_gpio_dt, GPIO_OUTPUT_INACTIVE); return ret; } @@ -376,14 +473,37 @@ void TemperatureSamplerThread::threadMain() { thread.loop(); } -void TemperatureSamplerOrchestrator::handle_filterConfigRequest(const FilterConfigRequest &req, FilterConfigResponse &resp) const -{ - -} +uint32_t TemperatureSampler::takeStableRead() { + const auto start = std::chrono::steady_clock::now(); + ULOG_INFO("Starting ADC stable read"); -void TemperatureSamplerOrchestrator::handle_filterRequest(const FilterRequest &req, FilterResponse &resp) const -{ - + int maxIterations = 1000; + int STABILITY_COUNT_REQUIRED = 5; + int STABILITY_THRESHOLD = 5; + + int prevAdc = 0; + uint8_t stableCount = 0; + + while (stableCount < STABILITY_COUNT_REQUIRED && maxIterations > 0) { + std::this_thread::sleep_for(std::chrono::microseconds{1000}); // small delay between samples + uint16_t adcraw = adc(); + + if (std::abs(adcraw - prevAdc) < STABILITY_THRESHOLD) { + stableCount++; + } else { + stableCount = 0; + } + + prevAdc = adcraw; + maxIterations--; + } + + const auto stop = std::chrono::steady_clock::now(); + auto ms = (stop - start).count() / 1000 / 1000; + auto itleft = (1000 - maxIterations); + ULOG_INFO("ADC stable read %d end after %lld ms in %d iterations", prevAdc, ms, itleft); + + return prevAdc; } } // namespace rims diff --git a/rims_app/src/temperature_measurements.hpp b/rims_app/src/temperature_measurements.hpp index 37a4ded00f5..2c2dde405a0 100644 --- a/rims_app/src/temperature_measurements.hpp +++ b/rims_app/src/temperature_measurements.hpp @@ -38,25 +38,40 @@ class TemperatureSampler { public: using Sample = float; - constexpr TemperatureSampler(std::uint8_t channel) : _channel{channel} { - } + TemperatureSampler(std::uint8_t channel); TemperatureSampler(const TemperatureSampler &) = delete; TemperatureSampler &operator=(const TemperatureSampler &) = delete; TemperatureSampler(TemperatureSampler &&) = delete; TemperatureSampler &operator=(TemperatureSampler &&) = delete; - - void take_sample(); - - TemperatureStatistics temperature_statistics() const; + + void calibration(); + void tick(); + + BroadcastTemperatureStatistics temperature_statistics() const; TemperatureCurrent temperature() const; - + + void take_sample(); + void setSamplerConfig(const temperature_SamplerConfig &config) noexcept(false); + protected: - Sample to_temperature(int adc_value) const; + uint32_t takeStableRead(); + + Sample to_temperature(uint32_t adc_value) const; + uint32_t adc() const; Sample adc_take_sample() const; - + + uint32_t _adc_Tmin{}; + uint32_t _adc_Tmax{}; + float _Tmin{-25.0f}; + float _Tmax{103.0f}; + + // zephyr::semaphore::sem _samplerSem{0, 1}; + // RecurringSemaphoreTimer _samplerTimer; + circular_buffer _samples; + std::size_t _samplesNumber{MaxSampleSize}; const std::uint8_t _channel{}; }; @@ -73,7 +88,7 @@ class TemperatureSamplerOrchestrator { void handle_getTemperatureRequest(const GetTemperatureRequest &req, GetTemperatureResponse &resp) const; void handle_getCurrentTemperatureRequest(const GetCurrentTemperatureRequest &req, GetCurrentTemperatureResponse &resp) const; - void handle_samplerConfigRequest(const SamplerConfigRequest &req, SamplerConfigResponse &resp) const; + void handle_samplerConfigRequest(const SamplerConfigRequest &req, SamplerConfigResponse &resp); void handle_filterConfigRequest(const FilterConfigRequest &req, FilterConfigResponse &resp) const; void handle_filterRequest(const FilterRequest &req, FilterResponse &resp) const; @@ -94,7 +109,7 @@ class TemperatureSamplerOrchestrator { class TemperatureSamplerThread : public ZephyrThread { public: - TemperatureSamplerThread(TStackBase &stack) : ZephyrThread(stack, 10, 0, "TemperatureSampler"){}; + TemperatureSamplerThread(TStackBase &stack) : ZephyrThread(stack, 15, 0, "TemperatureSampler"){}; int do_hardwarenInit() override; void threadMain() override; }; diff --git a/rims_app/src/zephyr.cpp b/rims_app/src/zephyr.cpp index dcbe1a48b7b..4447e5dfb81 100644 --- a/rims_app/src/zephyr.cpp +++ b/rims_app/src/zephyr.cpp @@ -4,7 +4,8 @@ #include void zephyr::gpio::pin_configure(const gpio_dt_spec &pin, gpio_flags_t flags, std::source_location sl) { - if (gpio_pin_configure_dt(&pin, flags) != 0) { + auto status = gpio_pin_configure_dt(&pin, flags); + if (status != 0) { rims::Log{sl}.error("%s for %s:%d failed","gpio_pin_configure_dt", pin.port->name, pin.pin); throw gpio_pin_configure_error{}; } else { diff --git a/rims_app/src/zephyr.hpp b/rims_app/src/zephyr.hpp index 9d4fda74eb9..fb495b28086 100644 --- a/rims_app/src/zephyr.hpp +++ b/rims_app/src/zephyr.hpp @@ -24,27 +24,37 @@ constexpr static k_timeout_t chronoToKTimeout(std::chrono::nanoseconds duration) return K_NSEC(duration.count()); } -namespace zephyr::gpio{ -struct error : public std::exception{ -}; +namespace zephyr::gpio { +struct error : public std::exception {}; -struct io_error : public error{ -}; +struct io_error : public error {}; -inline void pin_set_dt(const gpio_dt_spec &spec, int value){ - if(auto ret = gpio_pin_set_dt(&spec, value); ret!=0){ +inline void pin_set_dt(const gpio_dt_spec &spec, int value) { + if (auto ret = gpio_pin_set_dt(&spec, value); ret != 0) { throw io_error{}; } } -inline void pin_toggle_dt(const gpio_dt_spec &spec){ - if(auto ret = gpio_pin_toggle_dt(&spec); ret!=0){ +inline void pin_toggle_dt(const gpio_dt_spec &spec) { + if (auto ret = gpio_pin_toggle_dt(&spec); ret != 0) { throw io_error{}; } } +inline void pin_set_low(const gpio_dt_spec &spec) { + if (auto ret = gpio_pin_set_dt(&spec, 0); ret != 0) { + throw io_error{}; + } } +inline void pin_set_high(const gpio_dt_spec &spec) { + if (auto ret = gpio_pin_set_dt(&spec, 1); ret != 0) { + throw io_error{}; + } +} + +} // namespace zephyr::gpio + namespace zephyr::crc { inline uint32_t crc32_ieee(std::span data) { @@ -69,7 +79,6 @@ inline void k_init(k_poll_event &event, k_fifo &obj) { ::k_poll_event_init(&event, K_POLL_TYPE_FIFO_DATA_AVAILABLE, K_POLL_MODE_NOTIFY_ONLY, as_voidptr(obj)); } - inline int k_poll(std::span events, std::chrono::nanoseconds timeout = std::chrono::nanoseconds{0}) { return ::k_poll(events.data(), events.size(), chronoToKTimeout(timeout)); } @@ -102,7 +111,7 @@ template auto k_poll_handle(k_poll_event &event, Fn &&fn) { } // namespace zephyr::event_pool -namespace zephyr::semaphore{ +namespace zephyr::semaphore { struct sem : public ::k_sem { sem(unsigned int initial_count, unsigned int limit) { @@ -110,11 +119,11 @@ struct sem : public ::k_sem { } }; -inline int k_sem_take_now(sem &sem){ +inline int k_sem_take_now(sem &sem) { return ::k_sem_take(&sem, K_NO_WAIT); } -} +} // namespace zephyr::semaphore namespace zephyr::gpio { diff --git a/rims_app/src/zero_cross_detection.cpp b/rims_app/src/zero_cross_detection.cpp index d47838e66bf..e7d9329da32 100644 --- a/rims_app/src/zero_cross_detection.cpp +++ b/rims_app/src/zero_cross_detection.cpp @@ -13,8 +13,8 @@ #include namespace rims { -gpio_dt_spec ch1ZCD_pin_spec = GPIO_DT_SPEC_GET(DT_NODELABEL(zcd_state_1), gpios); -gpio_dt_spec ch2ZCD_pin_spec = GPIO_DT_SPEC_GET(DT_NODELABEL(zcd_state_2), gpios); +gpio_dt_spec ch1ZCD_pin_spec = GPIO_DT_SPEC_GET(DT_NODELABEL(ch1_zcd), gpios); +gpio_dt_spec ch2ZCD_pin_spec = GPIO_DT_SPEC_GET(DT_NODELABEL(ch2_zcd), gpios); static int gpio_pin_mask_to_index(uint32_t pin_mask) { std::bitset<32> bits{pin_mask}; @@ -43,7 +43,7 @@ void ZeroCrossDetection::interrupt_handler(const device *dev, struct gpio_callba ZeroCrossDetection::ZeroCrossDetection(gpio_dt_spec *gpio, ZCDFifo_t *queue, uint8_t channel) : _gpio{gpio}, _channel{channel}, _msgQueue{queue}, _intCheckSem{0, 1}, _intCheckTimer{_intCheckSem, std::chrono::seconds{1}} { ULOG_INFO("zero_cross_detection_entrypoint"); - + _intCheckTimer.start(); if (gpio_pin_interrupt_configure_dt(_gpio, GPIO_INT_EDGE_BOTH) != 0) { @@ -59,7 +59,7 @@ ZeroCrossDetection::ZeroCrossDetection(gpio_dt_spec *gpio, ZCDFifo_t *queue, uin } else { ULOG_INFO("%s pin %d@%s ok", "gpio_add_callback", _gpio->pin, gpio->port->name); } - + ULOG_INFO("zero_cross_detection_entrypoint DONE"); } @@ -79,8 +79,8 @@ void ZeroCrossDetectionOrchestrator::loop() { // std::this_thread::sleep_for(std::chrono::seconds{1}); auto ret = zephyr::event_pool::k_poll_forever(_events); if (ret == 0) { - zephyr::event_pool::k_poll_handle(_events.at(0), [&](){ event_zcdCheck(_zcd.at(0));} ); - zephyr::event_pool::k_poll_handle(_events.at(1), [&](){ event_zcdCheck(_zcd.at(1));} ); + zephyr::event_pool::k_poll_handle(_events.at(0), [&]() { event_zcdCheck(_zcd.at(0)); }); + zephyr::event_pool::k_poll_handle(_events.at(1), [&]() { event_zcdCheck(_zcd.at(1)); }); } } }