From 31ecbc1bda17dc0da89edb595327b1f714f5ce94 Mon Sep 17 00:00:00 2001 From: Bartosz Wieczorek Date: Tue, 22 Apr 2025 11:25:15 +0200 Subject: [PATCH] Fix uart logging issues --- rims_app/prj.conf | 2 +- rims_app/src/circular_buffer.hpp | 585 +++++++++++++--------- rims_app/src/main.cpp | 4 +- rims_app/src/temperature_measurements.cpp | 16 +- rims_app/src/temperature_measurements.hpp | 4 +- rims_app/src/uart.cpp | 84 ++-- rims_app/src/uart.hpp | 48 +- 7 files changed, 443 insertions(+), 300 deletions(-) diff --git a/rims_app/prj.conf b/rims_app/prj.conf index dce83b24e2c..18e701bf896 100644 --- a/rims_app/prj.conf +++ b/rims_app/prj.conf @@ -50,7 +50,7 @@ CONFIG_LOG_BACKEND_UART=n # Use UART for log output CONFIG_MAIN_STACK_SIZE=1500 # CONFIG_CRC=y -CONFIG_ASSERT=n +CONFIG_ASSERT=y #CONFIG_NUM_PREEMPT_PRIORITIES=0 diff --git a/rims_app/src/circular_buffer.hpp b/rims_app/src/circular_buffer.hpp index d3a9d8af05e..9b99345a7d8 100644 --- a/rims_app/src/circular_buffer.hpp +++ b/rims_app/src/circular_buffer.hpp @@ -1,8 +1,8 @@ #pragma once #include "zephyr.hpp" -#include "zephyr/sys/__assert.h" #include +#include #include #include @@ -11,17 +11,6 @@ namespace rims { -// template class circular_buffer_base { -// public: -// virtual ~circular_buffer_base() = default; - -// virtual const T &back() const = 0; -// virtual T &back() = 0; - -// virtual T &emplace_back(T &&item) = 0; -// virtual void pop_back() = 0; -// }; - template class static_vector { private: std::array buffer; @@ -32,7 +21,7 @@ template class static_vector { return count; } - constexpr void clean() noexcept { + constexpr void clean() noexcept { /// TODO call dtors count = 0; } @@ -60,308 +49,432 @@ template class static_vector { } }; -template class circular_buffer { +struct RingIndex { + using index_t = uint16_t; + + constexpr RingIndex(index_t n) : N{n} { + } + + constexpr void reset() { + _head = 0; + _tail = 0; + _full = false; + } + + constexpr bool empty() const { + return (!_full && _head == _tail); + } + + constexpr bool full() const { + return _full; + } + + constexpr std::size_t size() const { + if (full()) { + return N; + } + if (_head >= _tail) { + return _head - _tail; + } else { + return N + _head - _tail; + } + } + + constexpr std::size_t capacity() const { + return N; + } + + constexpr std::size_t free() const { + return capacity() - size(); + } + + constexpr index_t increment_head() { + __ASSERT(not full(), "Cannot increment head on full buffer"); + return increment_head(1); + } + + constexpr index_t increment_head(index_t n) { + __ASSERT(free() >= n, "Cannot increment head more than free space"); + _head = (_head + n) % N; + _full = _head == _tail; + return prev_head(); + } + + constexpr index_t decrement_head() { + __ASSERT(not empty(), "Cannot decrement head on empty buffer"); + _head = (_head + N - 1) % N; + _full = false; + return _head; + } + + constexpr index_t increment_tail() { + __ASSERT(!empty(), "Cannot increment tail on empty buffer"); + return increment_tail(1); + } + + constexpr index_t increment_tail(index_t n) { + __ASSERT(size() >= n, "Tail cannot pass head"); + _tail = (_tail + n) % N; + _full = false; + return prev_tail(); + } + + constexpr index_t head() const { + return _head; + } + + constexpr index_t tail() const { + return _tail; + } + + constexpr index_t firstFree() const { + __ASSERT(!full(), "Buffer is full"); + return head(); + } + + constexpr bool headAfterTail() const { + return head() >= tail(); + } + constexpr std::size_t spaceBack() const { + if (headAfterTail()) { + return capacity() - head(); + } else { + if (spaceMid() == 0) { + return capacity() - tail() - 1; + } + } + return 0; + } + + constexpr std::size_t spaceFront() const { + if (headAfterTail()) { + return tail(); + } else { + if (spaceMid() == 0) { + return head(); + } + } + return 0; + } + + constexpr std::size_t spaceMid() const { + const auto head2Tail = tail() - head(); + return head2Tail > 0 ? head2Tail : 0; + } + + using area = std::pair; + constexpr std::pair getFreeAreas() const { + __ASSERT((spaceFront() + spaceBack() + spaceMid()) == free(), "check that capacity always matches"); + + const std::size_t capacityMid = spaceMid(); + if (capacityMid) { + return {{head(), capacityMid}, {0, 0}}; + } else { + return {{head(), spaceBack()}, {0, spaceFront()}}; + } + } + + private: + constexpr index_t prev_tail() const { + return _tail == 0 ? N - 1 : _tail - 1; + } + constexpr index_t prev_head() const { + return _head == 0 ? N - 1 : _head - 1; + } + + index_t N{0}; + index_t _head{0}; + index_t _tail{0}; + bool _full{false}; +}; + +template class ring_buffer { public: - explicit circular_buffer() = default; + using value_type = T; + explicit ring_buffer() : _index{N} { + } - T &emplace_front(const T &item) { - if (size() == capacity()) { - std::destroy_at(std::addressof(buf_[head_])); - std::memset(std::addressof(buf_[head_]), 0, sizeof(T)); - } - auto at = head_; - std::construct_at(reinterpret_cast(std::addressof(buf_[head_])), item); - - if (full_) { - tail_ = (tail_ + 1) % N; + T &push_back(const T &item) { + if (full()) { + pop_front(); } - head_ = (head_ + 1) % N; - full_ = head_ == tail_; + auto at = _index.head(); + std::construct_at(reinterpret_cast(std::addressof(buf_[_index.increment_head()])), item); + return directy_at(at); } + void put_n(const T *items, std::size_t n) { + static_assert(std::is_trivially_constructible_v); + __ASSERT(_index.free() >= n, "need to have space for all items"); + + auto areas = _index.getFreeAreas(); + + __ASSERT((areas.first.second + areas.second.second) >= n, "areas should contains enaough free space to fit all items"); + + std::memcpy(buf_[areas.first.first], items, areas.first.second * sizeof(T)); + if (areas.first.second <= n) std::memcpy(buf_[areas.second.first], items + areas.first.second, areas.second.second * sizeof(T)); + + _index.increment_head(n); + } + T get() { __ASSERT_NO_MSG(not empty()); - // Read data and advance the tail (we now have a free space) - T val = std::move(*reinterpret_cast(std::addressof(buf_[tail_]))); - std::destroy_at(std::addressof(buf_[tail_])); - std::memset(std::addressof(buf_[tail_]), 0, sizeof(T)); - tail_ = (tail_ + 1) % N; - full_ = false; + // Read data and advance the tail (we now have a free space) + T val = std::move(*reinterpret_cast(std::addressof(buf_[_index.tail()]))); + destroy_at(_index.increment_tail()); + return val; } + // removes the last (youngest) element void pop_back() { - std::destroy_at(std::addressof(buf_[tail_])); - std::memset(std::addressof(buf_[tail_]), 0, sizeof(T)); - tail_ = (tail_ + 1) % N; - full_ = false; + __ASSERT_NO_MSG(not empty()); + + destroy_at(_index.decrement_head()); } + // removes the first (oldest) element void pop_front() { - std::destroy_at(std::addressof(buf_[head_])); - std::memset(std::addressof(buf_[head_]), 0, sizeof(T)); - head_ = (head_ - 1) % N; - full_ = false; + __ASSERT_NO_MSG(not empty()); + + destroy_at(_index.increment_tail()); } + // oldest element const T &front() const { - assert(!empty()); - return *begin(); + __ASSERT_NO_MSG(not empty()); + + return *cbegin(); } T &front() { - int i = head_ - 1; - if (i == -1) i = N - 1; + __ASSERT_NO_MSG(not empty()); + return *begin(); } const T &back() const { - int i = head_ - 1; - if (i == -1) i = N - 1; - return directy_at(i); + __ASSERT_NO_MSG(not empty()); + + return *std::prev(cend()); } T &back() { - int i = head_ - 1; - if (i == -1) i = N - 1; - return directy_at(i); + __ASSERT_NO_MSG(not empty()); + + return *(std::prev(end())); } constexpr void reset() { - head_ = tail_; - full_ = false; + // destroy elements? :| + _index.reset(); } - bool empty() const { - return (!full() && (head_ == tail_)); + constexpr bool empty() const { + return _index.empty(); } - bool full() const { - return full_; + constexpr bool full() const { + return _index.full(); } - std::size_t capacity() const { - return N; + constexpr std::size_t capacity() const { + return _index.capacity(); } - std::size_t size() const { - std::size_t size = N; - - if (!full()) { - if (head_ >= tail_) { - size = head_ - tail_; - } else { - size = N + head_ - tail_; - } - } - - return size; + constexpr std::size_t size() const { + return _index.size(); } - // Iterator class for circular_buffer - class iterator { + constexpr std::size_t free() const { + return _index.free(); + } + + // Iterator class for ring_buffer + template class iterator_base { public: - using iterator_category = std::forward_iterator_tag; + using iterator_category = std::random_access_iterator_tag; using value_type = T; using difference_type = std::ptrdiff_t; - using pointer = T *; - using reference = T &; + using pointer = std::conditional_t; + using reference = std::conditional_t; - iterator(std::array &buf, std::size_t index, std::size_t tail, std::size_t head) - : buf_(buf), index_(index), tail_(tail), head_(head), looped_(false) { + using iterator_t = iterator_base; + using buf_t = std::conditional_t, std::array>; + + constexpr iterator_base() = default; + constexpr iterator_base(buf_t &buf, std::size_t index, std::size_t tail, std::size_t head) + : buf_(&buf), current_(index), tail_(tail), head_(head), looped_(false) { } - reference operator*() { - return *reinterpret_cast(std::addressof(buf_[index_])); - } - - pointer operator->() { - return reinterpret_cast(std::addressof(buf_[index_])); - } - - iterator &operator++() { - index_ = (index_ + 1) % N; - // Detect if we have looped over the circular buffer - if (index_ == tail_ && looped_) { - index_ = head_; // Move iterator to end - } - if (index_ == head_) { - looped_ = true; - } - return *this; - } - - iterator operator++(int) { - iterator tmp = *this; - ++(*this); - return tmp; - } - - iterator operator+(difference_type n) const { - std::size_t new_index = (index_ + n) % N; - return iterator(buf_, new_index, tail_, head_); - } - - iterator operator-(difference_type n) const { - std::size_t new_index = (index_ + N - (n % N)) % N; - return iterator(buf_, new_index, tail_, head_); - } - - iterator& operator+=(difference_type n) { - index_ = (index_ + n) % N; - return *this; - } - - iterator& operator-=(difference_type n) { - index_ = (index_ + N - (n % N)) % N; - return *this; - } - - reference operator[](difference_type n) { - std::size_t idx = (index_ + n) % N; - return *reinterpret_cast(std::addressof(buf_[idx])); - } - - bool operator<(const iterator& other) const { - return (*this - other) < 0; - } - bool operator>(const iterator& other) const { - return other < *this; - } - bool operator<=(const iterator& other) const { - return !(*this > other); - } - bool operator>=(const iterator& other) const { - return !(*this < other); - } - - bool operator==(const iterator &other) const { - return index_ == other.index_ && looped_; - } - - bool operator!=(const iterator &other) const { - return !(*this == other); - } - - private: - std::array &buf_; - std::size_t index_; - const std::size_t tail_; - const std::size_t head_; - bool looped_; - }; - - class const_iterator { - public: - using iterator_category = std::forward_iterator_tag; - using value_type = T; - using difference_type = std::ptrdiff_t; - using pointer = const T *; - using reference = const T &; - - const_iterator(const std::array &buf, std::size_t index, std::size_t tail, std::size_t head) - : buf_(buf), index_(index), tail_(tail), head_(head), looped_(false) { - } + constexpr iterator_base(const iterator_base &rhs) = default; + constexpr iterator_base(iterator_base &&rhs) = default; + constexpr iterator_base &operator=(const iterator_base &rhs) = default; + constexpr iterator_base &operator=(iterator_base &&rhs) = default; reference operator*() const { - return *reinterpret_cast(std::addressof(buf_[index_])); + return *reinterpret_cast(std::addressof((*buf_)[current_])); } pointer operator->() const { - return reinterpret_cast(std::addressof(buf_[index_])); + return std::addressof(operator*()); } - const_iterator &operator++() { - index_ = (index_ + 1) % N; + constexpr iterator_t &operator++() { + current_ = (current_ + 1) % N; // Detect if we have looped over the circular buffer - if (index_ == tail_ && looped_) { - index_ = head_; // Move iterator to end + if (current_ == tail_ && looped_) { + current_ = head_; // Move iterator to end } - if (index_ == head_) { + if (current_ == head_) { looped_ = true; } return *this; } - const_iterator operator++(int) { - iterator tmp = *this; + constexpr iterator_t operator++(int) { + iterator_t tmp = *this; ++(*this); return tmp; } - - const_iterator operator+(difference_type n) const { - std::size_t new_index = (index_ + n) % N; - return {buf_, new_index, tail_, head_}; - } - - const_iterator operator-(difference_type n) const { - std::size_t new_index = (index_ + N - (n % N)) % N; - return {buf_, new_index, tail_, head_}; - } - - const_iterator& operator+=(difference_type n) { - index_ = (index_ + n) % N; + + constexpr iterator_t &operator--() { + if (current_ == 0) current_ = N - 1; + else current_--; + + // Detect if we have looped over the circular buffer + if (current_ == head_ && looped_) { + current_ = tail_; // Move iterator to end + } + if (current_ == tail_) { + looped_ = true; + } return *this; } - - const_iterator& operator-=(difference_type n) { - index_ = (index_ + N - (n % N)) % N; + + constexpr iterator_t operator--(int) { + iterator_t tmp = *this; + --(*this); + return tmp; + } + + constexpr bool operator==(const iterator_t &other) const { + return current_ == other.current_ && looped_; + } + + constexpr iterator_t &operator+=(difference_type n) { + current_ = (current_ + n) % N; return *this; } - - reference operator[](difference_type n) { - std::size_t idx = (index_ + n) % N; - return *reinterpret_cast(std::addressof(buf_[idx])); - } - - bool operator<(const iterator& other) const { - return (*this - other) < 0; - } - bool operator>(const iterator& other) const { - return other < *this; - } - bool operator<=(const iterator& other) const { - return !(*this > other); - } - bool operator>=(const iterator& other) const { - return !(*this < other); + + constexpr iterator_t &operator+=(const iterator_t &n) { + current_ = (current_ + n.current_) % N; + return *this; } - bool operator==(const const_iterator &other) const { - return index_ == other.index_ && looped_; + constexpr iterator_t operator+(difference_type n) const { + std::size_t new_index = (current_ + n) % N; + return iterator_t(*buf_, new_index, tail_, head_); } - bool operator!=(const const_iterator &other) const { - return !(*this == other); + constexpr friend iterator_t operator+(iterator_t it, const iterator_t &n) { + it += n; + return it; } + constexpr friend iterator_t operator+(difference_type it, const iterator_t &n) { + iterator_t tmp = n; + tmp += it; + return tmp; + } + + constexpr iterator_t &operator-=(difference_type n) { + current_ = (current_ + N - (n % N)) % N; + return *this; + } + + constexpr friend iterator_t operator-(const iterator_t &lhs, difference_type n) { + iterator_t tmp = lhs; + tmp -= n; + return tmp; + } + + constexpr friend difference_type operator-(const iterator_t &lhs, const iterator_t &rhs) { + if (lhs.current_ >= rhs.current_) { + return static_cast(lhs.current_ - rhs.current_); + } else { + return static_cast(N + lhs.current_ - rhs.current_); + } + } + + constexpr reference operator[](difference_type n) const { + return *(*this + n); + } + + // constexpr bool operator<(const iterator_t & other) const { + + // return (*this - other) < 0; + // } + // constexpr bool operator>(const iterator_t & other) const { + // return other < *this; + // } + // constexpr bool operator<=(const iterator_t & other) const { + // return !(*this > other); + // } + // constexpr bool operator>=(const iterator_t & other) const { + // return !(*this < other); + // } + // constexpr bool operator!=(const iterator_t & other) const { + // return !(*this == other); + // } + private: - const std::array &buf_; - std::size_t index_; - const std::size_t tail_; - const std::size_t head_; - bool looped_; + buf_t *buf_{nullptr}; + std::size_t current_{0}; + + std::size_t tail_{0}; + std::size_t head_{0}; + + bool looped_{false}; }; + using iterator = iterator_base; + using const_iterator = iterator_base; + + static_assert(std::bidirectional_iterator); + // static_assert(std::random_access_iterator< iterator >); + static_assert(std::bidirectional_iterator); + // static_assert(std::random_access_iterator< const_iterator >); + // Begin and end functions to return iterator iterator begin() { - return iterator(buf_, tail_, tail_, head_); + return {buf_, _index.tail(), _index.tail(), _index.head()}; } - - iterator end() { - return iterator(buf_, head_, tail_, head_); + const_iterator begin() const { + return {buf_, _index.tail(), _index.tail(), _index.head()}; } const_iterator cbegin() const { - return const_iterator(buf_, tail_, tail_, head_); + return {buf_, _index.tail(), _index.tail(), _index.head()}; } + iterator end() { + return {buf_, _index.head(), _index.tail(), _index.head()}; + } + const_iterator end() const { + return {buf_, _index.head(), _index.tail(), _index.head()}; + } const_iterator cend() const { - return const_iterator(buf_, head_, tail_, head_); + return {buf_, _index.head(), _index.tail(), _index.head()}; } private: + void destroy_at(int index) { + std::destroy_at(std::addressof(buf_[index])); + std::memset(std::addressof(buf_[index]), 0, sizeof(T)); + } + const T &directy_at(int index) const { return *reinterpret_cast(std::addressof(buf_[index])); } @@ -369,13 +482,9 @@ template class circular_buffer { return *reinterpret_cast(std::addressof(buf_[index])); } - // std::mutex mutex_; std::array buf_; - std::size_t head_ = 0; - std::size_t tail_ = 0; - bool full_{false}; + RingIndex _index; }; - class ZephyrMutex { public: ZephyrMutex() { @@ -402,8 +511,8 @@ class ZephyrMutex { ZephyrMutex &operator=(const ZephyrMutex &) = delete; }; -template class thread_safe_circular_buffer : public circular_buffer { - using base = circular_buffer; +template class thread_safe_circular_buffer : public ring_buffer { + using base = ring_buffer; public: const T &back() const { @@ -455,9 +564,9 @@ template class zephyr_fifo_buffer { try { if (not el) return false; // should be a assert fn(el->item); // consume item, fn can throw - _elements.pop_back(); // clear item from queue + _elements.pop_front(); // clear first item from queue } catch (...) { - _elements.pop_back(); + _elements.pop_front(); throw; } @@ -472,7 +581,7 @@ template class zephyr_fifo_buffer { auto tmp = ZephyrFifoElement{}; if (fn(tmp.item)) // fill new data { - auto &el = _elements.emplace_front(std::move(tmp)); + auto &el = _elements.push_back(std::move(tmp)); k_fifo_put(&_fifo, &el); // put data into a queue } @@ -481,8 +590,8 @@ template class zephyr_fifo_buffer { private: // ZephyrMutex _mutex{}; - circular_buffer, N> _elements{}; - k_fifo &_fifo; + ring_buffer, N> _elements{}; + k_fifo &_fifo; }; } // namespace rims diff --git a/rims_app/src/main.cpp b/rims_app/src/main.cpp index 6b785b435a0..657d2d38b48 100644 --- a/rims_app/src/main.cpp +++ b/rims_app/src/main.cpp @@ -29,7 +29,7 @@ TStack messengerStack{k_messengerStack, K_THREAD_STACK_SIZEOF(k_messengerStack)} static K_THREAD_STACK_DEFINE(k_uartStack, 2048); TStack uartStack{k_uartStack, K_THREAD_STACK_SIZEOF(k_uartStack)}; -static K_THREAD_STACK_DEFINE(k_temperatureSamplerStack, 4096); +static K_THREAD_STACK_DEFINE(k_temperatureSamplerStack, 2048); TStack temperatureSamplerStack{k_temperatureSamplerStack, K_THREAD_STACK_SIZEOF(k_temperatureSamplerStack)}; static K_THREAD_STACK_DEFINE(k_zeroCrossDetectionStack, 2048); @@ -105,7 +105,7 @@ int main() { zephyr::gpio::pin_configure(gprelay_2_en, GPIO_OUTPUT_INACTIVE); while (1) { - std::this_thread::sleep_for(std::chrono::seconds{5}); + std::this_thread::sleep_for(std::chrono::seconds{1}); // zephyr::gpio::pin_toggle_dt(gprelay_2_en); // const unsigned char data[] = {'b', 0x55}; diff --git a/rims_app/src/temperature_measurements.cpp b/rims_app/src/temperature_measurements.cpp index b645701a8ae..09d8513854b 100644 --- a/rims_app/src/temperature_measurements.cpp +++ b/rims_app/src/temperature_measurements.cpp @@ -20,6 +20,8 @@ #include #include "zephyr.hpp" +#include "zephyr/device.h" +#include "zephyr/sys/__assert.h" #define ADC_NODE DT_NODELABEL(adc1) #define DT_ADC_TEMP_NODELABEL DT_NODELABEL(adc_temp) @@ -94,7 +96,7 @@ template void PB_encode_egress(const T &tempresp) { void TemperatureSampler::take_sample() { ULOG_DEBUG("Samples on ch %d, size %d", this->_channel, this->_samples.size()); - _samples.emplace_front(adc_take_sample()); + _samples.push_back(adc_take_sample()); } TemperatureSampler::TemperatureSampler(uint8_t channel) @@ -424,7 +426,9 @@ void TemperatureSamplerOrchestrator::action_sendTemperature(uint8_t ch) { int TemperatureSamplerThread::do_hardwarenInit() { const device *adc = DEVICE_DT_GET(ADC_NODE); const gpio_dt_spec chsel_pin_dev = GPIO_DT_SPEC_GET(DT_NODELABEL(temp_channel_sel), gpios); - + + __ASSERT(device_is_ready(adc), "adc needs to work"); + zephyr::gpio::pin_configure(chsel_pin_dev, GPIO_OUTPUT_INACTIVE); if (!device_is_ready(adc)) { @@ -451,12 +455,12 @@ int TemperatureSamplerThread::do_hardwarenInit() { }; const adc_channel_cfg adc_temp = ADC_CHANNEL_CFG_DT(DT_NODELABEL(adc_temp)); - // adc_channel_cfg adc_current_ch1 = ADC_CHANNEL_CFG_DT(DT_NODELABEL(adc_current_ch1)); - // adc_channel_cfg adc_current_ch2 = ADC_CHANNEL_CFG_DT(DT_NODELABEL(adc_current_ch2)); + adc_channel_cfg adc_current_ch1 = ADC_CHANNEL_CFG_DT(DT_NODELABEL(adc_current_ch1)); + adc_channel_cfg adc_current_ch2 = ADC_CHANNEL_CFG_DT(DT_NODELABEL(adc_current_ch2)); auto ret = configurechannel(adc_temp); - // configurechannel(adc_current_ch1); - // configurechannel(adc_current_ch2); + configurechannel(adc_current_ch1); + configurechannel(adc_current_ch2); zephyr::gpio::pin_configure(rmax_en_gpio_dt, GPIO_OUTPUT_INACTIVE); zephyr::gpio::pin_configure(rmin_en_gpio_dt, GPIO_OUTPUT_INACTIVE); diff --git a/rims_app/src/temperature_measurements.hpp b/rims_app/src/temperature_measurements.hpp index 2c2dde405a0..29410feaf80 100644 --- a/rims_app/src/temperature_measurements.hpp +++ b/rims_app/src/temperature_measurements.hpp @@ -70,7 +70,7 @@ class TemperatureSampler { // zephyr::semaphore::sem _samplerSem{0, 1}; // RecurringSemaphoreTimer _samplerTimer; - circular_buffer _samples; + ring_buffer _samples; std::size_t _samplesNumber{MaxSampleSize}; const std::uint8_t _channel{}; @@ -109,7 +109,7 @@ class TemperatureSamplerOrchestrator { class TemperatureSamplerThread : public ZephyrThread { public: - TemperatureSamplerThread(TStackBase &stack) : ZephyrThread(stack, 15, 0, "TemperatureSampler"){}; + TemperatureSamplerThread(TStackBase &stack) : ZephyrThread(stack, 14, 0, "TemperatureSampler"){}; int do_hardwarenInit() override; void threadMain() override; }; diff --git a/rims_app/src/uart.cpp b/rims_app/src/uart.cpp index a8fbf570f60..f5650bac90d 100644 --- a/rims_app/src/uart.cpp +++ b/rims_app/src/uart.cpp @@ -3,6 +3,10 @@ #include "log.hpp" #include "messenger.hpp" #include "syscalls/uart.h" +#include "zephyr/device.h" +#include "zephyr/irq.h" +#include "zephyr/spinlock.h" +#include "zephyr/sys/__assert.h" #include #include @@ -79,26 +83,27 @@ void AsyncUART::loop() { } } +static std::size_t buffer_copy_wait {}; + // free function, only need to copy data to uart's TX_BUFFER and that's it void AsyncUART::transmit(AsyncUART *dev, std::span bytes) { if (bytes.empty()) return; + __ASSERT(bytes.size_bytes() <= dev->tx_buffer.capacity(), "for now, all bytes needs to fir in tx buffer"); - bool first = true; - for (auto byte : bytes) { - if (first) { - if (dev->tx_buffer.empty()) { - dev->tx_buffer.emplace_front(byte); - uart_irq_tx_enable(dev->_dev); // enable interrupt - continue; - } - first = false; - } - - while (dev->tx_buffer.full()) { - std::this_thread::sleep_for(std::chrono::microseconds{12}); - }; - dev->tx_buffer.emplace_front(byte); + while (dev->tx_buffer.free() < bytes.size_bytes()) { + buffer_copy_wait++; + std::this_thread::sleep_for(std::chrono::microseconds{20}); } + + k_spinlock_key_t key = k_spin_lock(&dev->tx_lock); + + __ASSERT(dev->no_copyInProgress(), "multiple copies at the same time are not allowed"); + // copy all data to TX + dev->tx_buffer.put_n(bytes.data(), bytes.size()); + // if disabled, enable tx interrupts + if (not dev->tx_irq_enabled()) dev->tx_irq_enable(); + + k_spin_unlock(&dev->tx_lock, key); } void AsyncUART::uartCallback(const device *dev, void *user_data) { @@ -107,13 +112,14 @@ void AsyncUART::uartCallback(const device *dev, void *user_data) { } void AsyncUART::uartISR() { + __ASSERT(device_is_ready(_dev), "device needs to work"); try { while (uart_irq_update(_dev) && uart_irq_is_pending(_dev)) { - if (rxHasByte()) { + if (uart_irq_rx_ready(_dev)) { readByteUart(); } if (uart_irq_tx_ready(_dev)) { - putByteUart(); + writeByteUart(); } } @@ -128,21 +134,21 @@ void AsyncUART::uartISR() { } } -bool AsyncUART::rxHasByte() const { - return uart_irq_rx_ready(_dev); -} - void AsyncUART::readByteUart() { - if (faultFlag) { - if (rxByte() == 0) faultFlag = false; + if (_faultFlag) { + if (rxByte() == 0) _faultFlag = false; } // throw on buffer overflow - else if (tx_buffer.full()) { + else if (rxBuffer().full()) { throw uart_rx_buffer_overflow{}; } // push_back returns last placed byte, if the byte is 0x00 we got end of frame else if (rxBuffer().push_back(rxByte()) == 0) { - processMessage(); + if(rxBuffer().size()>1){ + processMessage(); + }else{ + rxBuffer().clean(); + } } } @@ -175,24 +181,20 @@ void AsyncUART::switchRxBuffer() { } bool AsyncUART::txHasByte() const { - return tx_buffer.size(); + return not tx_buffer.empty(); } -bool AsyncUART::txByte(uint8_t byte) { - auto send_len = uart_fifo_fill(_dev, &byte, 1); - if (send_len != 1) { - // LOG_ERR("Drop %d bytes", rb_len - send_len); - while (1) { - /// just loop here - } +void AsyncUART::txByte(uint8_t byte) { + [[maybe_unused]] auto send_len = uart_fifo_fill(_dev, &byte, 1); + __ASSERT(send_len == 1, "fifo fill has to work as expected"); +} + +void AsyncUART::writeByteUart() { + if (txHasByte()) { + txByte(tx_buffer.get()); + } else { + tx_irq_disable(); } - return false; -} - -void AsyncUART::putByteUart() { - bool hasData = txHasByte(); - if (hasData) txByte(tx_buffer.get()); - else uart_irq_tx_disable(_dev); } void AsyncUART::handleRxBufferOverflowError(const uart_rx_buffer_overflow &e) { @@ -205,7 +207,7 @@ void AsyncUART::handleRxBufferOverflowError(const uart_rx_buffer_overflow &e) { rxBuffer().clean(); // indicate the driver to skip bytes until end of frame - faultFlag = true; + _faultFlag = true; } void AsyncUART::handleRxNotReadyError(const uart_rx_not_ready_error &e) { diff --git a/rims_app/src/uart.hpp b/rims_app/src/uart.hpp index 7fa32c04cfb..7559f80c461 100644 --- a/rims_app/src/uart.hpp +++ b/rims_app/src/uart.hpp @@ -19,27 +19,55 @@ class uart_rx_not_ready_error; class AsyncUART { using rx_buffer_t = static_vector; - using tx_buffer_t = circular_buffer; + using tx_buffer_t = ring_buffer; public: AsyncUART(); void loop(); - + static void transmit(AsyncUART *dev, std::span bytes); static void workHandler(k_work *work); static void uartCallback(const struct device *dev, void *user_data); - - void uartISR(); - const device *_dev; + void uartISR(); + + bool tx_irq_enabled() const { + return _txIrqEnabled; + } + void tx_irq_enable() { + _txIrqEnabled = true; + uart_irq_tx_enable(_dev); + } + void tx_irq_disable() { + _txIrqEnabled = false; + uart_irq_tx_disable(_dev); + } + + void beginCopy() { + _copyInProgress = true; + } + void endCopy() { + _copyInProgress = false; + } + constexpr bool copyInProgress() const { + return _copyInProgress; + } + constexpr bool no_copyInProgress() const { + return not _copyInProgress; + } + + const device *_dev; private: std::array rx_buffers; uint8_t _currentBufferIndex{}; - bool faultFlag = false; - tx_buffer_t tx_buffer; + bool _faultFlag = false; + bool _copyInProgress = false; + bool _txIrqEnabled = false; + + k_spinlock tx_lock; + tx_buffer_t tx_buffer; // ISR RX part - inline bool rxHasByte() const; inline void readByteUart(); // process byte inline uint8_t rxByte(); // low level read byte from device inline void processMessage(); @@ -47,9 +75,9 @@ class AsyncUART { inline void switchRxBuffer(); // ISR TX part - inline void putByteUart(); + inline void writeByteUart(); inline bool txHasByte() const; - inline bool txByte(uint8_t byte); // low level write byte to device + inline void txByte(uint8_t byte); // low level write byte to device // exception handlers inline void handleRxNotReadyError(const uart_rx_not_ready_error &e);