diff --git a/.clang-format b/.clang-format new file mode 100644 index 000000000..4ced28ac3 --- /dev/null +++ b/.clang-format @@ -0,0 +1,17 @@ +BasedOnStyle: LLVM +IndentWidth: 4 +TabWidth: 4 +UseTab: Never +ColumnLimit: 100 +PointerAlignment: Left +AccessModifierOffset: -4 +SortIncludes: Never +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +Cpp11BracedListStyle: true +BreakBeforeBraces: Attach +BinPackArguments: false +BinPackParameters: false +AllowAllArgumentsOnNextLine: false +AllowAllParametersOfDeclarationOnNextLine: false +AlignAfterOpenBracket: BlockIndent diff --git a/.github/workflows/format-checks.yml b/.github/workflows/format-checks.yml new file mode 100644 index 000000000..a225afb4a --- /dev/null +++ b/.github/workflows/format-checks.yml @@ -0,0 +1,37 @@ +name: Format Checks + +on: + workflow_dispatch: + pull_request: + paths: + - '**.cpp' + - '**.hpp' + - '**.c' + - '**.h' + - '.pre-commit-config.yaml' + - '.clang-format' + - '.github/workflows/format-checks.yml' + +jobs: + format: + name: Check code format + runs-on: ubuntu-24.04 + + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Setup Python + uses: actions/setup-python@v5 + with: + python-version: '3.12' + + - name: Install tools + run: | + python -m pip install --upgrade pip + python -m pip install pre-commit + sudo apt-get update + sudo apt-get install -y clang-format + + - name: Run pre-commit + run: pre-commit run --all-files --show-diff-on-failure diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 000000000..d1201404c --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,14 @@ +minimum_pre_commit_version: "3.5.0" +default_install_hook_types: + - pre-commit + - pre-push +repos: + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v17.0.6 + hooks: + - id: clang-format + files: ^(Inc/|Src/|Tests/|tools/).* + types_or: + - c + - c++ + exclude: ^STM32CubeH7/.* diff --git a/Inc/C++Utilities/CppImports.hpp b/Inc/C++Utilities/CppImports.hpp index 548936a4a..d28013f2f 100644 --- a/Inc/C++Utilities/CppImports.hpp +++ b/Inc/C++Utilities/CppImports.hpp @@ -17,13 +17,13 @@ #include #include #if defined(SIM_ON) -# ifdef __APPLE__ -# include // macOS -# else -# include // Linux/Unix -# endif +#ifdef __APPLE__ +#include // macOS #else -# include +#include // Linux/Unix +#endif +#else +#include #endif #include #include diff --git a/Inc/C++Utilities/CppUtils.hpp b/Inc/C++Utilities/CppUtils.hpp index 23c843600..f94664fee 100644 --- a/Inc/C++Utilities/CppUtils.hpp +++ b/Inc/C++Utilities/CppUtils.hpp @@ -4,32 +4,31 @@ #include "RingBuffer.hpp" #include "Stack.hpp" - namespace chrono = std::chrono; namespace placeholders = std::placeholders; using std::array; -using std::map; -using std::span; -using std::pair; using std::function; -using std::string; -using std::to_string; -using std::reference_wrapper; -using std::set; -using std::stack; -using std::nullopt; +using std::hash; +using std::integral_constant; using std::is_integral; using std::is_same; +using std::make_unique; +using std::map; +using std::move; +using std::nullopt; +using std::pair; +using std::queue; +using std::reference_wrapper; using std::remove_reference; -using std::integral_constant; +using std::set; +using std::shared_ptr; using std::snprintf; -using std::make_unique; +using std::span; +using std::stack; +using std::string; +using std::stringstream; +using std::to_string; using std::unique_ptr; -using std::shared_ptr; -using std::hash; using std::unordered_map; using std::vector; -using std::queue; -using std::stringstream; -using std::move; diff --git a/Inc/C++Utilities/RingBuffer.hpp b/Inc/C++Utilities/RingBuffer.hpp index 0f0ffbf28..18799dead 100644 --- a/Inc/C++Utilities/RingBuffer.hpp +++ b/Inc/C++Utilities/RingBuffer.hpp @@ -2,8 +2,7 @@ #include "CppImports.hpp" -template -class RingBuffer { +template class RingBuffer { std::array buffer{}; size_t stored_items{0}; @@ -11,9 +10,7 @@ class RingBuffer { size_t front{0}; size_t back{0}; - size_t move_forward(size_t origin, size_t amount) { - return (origin + amount) % N; - } + size_t move_forward(size_t origin, size_t amount) { return (origin + amount) % N; } size_t move_backward(size_t origin, size_t amount) { // N * ((amount / N) + 1) makes it so that the operation doesn't @@ -22,11 +19,12 @@ class RingBuffer { return ((origin + (N * ((amount / N) + 1))) - amount) % N; } - public: +public: RingBuffer() {} bool push(T item) { - if (is_full()) return false; + if (is_full()) + return false; buffer[front] = item; front = move_forward(front, 1); @@ -36,7 +34,8 @@ class RingBuffer { } bool pop() { - if (is_empty()) return false; + if (is_empty()) + return false; back = move_forward(back, 1); --stored_items; @@ -45,7 +44,8 @@ class RingBuffer { } bool push_pop(T item) { - if (is_empty()) return false; + if (is_empty()) + return false; buffer[front] = item; front = move_forward(front, 1); @@ -54,13 +54,11 @@ class RingBuffer { return true; } - T &operator[](size_t index) { - return buffer[move_backward(front, index + 1)]; - } + T& operator[](size_t index) { return buffer[move_backward(front, index + 1)]; } - T &last() { return buffer[back]; } + T& last() { return buffer[back]; } - T &first() { return buffer[move_backward(front, 1)]; } + T& first() { return buffer[move_backward(front, 1)]; } constexpr size_t capacity() { return N; } size_t size() { return stored_items; } diff --git a/Inc/C++Utilities/Stack.hpp b/Inc/C++Utilities/Stack.hpp index e6716a7e2..90b167d8e 100644 --- a/Inc/C++Utilities/Stack.hpp +++ b/Inc/C++Utilities/Stack.hpp @@ -15,11 +15,10 @@ * @tparam T The type of elements stored in the stack. * @tparam S The maximum number of elements. */ -template -class Stack { +template class Stack { public: Stack() : top_idx(0) {} - + bool push(const T& value) { if (top_idx < S) { data[top_idx++] = value; @@ -27,7 +26,7 @@ class Stack { } return false; } - + bool pop() { if (top_idx == 0) { return false; @@ -43,11 +42,11 @@ class Stack { } return data[top_idx - 1]; } - + size_t size() const { return top_idx; } size_t capacity() const { return S; } bool empty() const { return top_idx == 0; } - + private: T data[S]; size_t top_idx; diff --git a/Inc/C++Utilities/StaticVector.hpp b/Inc/C++Utilities/StaticVector.hpp index 76efeaa11..31b5a5c48 100644 --- a/Inc/C++Utilities/StaticVector.hpp +++ b/Inc/C++Utilities/StaticVector.hpp @@ -2,52 +2,46 @@ #include #include "ErrorHandler/ErrorHandler.hpp" -template -class StaticVector { - private: +template class StaticVector { +private: std::array data{}; size_t size_ = 0; public: constexpr StaticVector() = default; - - template - constexpr StaticVector(Args&&... args) : data{std::forward(args)...}, size_(sizeof...(args)) {} + + template + constexpr StaticVector(Args&&... args) + : data{std::forward(args)...}, size_(sizeof...(args)) {} constexpr bool operator==(const StaticVector&) const = default; - constexpr void push_back(const T& value) - { - if (size_ >= Capacity) - { + constexpr void push_back(const T& value) { + if (size_ >= Capacity) { ErrorHandler("StaticVector capacity exceeded"); return; - } + } data[size_] = value; size_++; } constexpr auto begin() { return data.begin(); } constexpr auto begin() const { return data.begin(); } - constexpr auto end() { return data.begin() + size_; } - constexpr auto end() const { return data.begin() + size_; } - + constexpr auto end() { return data.begin() + size_; } + constexpr auto end() const { return data.begin() + size_; } + constexpr const std::array& get_array() const { return data; } constexpr size_t size() const { return size_; } constexpr T* get_data() { return data.data(); } constexpr const T* get_data() const { return data.data(); } - constexpr T& operator[](size_t i) { return data[i]; } - constexpr const T& operator[](size_t i) const { return data[i]; } - constexpr bool contains(const T& value) const - { - for (size_t i = 0; i < size_; ++i) - { - if (data[i] == value) - { + constexpr T& operator[](size_t i) { return data[i]; } + constexpr const T& operator[](size_t i) const { return data[i]; } + constexpr bool contains(const T& value) const { + for (size_t i = 0; i < size_; ++i) { + if (data[i] == value) { return true; } } return false; } }; - diff --git a/Inc/HALAL/Benchmarking_toolkit/DataWatchpointTrace/DataWatchpointTrace.hpp b/Inc/HALAL/Benchmarking_toolkit/DataWatchpointTrace/DataWatchpointTrace.hpp index 31f5d3bf2..e1d5a0707 100644 --- a/Inc/HALAL/Benchmarking_toolkit/DataWatchpointTrace/DataWatchpointTrace.hpp +++ b/Inc/HALAL/Benchmarking_toolkit/DataWatchpointTrace/DataWatchpointTrace.hpp @@ -15,9 +15,9 @@ #define DEMCR_TRCENA 0x01000000 #define DWT_CTRL_CYCCNTENA 0x00000001 /* - This class is designed to study the efficiency of an algorithm - by counting the number of clock cycles required for execution. - It provides a more predictable and reliable method for measuring + This class is designed to study the efficiency of an algorithm + by counting the number of clock cycles required for execution. + It provides a more predictable and reliable method for measuring performance compared to using time-based measurements. */ /* @@ -27,32 +27,32 @@ The difference will be the number of clock cycles done in the algorithm */ class DataWatchpointTrace { - public: +public: static void start() { unlock_dwt(); reset_cnt(); } static unsigned int start_count() { - DWT->CTRL |= DWT_CTRL_CYCCNTENA; // enables the counter + DWT->CTRL |= DWT_CTRL_CYCCNTENA; // enables the counter DWT->CYCCNT = 0; return DWT->CYCCNT; } static unsigned int stop_count() { - DWT->CTRL &= ~DWT_CTRL_CYCCNTENA; // disable the counter + DWT->CTRL &= ~DWT_CTRL_CYCCNTENA; // disable the counter return DWT->CYCCNT; } static unsigned int get_count() { - return DWT->CYCCNT; // returns the current value of the counter + return DWT->CYCCNT; // returns the current value of the counter } - private: +private: static void reset_cnt() { CoreDebug->DEMCR |= DEMCR_TRCENA; - DWT->CYCCNT = 0; // reset the counter + DWT->CYCCNT = 0; // reset the counter DWT->CTRL = 0; } - static void unlock_dwt() { // unlock the dwt + static void unlock_dwt() { // unlock the dwt uint32_t lsr = DWT->LSR; if ((lsr & DWT_LSR_Present_Msk) != 0) { if ((lsr & DWT_LSR_Access_Msk) != 0) { diff --git a/Inc/HALAL/HALAL.hpp b/Inc/HALAL/HALAL.hpp index bba8c315e..ceedb7ef1 100644 --- a/Inc/HALAL/HALAL.hpp +++ b/Inc/HALAL/HALAL.hpp @@ -73,10 +73,9 @@ namespace HALAL { #ifdef STLIB_ETH -void start(MAC mac, IPV4 ip, IPV4 subnet_mask, IPV4 gateway, - UART::Peripheral &printf_peripheral); +void start(MAC mac, IPV4 ip, IPV4 subnet_mask, IPV4 gateway, UART::Peripheral& printf_peripheral); #else -void start(UART::Peripheral &printf_peripheral); +void start(UART::Peripheral& printf_peripheral); #endif } // namespace HALAL diff --git a/Inc/HALAL/HardFault/HardfaultTrace.h b/Inc/HALAL/HardFault/HardfaultTrace.h index 151ee8376..a81348b1d 100644 --- a/Inc/HALAL/HardFault/HardfaultTrace.h +++ b/Inc/HALAL/HardFault/HardfaultTrace.h @@ -5,46 +5,44 @@ #include "stm32h7xx_ll_gpio_wrapper.h" #include "stm32h7xx_ll_bus_wrapper.h" #include "stm32h7xx_ll_tim_wrapper.h" -#define METADATA_FLASH_ADDR (0x080DFD00) //Metadata pool flash address -#define HF_FLASH_ADDR (0x080C0000U) //Hard_fault_flash address -#define HF_FLAG_VALUE (0xFF00FF00U) //Flag to know if already is written information in the flash +#define METADATA_FLASH_ADDR (0x080DFD00) // Metadata pool flash address +#define HF_FLASH_ADDR (0x080C0000U) // Hard_fault_flash address +#define HF_FLAG_VALUE (0xFF00FF00U) // Flag to know if already is written information in the flash #define METADATA_FLASH_SIZE (0X100U) #define HARD_FAULT_FLASH_SIZE (0X200U) #define CALL_TRACE_MAX_DEPTH 16 typedef struct __attribute__((packed)) ContextStateFrame { - uint32_t r0; - uint32_t r1; - uint32_t r2; - uint32_t r3; - uint32_t r12; - uint32_t lr; - uint32_t return_address; - uint32_t xpsr; + uint32_t r0; + uint32_t r1; + uint32_t r2; + uint32_t r3; + uint32_t r12; + uint32_t lr; + uint32_t return_address; + uint32_t xpsr; } sContextStateFrame; - -typedef struct __attribute__((packed)) HardFaultLog{ - uint32_t HF_flag; - sContextStateFrame frame; - union{ - uint32_t cfsr; - struct CfSrFields{ - uint8_t MMFSR; - uint8_t BFSR; - uint16_t UFSR; - }fields; - }CfsrDecode; - union{ - uint32_t Nothing_Valid; - uint32_t MMAR_VALID; - uint32_t BFAR_VALID; - }fault_address; - struct __attribute__((packed)){ - uint32_t depth; - uint32_t pcs[CALL_TRACE_MAX_DEPTH]; - }CallTrace; -}HardFaultLog; // 112 bytes this estructure - +typedef struct __attribute__((packed)) HardFaultLog { + uint32_t HF_flag; + sContextStateFrame frame; + union { + uint32_t cfsr; + struct CfSrFields { + uint8_t MMFSR; + uint8_t BFSR; + uint16_t UFSR; + } fields; + } CfsrDecode; + union { + uint32_t Nothing_Valid; + uint32_t MMAR_VALID; + uint32_t BFAR_VALID; + } fault_address; + struct __attribute__((packed)) { + uint32_t depth; + uint32_t pcs[CALL_TRACE_MAX_DEPTH]; + } CallTrace; +} HardFaultLog; // 112 bytes this estructure #ifdef __cplusplus extern "C" { diff --git a/Inc/HALAL/Models/BoardID/BoardID.hpp b/Inc/HALAL/Models/BoardID/BoardID.hpp index 4a0e79409..da2384db6 100644 --- a/Inc/HALAL/Models/BoardID/BoardID.hpp +++ b/Inc/HALAL/Models/BoardID/BoardID.hpp @@ -1,8 +1,8 @@ #pragma once namespace Boards { - - enum ID{ + +enum ID { VCU, BLCU_VEHICLE, BLCU_INFRASTRUCTURE, @@ -12,8 +12,8 @@ namespace Boards { PCU, BMSA, OBCCU, - BCU, + BCU, NOBOARD - }; +}; } diff --git a/Inc/HALAL/Models/Concepts/Concepts.hpp b/Inc/HALAL/Models/Concepts/Concepts.hpp index e541ca53d..4dd87a85c 100644 --- a/Inc/HALAL/Models/Concepts/Concepts.hpp +++ b/Inc/HALAL/Models/Concepts/Concepts.hpp @@ -1,32 +1,28 @@ #pragma once #include "C++Utilities/CppUtils.hpp" -template +template concept Integral = is_integral::value; -template +template concept NotIntegral = !is_integral::value; template -concept Callable = requires(T a) -{ +concept Callable = requires(T a) { { a() }; }; template concept NotCallable = !Callable; -template -struct has_callable; +template struct has_callable; -template -struct has_callable{ - static constexpr bool value = Callable || has_callable::value; +template struct has_callable { + static constexpr bool value = Callable || has_callable::value; }; -template <> -struct has_callable<>{ - static constexpr bool value = false; +template <> struct has_callable<> { + static constexpr bool value = false; }; template @@ -36,17 +32,20 @@ template concept NotCallablePack = !CallablePack; template -concept Container = requires(T a, const T b) -{ +concept Container = requires(T a, const T b) { requires std::regular; requires std::swappable; - requires std::same_as; - requires std::same_as; + requires std::same_as; + requires std::same_as; requires std::forward_iterator; requires std::forward_iterator; requires std::signed_integral; - requires std::same_as::difference_type>; - requires std::same_as::difference_type>; + requires std::same_as< + typename T::difference_type, + typename std::iterator_traits::difference_type>; + requires std::same_as< + typename T::difference_type, + typename std::iterator_traits::difference_type>; { a.begin() } -> std::same_as; { a.end() } -> std::same_as; { b.begin() } -> std::same_as; @@ -64,58 +63,49 @@ concept NotContainer = !Container; template concept PackDerivesFrom = (std::derived_from && ...); +template struct has_container; -template struct has_container; - -template -struct has_container{ +template struct has_container { public: static constexpr bool value = Container || has_container::value; }; -template<> -struct has_container<>{ +template <> struct has_container<> { public: static constexpr bool value = false; }; -template struct total_sizeof; +template struct total_sizeof; -template -struct total_sizeof{ +template struct total_sizeof { public: static constexpr size_t value = sizeof(Type) + total_sizeof::value; }; -template<> -struct total_sizeof<>{ +template <> struct total_sizeof<> { public: static constexpr size_t value = 0; }; -template +template concept Array = std::is_array::value; -template +template concept NotArray = not Array; -template -class FollowingUint; +template class FollowingUint; -template<> -class FollowingUint{ +template <> class FollowingUint { public: - using Value = uint16_t; + using Value = uint16_t; }; -template<> -class FollowingUint{ +template <> class FollowingUint { public: - using Value = uint32_t; + using Value = uint32_t; }; -template<> -class FollowingUint{ +template <> class FollowingUint { public: - using Value = uint64_t; + using Value = uint64_t; }; diff --git a/Inc/HALAL/Models/DMA/DMA.hpp b/Inc/HALAL/Models/DMA/DMA.hpp index a9799cbb9..b2bb1a020 100644 --- a/Inc/HALAL/Models/DMA/DMA.hpp +++ b/Inc/HALAL/Models/DMA/DMA.hpp @@ -11,32 +11,31 @@ #include "stm32h7xx_hal.h" #include "HALAL/Models/MPUManager/MPUManager.hpp" - class DMA { public: - enum Stream : uint8_t { - DMA1Stream0 = 11, - DMA1Stream1 = 12, - DMA1Stream2 = 13, - DMA1Stream3 = 14, - DMA1Stream4 = 15, - DMA1Stream5 = 16, - DMA1Stream6 = 17, - DMA2Stream0 = 56, - DMA2Stream1 = 57, - DMA2Stream2 = 58, - DMA2Stream3 = 59, - DMA2Stream4 = 60, - DMA2Stream5 = 68, - DMA2Stream6 = 69, - DMA2Stream7 = 70, - }; + enum Stream : uint8_t { + DMA1Stream0 = 11, + DMA1Stream1 = 12, + DMA1Stream2 = 13, + DMA1Stream3 = 14, + DMA1Stream4 = 15, + DMA1Stream5 = 16, + DMA1Stream6 = 17, + DMA2Stream0 = 56, + DMA2Stream1 = 57, + DMA2Stream2 = 58, + DMA2Stream3 = 59, + DMA2Stream4 = 60, + DMA2Stream5 = 68, + DMA2Stream6 = 69, + DMA2Stream7 = 70, + }; - static void inscribe_stream(); - static void inscribe_stream(Stream dma_stream); - static void start(); + static void inscribe_stream(); + static void inscribe_stream(Stream dma_stream); + static void start(); private: - static vector available_streams; - static vector inscribed_streams; + static vector available_streams; + static vector inscribed_streams; }; \ No newline at end of file diff --git a/Inc/HALAL/Models/DMA/DMA2.hpp b/Inc/HALAL/Models/DMA/DMA2.hpp index 3b33c2c30..c09990432 100644 --- a/Inc/HALAL/Models/DMA/DMA2.hpp +++ b/Inc/HALAL/Models/DMA/DMA2.hpp @@ -16,386 +16,459 @@ using std::tuple; #define MAX_STREAMS 16 - extern "C" { - inline DMA_HandleTypeDef *dma_irq_table[16] = {nullptr}; +inline DMA_HandleTypeDef* dma_irq_table[16] = {nullptr}; } - namespace ST_LIB { - extern void compile_error(const char *msg); - struct DMA_Domain { - - enum class Peripheral : uint8_t {none, adc1, adc2, adc3, - i2c1, i2c2, i2c3, i2c5, - spi1, spi2, spi3, spi4, spi5, spi6, - fmac}; - - enum class Stream : uint8_t {none, dma1_stream0, dma1_stream1, dma1_stream2, dma1_stream3, - dma1_stream4, dma1_stream5, dma1_stream6, dma1_stream7, - dma2_stream0, dma2_stream1, dma2_stream2, dma2_stream3, - dma2_stream4, dma2_stream5, dma2_stream6, dma2_stream7}; - - - static inline DMA_Stream_TypeDef* stream_to_DMA_StreamTypeDef(Stream s) { - switch (s) { - case Stream::dma1_stream0: return DMA1_Stream0; - case Stream::dma1_stream1: return DMA1_Stream1; - case Stream::dma1_stream2: return DMA1_Stream2; - case Stream::dma1_stream3: return DMA1_Stream3; - case Stream::dma1_stream4: return DMA1_Stream4; - case Stream::dma1_stream5: return DMA1_Stream5; - case Stream::dma1_stream6: return DMA1_Stream6; - case Stream::dma1_stream7: return DMA1_Stream7; - - case Stream::dma2_stream0: return DMA2_Stream0; - case Stream::dma2_stream1: return DMA2_Stream1; - case Stream::dma2_stream2: return DMA2_Stream2; - case Stream::dma2_stream3: return DMA2_Stream3; - case Stream::dma2_stream4: return DMA2_Stream4; - case Stream::dma2_stream5: return DMA2_Stream5; - case Stream::dma2_stream6: return DMA2_Stream6; - case Stream::dma2_stream7: return DMA2_Stream7; - case Stream::none: return nullptr; - } - return nullptr; - } - - struct Entry { - Peripheral instance; - Stream stream; - IRQn_Type irqn; - uint8_t id; - }; - - template - struct DMA { - using domain = DMA_Domain; - - std::array e{}; - - - consteval DMA(Peripheral instance) { - static_assert(sizeof...(Ss) <= 3, "Máximo 3 streams"); - - Stream streams[] = { Ss... }; - constexpr uint8_t n = sizeof...(Ss); - - for (uint8_t j = 0; j < n; j++) { - e[j].instance = instance; - e[j].stream = streams[j]; - if (streams[j] != Stream::none) { - e[j].irqn = get_irqn(streams[j]); - } else { - e[j].irqn = (IRQn_Type)0; // Dummy value - } - e[j].id = j; - } - - } +extern void compile_error(const char* msg); +struct DMA_Domain { + + enum class Peripheral : uint8_t { + none, + adc1, + adc2, + adc3, + i2c1, + i2c2, + i2c3, + i2c5, + spi1, + spi2, + spi3, + spi4, + spi5, + spi6, + fmac + }; - template - consteval array inscribe(Ctx &ctx) const { - array indices{}; - for (size_t i = 0; i < sizeof...(Ss); i++) { - indices[i] = ctx.template add(e[i], this); - } - return indices; - } - }; - - static constexpr std::size_t max_instances {MAX_STREAMS}; - static_assert(max_instances > 0, "The number of instances must be greater than 0"); - - static inline constexpr IRQn_Type get_irqn(Stream stream) { - if (stream == Stream::dma1_stream0) return DMA1_Stream0_IRQn; - else if (stream == Stream::dma1_stream1) return DMA1_Stream1_IRQn; - else if (stream == Stream::dma1_stream2) return DMA1_Stream2_IRQn; - else if (stream == Stream::dma1_stream3) return DMA1_Stream3_IRQn; - else if (stream == Stream::dma1_stream4) return DMA1_Stream4_IRQn; - else if (stream == Stream::dma1_stream5) return DMA1_Stream5_IRQn; - else if (stream == Stream::dma1_stream6) return DMA1_Stream6_IRQn; - else if (stream == Stream::dma1_stream7) return DMA1_Stream7_IRQn; - - else if (stream == Stream::dma2_stream0) return DMA2_Stream0_IRQn; - else if (stream == Stream::dma2_stream1) return DMA2_Stream1_IRQn; - else if (stream == Stream::dma2_stream2) return DMA2_Stream2_IRQn; - else if (stream == Stream::dma2_stream3) return DMA2_Stream3_IRQn; - else if (stream == Stream::dma2_stream4) return DMA2_Stream4_IRQn; - else if (stream == Stream::dma2_stream5) return DMA2_Stream5_IRQn; - else if (stream == Stream::dma2_stream6) return DMA2_Stream6_IRQn; - else if (stream == Stream::dma2_stream7) return DMA2_Stream7_IRQn; - else if (stream == Stream::none) return (IRQn_Type)0; - else compile_error("No tiene que llegar aqui nunca, creo"); - } + enum class Stream : uint8_t { + none, + dma1_stream0, + dma1_stream1, + dma1_stream2, + dma1_stream3, + dma1_stream4, + dma1_stream5, + dma1_stream6, + dma1_stream7, + dma2_stream0, + dma2_stream1, + dma2_stream2, + dma2_stream3, + dma2_stream4, + dma2_stream5, + dma2_stream6, + dma2_stream7 + }; - static constexpr inline bool is_one_of(Peripheral instance, auto... bases) { - return ((instance == bases) || ...); + static inline DMA_Stream_TypeDef* stream_to_DMA_StreamTypeDef(Stream s) { + switch (s) { + case Stream::dma1_stream0: + return DMA1_Stream0; + case Stream::dma1_stream1: + return DMA1_Stream1; + case Stream::dma1_stream2: + return DMA1_Stream2; + case Stream::dma1_stream3: + return DMA1_Stream3; + case Stream::dma1_stream4: + return DMA1_Stream4; + case Stream::dma1_stream5: + return DMA1_Stream5; + case Stream::dma1_stream6: + return DMA1_Stream6; + case Stream::dma1_stream7: + return DMA1_Stream7; + + case Stream::dma2_stream0: + return DMA2_Stream0; + case Stream::dma2_stream1: + return DMA2_Stream1; + case Stream::dma2_stream2: + return DMA2_Stream2; + case Stream::dma2_stream3: + return DMA2_Stream3; + case Stream::dma2_stream4: + return DMA2_Stream4; + case Stream::dma2_stream5: + return DMA2_Stream5; + case Stream::dma2_stream6: + return DMA2_Stream6; + case Stream::dma2_stream7: + return DMA2_Stream7; + case Stream::none: + return nullptr; } + return nullptr; + } + + struct Entry { + Peripheral instance; + Stream stream; + IRQn_Type irqn; + uint8_t id; + }; - static constexpr inline bool is_spi(Peripheral instance) { - return is_one_of(instance, Peripheral::spi1, Peripheral::spi2, - Peripheral::spi3, Peripheral::spi4, Peripheral::spi5, Peripheral::spi6); - } + template struct DMA { + using domain = DMA_Domain; - static constexpr inline bool is_i2c(Peripheral instance) { - return is_one_of(instance, Peripheral::i2c1, Peripheral::i2c2, - Peripheral::i2c3, Peripheral::i2c5); - } + std::array e{}; - static constexpr inline bool is_adc(Peripheral instance) { - return is_one_of(instance, Peripheral::adc1, Peripheral::adc2, Peripheral::adc3); - } + consteval DMA(Peripheral instance) { + static_assert(sizeof...(Ss) <= 3, "Máximo 3 streams"); - static constexpr inline bool is_fmac(Peripheral instance) { - return instance == Peripheral::fmac; - } + Stream streams[] = {Ss...}; + constexpr uint8_t n = sizeof...(Ss); - static constexpr inline bool is_none(Peripheral instance){ - return instance == Peripheral::none; + for (uint8_t j = 0; j < n; j++) { + e[j].instance = instance; + e[j].stream = streams[j]; + if (streams[j] != Stream::none) { + e[j].irqn = get_irqn(streams[j]); + } else { + e[j].irqn = (IRQn_Type)0; // Dummy value + } + e[j].id = j; + } } - static consteval inline uint32_t get_Request(Peripheral instance, uint8_t i) { - if (instance == Peripheral::none) return DMA_REQUEST_MEM2MEM; - - if (instance == Peripheral::adc1) return DMA_REQUEST_ADC1; - if (instance == Peripheral::adc2) return DMA_REQUEST_ADC2; - if (instance == Peripheral::adc3) return DMA_REQUEST_ADC3; - - if (instance == Peripheral::i2c1 && i == 0) return DMA_REQUEST_I2C1_RX; - if (instance == Peripheral::i2c1 && i == 1) return DMA_REQUEST_I2C1_TX; - if (instance == Peripheral::i2c2 && i == 0) return DMA_REQUEST_I2C2_RX; - if (instance == Peripheral::i2c2 && i == 1) return DMA_REQUEST_I2C2_TX; - if (instance == Peripheral::i2c3 && i == 0) return DMA_REQUEST_I2C3_RX; - if (instance == Peripheral::i2c3 && i == 1) return DMA_REQUEST_I2C3_TX; - if (instance == Peripheral::i2c5 && i == 0) return DMA_REQUEST_I2C5_RX; - if (instance == Peripheral::i2c5 && i == 1) return DMA_REQUEST_I2C5_TX; - - if (instance == Peripheral::spi1 && i == 0) return DMA_REQUEST_SPI1_RX; - if (instance == Peripheral::spi1 && i == 1) return DMA_REQUEST_SPI1_TX; - if (instance == Peripheral::spi2 && i == 0) return DMA_REQUEST_SPI2_RX; - if (instance == Peripheral::spi2 && i == 1) return DMA_REQUEST_SPI2_TX; - if (instance == Peripheral::spi3 && i == 0) return DMA_REQUEST_SPI3_RX; - if (instance == Peripheral::spi3 && i == 1) return DMA_REQUEST_SPI3_TX; - if (instance == Peripheral::spi4 && i == 0) return DMA_REQUEST_SPI4_RX; - if (instance == Peripheral::spi4 && i == 1) return DMA_REQUEST_SPI4_TX; - if (instance == Peripheral::spi5 && i == 0) return DMA_REQUEST_SPI5_RX; - if (instance == Peripheral::spi5 && i == 1) return DMA_REQUEST_SPI5_TX; - - if (instance == Peripheral::fmac && i == 0) return DMA_REQUEST_MEM2MEM; - if (instance == Peripheral::fmac && i == 1) return DMA_REQUEST_FMAC_WRITE; - if (instance == Peripheral::fmac && i == 2) return DMA_REQUEST_FMAC_READ; - - compile_error("Invalid DMA request configuration"); - return 0; + template consteval array inscribe(Ctx& ctx) const { + array indices{}; + for (size_t i = 0; i < sizeof...(Ss); i++) { + indices[i] = ctx.template add(e[i], this); + } + return indices; } + }; - static consteval inline uint32_t get_Direction(Peripheral instance, uint8_t i) { - if ((is_fmac(instance) && i == 0) || instance == Peripheral::none) { - return DMA_MEMORY_TO_MEMORY; - } - else if ((is_i2c(instance) && i == 1) || - (is_spi(instance) && i == 1) || - (is_fmac(instance) && i == 1)){ - return DMA_MEMORY_TO_PERIPH; - } - return DMA_PERIPH_TO_MEMORY; + static constexpr std::size_t max_instances{MAX_STREAMS}; + static_assert(max_instances > 0, "The number of instances must be greater than 0"); + + static inline constexpr IRQn_Type get_irqn(Stream stream) { + if (stream == Stream::dma1_stream0) + return DMA1_Stream0_IRQn; + else if (stream == Stream::dma1_stream1) + return DMA1_Stream1_IRQn; + else if (stream == Stream::dma1_stream2) + return DMA1_Stream2_IRQn; + else if (stream == Stream::dma1_stream3) + return DMA1_Stream3_IRQn; + else if (stream == Stream::dma1_stream4) + return DMA1_Stream4_IRQn; + else if (stream == Stream::dma1_stream5) + return DMA1_Stream5_IRQn; + else if (stream == Stream::dma1_stream6) + return DMA1_Stream6_IRQn; + else if (stream == Stream::dma1_stream7) + return DMA1_Stream7_IRQn; + + else if (stream == Stream::dma2_stream0) + return DMA2_Stream0_IRQn; + else if (stream == Stream::dma2_stream1) + return DMA2_Stream1_IRQn; + else if (stream == Stream::dma2_stream2) + return DMA2_Stream2_IRQn; + else if (stream == Stream::dma2_stream3) + return DMA2_Stream3_IRQn; + else if (stream == Stream::dma2_stream4) + return DMA2_Stream4_IRQn; + else if (stream == Stream::dma2_stream5) + return DMA2_Stream5_IRQn; + else if (stream == Stream::dma2_stream6) + return DMA2_Stream6_IRQn; + else if (stream == Stream::dma2_stream7) + return DMA2_Stream7_IRQn; + else if (stream == Stream::none) + return (IRQn_Type)0; + else + compile_error("No tiene que llegar aqui nunca, creo"); + } + + static constexpr inline bool is_one_of(Peripheral instance, auto... bases) { + return ((instance == bases) || ...); + } + + static constexpr inline bool is_spi(Peripheral instance) { + return is_one_of( + instance, + Peripheral::spi1, + Peripheral::spi2, + Peripheral::spi3, + Peripheral::spi4, + Peripheral::spi5, + Peripheral::spi6 + ); + } + + static constexpr inline bool is_i2c(Peripheral instance) { + return is_one_of( + instance, + Peripheral::i2c1, + Peripheral::i2c2, + Peripheral::i2c3, + Peripheral::i2c5 + ); + } + + static constexpr inline bool is_adc(Peripheral instance) { + return is_one_of(instance, Peripheral::adc1, Peripheral::adc2, Peripheral::adc3); + } + + static constexpr inline bool is_fmac(Peripheral instance) { + return instance == Peripheral::fmac; + } + + static constexpr inline bool is_none(Peripheral instance) { + return instance == Peripheral::none; + } + + static consteval inline uint32_t get_Request(Peripheral instance, uint8_t i) { + if (instance == Peripheral::none) + return DMA_REQUEST_MEM2MEM; + + if (instance == Peripheral::adc1) + return DMA_REQUEST_ADC1; + if (instance == Peripheral::adc2) + return DMA_REQUEST_ADC2; + if (instance == Peripheral::adc3) + return DMA_REQUEST_ADC3; + + if (instance == Peripheral::i2c1 && i == 0) + return DMA_REQUEST_I2C1_RX; + if (instance == Peripheral::i2c1 && i == 1) + return DMA_REQUEST_I2C1_TX; + if (instance == Peripheral::i2c2 && i == 0) + return DMA_REQUEST_I2C2_RX; + if (instance == Peripheral::i2c2 && i == 1) + return DMA_REQUEST_I2C2_TX; + if (instance == Peripheral::i2c3 && i == 0) + return DMA_REQUEST_I2C3_RX; + if (instance == Peripheral::i2c3 && i == 1) + return DMA_REQUEST_I2C3_TX; + if (instance == Peripheral::i2c5 && i == 0) + return DMA_REQUEST_I2C5_RX; + if (instance == Peripheral::i2c5 && i == 1) + return DMA_REQUEST_I2C5_TX; + + if (instance == Peripheral::spi1 && i == 0) + return DMA_REQUEST_SPI1_RX; + if (instance == Peripheral::spi1 && i == 1) + return DMA_REQUEST_SPI1_TX; + if (instance == Peripheral::spi2 && i == 0) + return DMA_REQUEST_SPI2_RX; + if (instance == Peripheral::spi2 && i == 1) + return DMA_REQUEST_SPI2_TX; + if (instance == Peripheral::spi3 && i == 0) + return DMA_REQUEST_SPI3_RX; + if (instance == Peripheral::spi3 && i == 1) + return DMA_REQUEST_SPI3_TX; + if (instance == Peripheral::spi4 && i == 0) + return DMA_REQUEST_SPI4_RX; + if (instance == Peripheral::spi4 && i == 1) + return DMA_REQUEST_SPI4_TX; + if (instance == Peripheral::spi5 && i == 0) + return DMA_REQUEST_SPI5_RX; + if (instance == Peripheral::spi5 && i == 1) + return DMA_REQUEST_SPI5_TX; + + if (instance == Peripheral::fmac && i == 0) + return DMA_REQUEST_MEM2MEM; + if (instance == Peripheral::fmac && i == 1) + return DMA_REQUEST_FMAC_WRITE; + if (instance == Peripheral::fmac && i == 2) + return DMA_REQUEST_FMAC_READ; + + compile_error("Invalid DMA request configuration"); + return 0; + } + + static consteval inline uint32_t get_Direction(Peripheral instance, uint8_t i) { + if ((is_fmac(instance) && i == 0) || instance == Peripheral::none) { + return DMA_MEMORY_TO_MEMORY; + } else if ((is_i2c(instance) && i == 1) || (is_spi(instance) && i == 1) || (is_fmac(instance) && i == 1)) { + return DMA_MEMORY_TO_PERIPH; } + return DMA_PERIPH_TO_MEMORY; + } - static consteval inline uint32_t get_PeriphInc(Peripheral instance, uint8_t i) { - if ((is_fmac(instance) && i == 0) || is_none(instance)){ - return DMA_PINC_ENABLE; - } - return DMA_PINC_DISABLE; + static consteval inline uint32_t get_PeriphInc(Peripheral instance, uint8_t i) { + if ((is_fmac(instance) && i == 0) || is_none(instance)) { + return DMA_PINC_ENABLE; } + return DMA_PINC_DISABLE; + } - static consteval inline uint32_t get_MemInc(Peripheral instance, uint8_t i) { - if (is_fmac(instance) && i == 0){ - return DMA_MINC_DISABLE; - } - return DMA_MINC_ENABLE; + static consteval inline uint32_t get_MemInc(Peripheral instance, uint8_t i) { + if (is_fmac(instance) && i == 0) { + return DMA_MINC_DISABLE; } - - static consteval inline uint32_t get_PeriphDataAlignment(Peripheral instance, uint8_t i) { - if (is_spi(instance) || is_i2c(instance)){ - return DMA_PDATAALIGN_BYTE; - } - else if (is_none(instance)){ - return DMA_PDATAALIGN_WORD; - } - return DMA_PDATAALIGN_HALFWORD; + return DMA_MINC_ENABLE; + } + + static consteval inline uint32_t get_PeriphDataAlignment(Peripheral instance, uint8_t i) { + if (is_spi(instance) || is_i2c(instance)) { + return DMA_PDATAALIGN_BYTE; + } else if (is_none(instance)) { + return DMA_PDATAALIGN_WORD; + } + return DMA_PDATAALIGN_HALFWORD; + } + + static consteval inline uint32_t get_MemDataAlignment(Peripheral instance, uint8_t i) { + if (is_i2c(instance)) { + return DMA_MDATAALIGN_WORD; + } else if (is_spi(instance)) { + return DMA_MDATAALIGN_BYTE; } - static consteval inline uint32_t get_MemDataAlignment(Peripheral instance, uint8_t i) { - if (is_i2c(instance)){ - return DMA_MDATAALIGN_WORD; - } - else if (is_spi(instance)){ - return DMA_MDATAALIGN_BYTE; - } + return DMA_MDATAALIGN_HALFWORD; + } - return DMA_MDATAALIGN_HALFWORD; + static consteval inline uint32_t get_Mode(Peripheral instance, uint8_t i) { + if (is_spi(instance) || is_fmac(instance) || is_none(instance)) { + return DMA_NORMAL; } - static consteval inline uint32_t get_Mode(Peripheral instance, uint8_t i) { - if (is_spi(instance) || is_fmac(instance) || is_none(instance)){ - return DMA_NORMAL; - } - - return DMA_CIRCULAR; - } + return DMA_CIRCULAR; + } - static consteval inline uint32_t get_Priority(Peripheral instance, uint8_t i) { - if (is_fmac(instance)){ - return DMA_PRIORITY_HIGH; - } - - return DMA_PRIORITY_LOW; - } - - static consteval inline uint32_t get_FIFOMode(Peripheral instance, uint8_t i) { - if (is_fmac(instance)){ - return DMA_FIFOMODE_ENABLE; - } - return DMA_FIFOMODE_DISABLE; + static consteval inline uint32_t get_Priority(Peripheral instance, uint8_t i) { + if (is_fmac(instance)) { + return DMA_PRIORITY_HIGH; } - static consteval inline uint32_t get_FIFOThreshold(Peripheral instance, uint8_t i) { - if (is_spi(instance)){ - return DMA_FIFO_THRESHOLD_FULL; - } - return DMA_FIFO_THRESHOLD_HALFFULL; - } + return DMA_PRIORITY_LOW; + } - static consteval inline uint32_t get_MemBurst(Peripheral instance, uint8_t i) { - return DMA_MBURST_SINGLE; + static consteval inline uint32_t get_FIFOMode(Peripheral instance, uint8_t i) { + if (is_fmac(instance)) { + return DMA_FIFOMODE_ENABLE; } + return DMA_FIFOMODE_DISABLE; + } - static consteval inline uint32_t get_PeriphBurst(Peripheral instance, uint8_t i) { - return DMA_PBURST_SINGLE; + static consteval inline uint32_t get_FIFOThreshold(Peripheral instance, uint8_t i) { + if (is_spi(instance)) { + return DMA_FIFO_THRESHOLD_FULL; } + return DMA_FIFO_THRESHOLD_HALFFULL; + } - struct Config { - std::tuple - init_data{}; - }; + static consteval inline uint32_t get_MemBurst(Peripheral instance, uint8_t i) { + return DMA_MBURST_SINGLE; + } - template - static consteval std::array build(span instances) { - std::array cfgs{}; - std::array ents; - for(size_t i=0; i used_streams{}; // Defaults to false + struct Config { + std::tuple init_data{}; + }; - // First pass: process user-specified streams - for (std::size_t i = 0; i < N; ++i) { - const auto &e = ents[i]; - if (e.stream != Stream::none) { - uint8_t stream_idx = static_cast(e.stream) - 1; - if (used_streams[stream_idx]) { - compile_error("DMA stream already in use"); - } - used_streams[stream_idx] = true; + template static consteval std::array build(span instances) { + std::array cfgs{}; + std::array ents; + for (size_t i = 0; i < N; ++i) + ents[i] = instances[i]; + + std::array used_streams{}; // Defaults to false + + // First pass: process user-specified streams + for (std::size_t i = 0; i < N; ++i) { + const auto& e = ents[i]; + if (e.stream != Stream::none) { + uint8_t stream_idx = static_cast(e.stream) - 1; + if (used_streams[stream_idx]) { + compile_error("DMA stream already in use"); } + used_streams[stream_idx] = true; } + } - // Second pass: assign streams for entries with Stream::none - for (std::size_t i = 0; i < N; ++i) { - auto &e = ents[i]; - if (e.stream == Stream::none) { - bool assigned = false; - for (uint8_t j = 0; j < MAX_STREAMS; ++j) { - if (!used_streams[j]) { - e.stream = static_cast(j + 1); - e.irqn = get_irqn(e.stream); - used_streams[j] = true; - assigned = true; - break; - } - } - if (!assigned) { - compile_error("Not enough DMA streams available"); + // Second pass: assign streams for entries with Stream::none + for (std::size_t i = 0; i < N; ++i) { + auto& e = ents[i]; + if (e.stream == Stream::none) { + bool assigned = false; + for (uint8_t j = 0; j < MAX_STREAMS; ++j) { + if (!used_streams[j]) { + e.stream = static_cast(j + 1); + e.irqn = get_irqn(e.stream); + used_streams[j] = true; + assigned = true; + break; } } + if (!assigned) { + compile_error("Not enough DMA streams available"); + } } + } + for (std::size_t i = 0; i < N; ++i) { + const auto& e = ents[i]; - for (std::size_t i = 0; i < N; ++i){ - const auto &e = ents[i]; - - for (std::size_t j = 0; j < i; ++j){ - const auto &prev = ents[j]; - if (prev.stream == e.stream){ - compile_error("DMA stream already in use"); - } + for (std::size_t j = 0; j < i; ++j) { + const auto& prev = ents[j]; + if (prev.stream == e.stream) { + compile_error("DMA stream already in use"); } - - DMA_InitTypeDef DMA_InitStruct; - DMA_InitStruct.Request = get_Request(e.instance, e.id); - DMA_InitStruct.Direction = get_Direction(e.instance, e.id); - DMA_InitStruct.PeriphInc = get_PeriphInc(e.instance, e.id); - DMA_InitStruct.MemInc = get_MemInc(e.instance, e.id); - DMA_InitStruct.PeriphDataAlignment = get_PeriphDataAlignment(e.instance, e.id); - DMA_InitStruct.MemDataAlignment = get_MemDataAlignment(e.instance, e.id); - DMA_InitStruct.Mode = get_Mode(e.instance, e.id); - DMA_InitStruct.Priority = get_Priority(e.instance, e.id); - DMA_InitStruct.FIFOMode = get_FIFOMode(e.instance, e.id); - DMA_InitStruct.FIFOThreshold = get_FIFOThreshold(e.instance, e.id); - DMA_InitStruct.MemBurst = get_MemBurst(e.instance, e.id); - DMA_InitStruct.PeriphBurst = get_PeriphBurst(e.instance, e.id); - - - cfgs[i].init_data = std::make_tuple(e.instance, - DMA_InitStruct, - e.stream, - e.irqn, - e.id); } - return cfgs; + + DMA_InitTypeDef DMA_InitStruct; + DMA_InitStruct.Request = get_Request(e.instance, e.id); + DMA_InitStruct.Direction = get_Direction(e.instance, e.id); + DMA_InitStruct.PeriphInc = get_PeriphInc(e.instance, e.id); + DMA_InitStruct.MemInc = get_MemInc(e.instance, e.id); + DMA_InitStruct.PeriphDataAlignment = get_PeriphDataAlignment(e.instance, e.id); + DMA_InitStruct.MemDataAlignment = get_MemDataAlignment(e.instance, e.id); + DMA_InitStruct.Mode = get_Mode(e.instance, e.id); + DMA_InitStruct.Priority = get_Priority(e.instance, e.id); + DMA_InitStruct.FIFOMode = get_FIFOMode(e.instance, e.id); + DMA_InitStruct.FIFOThreshold = get_FIFOThreshold(e.instance, e.id); + DMA_InitStruct.MemBurst = get_MemBurst(e.instance, e.id); + DMA_InitStruct.PeriphBurst = get_PeriphBurst(e.instance, e.id); + + cfgs[i].init_data = std::make_tuple(e.instance, DMA_InitStruct, e.stream, e.irqn, e.id); } + return cfgs; + } + struct Instance { + DMA_HandleTypeDef dma; - - struct Instance { - DMA_HandleTypeDef dma; + void start(uint32_t SrcAddress, uint32_t DstAddress, uint32_t DataLength) { + HAL_DMA_Start_IT(&dma, SrcAddress, DstAddress, DataLength); + } + }; - void start(uint32_t SrcAddress, uint32_t DstAddress, uint32_t DataLength){ - HAL_DMA_Start_IT(&dma, SrcAddress, DstAddress, DataLength); - } - }; - - - template struct Init { - static inline std::array instances{}; - - static void init(std::span cfgs) { - if (N == 0) return; - __HAL_RCC_DMA1_CLK_ENABLE(); - __HAL_RCC_DMA2_CLK_ENABLE(); - for (std::size_t i = 0; i < N; ++i) { - const auto &e = cfgs[i]; - auto [instance, dma_init, stream, irqn, id] = e.init_data; - - instances[i].dma = {}; - instances[i].dma.Instance = stream_to_DMA_StreamTypeDef(stream); - instances[i].dma.Init = dma_init; - - if (HAL_DMA_Init(&instances[i].dma) != HAL_OK) { - ErrorHandler("DMA Init failed"); - } - else{ - HAL_NVIC_SetPriority(irqn, 0, 0); - HAL_NVIC_EnableIRQ(irqn); - dma_irq_table[static_cast(stream) - 1] = &instances[i].dma; - } + template struct Init { + static inline std::array instances{}; + + static void init(std::span cfgs) { + if (N == 0) + return; + __HAL_RCC_DMA1_CLK_ENABLE(); + __HAL_RCC_DMA2_CLK_ENABLE(); + for (std::size_t i = 0; i < N; ++i) { + const auto& e = cfgs[i]; + auto [instance, dma_init, stream, irqn, id] = e.init_data; + + instances[i].dma = {}; + instances[i].dma.Instance = stream_to_DMA_StreamTypeDef(stream); + instances[i].dma.Init = dma_init; + + if (HAL_DMA_Init(&instances[i].dma) != HAL_OK) { + ErrorHandler("DMA Init failed"); + } else { + HAL_NVIC_SetPriority(irqn, 0, 0); + HAL_NVIC_EnableIRQ(irqn); + dma_irq_table[static_cast(stream) - 1] = &instances[i].dma; } } - }; + } }; -} - +}; +} // namespace ST_LIB diff --git a/Inc/HALAL/Models/DataStructures/StackTuple.hpp b/Inc/HALAL/Models/DataStructures/StackTuple.hpp index 136eae436..eac3ac80c 100644 --- a/Inc/HALAL/Models/DataStructures/StackTuple.hpp +++ b/Inc/HALAL/Models/DataStructures/StackTuple.hpp @@ -2,24 +2,21 @@ #include "C++Utilities/CppUtils.hpp" -template class stack_tuple; +template class stack_tuple; -template<> -class stack_tuple<> { +template <> class stack_tuple<> { public: stack_tuple() = default; - template - void for_each(FunctionType function) {} + template void for_each(FunctionType function) {} }; -template -class stack_tuple: public stack_tuple { +template +class stack_tuple : public stack_tuple { public: Type value; stack_tuple() = default; - stack_tuple(Type value, Types... values): stack_tuple(values...), value(value) {} - template - void for_each(FunctionType function) { + stack_tuple(Type value, Types... values) : stack_tuple(values...), value(value) {} + template void for_each(FunctionType function) { function(value); stack_tuple::for_each(function); } diff --git a/Inc/HALAL/Models/DataStructures/TemplatePackFilter.hpp b/Inc/HALAL/Models/DataStructures/TemplatePackFilter.hpp index d8a0b5d75..02d0a2e01 100644 --- a/Inc/HALAL/Models/DataStructures/TemplatePackFilter.hpp +++ b/Inc/HALAL/Models/DataStructures/TemplatePackFilter.hpp @@ -1,25 +1,25 @@ #pragma once #include "C++Utilities/CppUtils.hpp" -template -struct TemplatePack{ +template struct TemplatePack { using Pack = Types...; }; -template -struct TemplatePackFilter; +template struct TemplatePackFilter; -template requires requires { requires !is_same::value; } -struct TemplatePackFilter{ +template + requires requires { requires !is_same::value; } +struct TemplatePackFilter { using FilteredPack = typename TemplatePackFilter::FilteredPack; }; -template -struct TemplatePackFilter{ - using FilteredPack = TemplatePack::FilteredPack>::Pack; +template +struct TemplatePackFilter { + using FilteredPack = TemplatePack< + FilterType, + typename TemplatePackFilter::FilteredPack>::Pack; }; -template -struct TemplatePackFilter{ +template struct TemplatePackFilter { using FilteredPack = TemplatePack<>::Pack; }; diff --git a/Inc/HALAL/Models/GPIO.hpp b/Inc/HALAL/Models/GPIO.hpp index 2540089b7..c8aaaa0c8 100644 --- a/Inc/HALAL/Models/GPIO.hpp +++ b/Inc/HALAL/Models/GPIO.hpp @@ -11,264 +11,268 @@ using std::span; using std::tuple; namespace ST_LIB { -extern void compile_error(const char *msg); +extern void compile_error(const char* msg); struct GPIODomain { - enum class OperationMode : uint8_t { - INPUT, // GPIO_MODE_INPUT - OUTPUT_PUSHPULL, // GPIO_MODE_OUTPUT_PP - OUTPUT_OPENDRAIN, // GPIO_MODE_OUTPUT_OD - ANALOG, // GPIO_MODE_ANALOG - ALT_PP, // GPIO_MODE_AF_PP - ALT_OD, // GPIO_MODE_AF_OD - EXTI_RISING, // GPIO_MODE_IT_RISING - EXTI_FALLING, // GPIO_MODE_IT_FALLING - EXTI_RISING_FALLING, // GPIO_MODE_IT_RISING_FALLING - }; - static constexpr uint32_t to_hal_mode(OperationMode m) { - switch (m) { - case OperationMode::INPUT: - return GPIO_MODE_INPUT; - case OperationMode::OUTPUT_PUSHPULL: - return GPIO_MODE_OUTPUT_PP; - case OperationMode::OUTPUT_OPENDRAIN: - return GPIO_MODE_OUTPUT_OD; - case OperationMode::ANALOG: - return GPIO_MODE_ANALOG; - case OperationMode::ALT_PP: - return GPIO_MODE_AF_PP; - case OperationMode::ALT_OD: - return GPIO_MODE_AF_OD; - case OperationMode::EXTI_RISING: - return GPIO_MODE_IT_RISING; - case OperationMode::EXTI_FALLING: - return GPIO_MODE_IT_FALLING; - case OperationMode::EXTI_RISING_FALLING: - return GPIO_MODE_IT_RISING_FALLING; - } - } - enum class Pull : uint8_t { None, Up, Down }; - static constexpr uint32_t to_hal_pull(Pull p) { - switch (p) { - case Pull::None: - return GPIO_NOPULL; - case Pull::Up: - return GPIO_PULLUP; - case Pull::Down: - return GPIO_PULLDOWN; - } - } - enum class Speed : uint8_t { Low, Medium, High, VeryHigh }; - static constexpr uint32_t to_hal_speed(Speed s) { - switch (s) { - case Speed::Low: - return GPIO_SPEED_FREQ_LOW; - case Speed::Medium: - return GPIO_SPEED_FREQ_MEDIUM; - case Speed::High: - return GPIO_SPEED_FREQ_HIGH; - case Speed::VeryHigh: - return GPIO_SPEED_FREQ_VERY_HIGH; - } - } - // Note: AF mapping is inverted: AF0 -> 15, AF1 -> 14, ..., AF15 -> 0, it is staticly casted and inverted later - enum class AlternateFunction : uint8_t { - NO_AF = 20, - AF0 = 15, - AF1 = 14, - AF2 = 13, - AF3 = 12, - AF4 = 11, - AF5 = 10, - AF6 = 9, - AF7 = 8, - AF8 = 7, - AF9 = 6, - AF10 = 5, - AF11 = 4, - AF12 = 3, - AF13 = 2, - AF14 = 1, - AF15 = 0 - }; - enum class Port : uint8_t { A, B, C, D, E, F, G, H }; - static inline GPIO_TypeDef *port_to_reg(Port p) { - switch (p) { - case Port::A: - return GPIOA; - case Port::B: - return GPIOB; - case Port::C: - return GPIOC; - case Port::D: - return GPIOD; - case Port::E: - return GPIOE; - case Port::F: - return GPIOF; - case Port::G: - return GPIOG; - case Port::H: - return GPIOH; - default: - return nullptr; - } - } - static inline void enable_gpio_clock(Port port) { - switch (port) { - case Port::A: - __HAL_RCC_GPIOA_CLK_ENABLE(); - break; - - case Port::B: - __HAL_RCC_GPIOB_CLK_ENABLE(); - break; - - case Port::C: - __HAL_RCC_GPIOC_CLK_ENABLE(); - break; - - case Port::D: - __HAL_RCC_GPIOD_CLK_ENABLE(); - break; - - case Port::E: - __HAL_RCC_GPIOE_CLK_ENABLE(); - break; - - case Port::F: - __HAL_RCC_GPIOF_CLK_ENABLE(); - break; - - case Port::G: - __HAL_RCC_GPIOG_CLK_ENABLE(); - break; - - case Port::H: - __HAL_RCC_GPIOH_CLK_ENABLE(); - break; + enum class OperationMode : uint8_t { + INPUT, // GPIO_MODE_INPUT + OUTPUT_PUSHPULL, // GPIO_MODE_OUTPUT_PP + OUTPUT_OPENDRAIN, // GPIO_MODE_OUTPUT_OD + ANALOG, // GPIO_MODE_ANALOG + ALT_PP, // GPIO_MODE_AF_PP + ALT_OD, // GPIO_MODE_AF_OD + EXTI_RISING, // GPIO_MODE_IT_RISING + EXTI_FALLING, // GPIO_MODE_IT_FALLING + EXTI_RISING_FALLING, // GPIO_MODE_IT_RISING_FALLING + }; + static constexpr uint32_t to_hal_mode(OperationMode m) { + switch (m) { + case OperationMode::INPUT: + return GPIO_MODE_INPUT; + case OperationMode::OUTPUT_PUSHPULL: + return GPIO_MODE_OUTPUT_PP; + case OperationMode::OUTPUT_OPENDRAIN: + return GPIO_MODE_OUTPUT_OD; + case OperationMode::ANALOG: + return GPIO_MODE_ANALOG; + case OperationMode::ALT_PP: + return GPIO_MODE_AF_PP; + case OperationMode::ALT_OD: + return GPIO_MODE_AF_OD; + case OperationMode::EXTI_RISING: + return GPIO_MODE_IT_RISING; + case OperationMode::EXTI_FALLING: + return GPIO_MODE_IT_FALLING; + case OperationMode::EXTI_RISING_FALLING: + return GPIO_MODE_IT_RISING_FALLING; + } } - } - - struct Pin { - GPIODomain::Port port; - uint32_t pin; - uint16_t afs; - - inline constexpr bool valid_af(const AlternateFunction af) const { - if (af == AlternateFunction::NO_AF) - return true; - return ((1 << static_cast(af)) & afs) != 0; + enum class Pull : uint8_t { None, Up, Down }; + static constexpr uint32_t to_hal_pull(Pull p) { + switch (p) { + case Pull::None: + return GPIO_NOPULL; + case Pull::Up: + return GPIO_PULLUP; + case Pull::Down: + return GPIO_PULLDOWN; + } } - - constexpr bool operator==(const Pin &other) const { - return (port == other.port) && (pin == other.pin); + enum class Speed : uint8_t { Low, Medium, High, VeryHigh }; + static constexpr uint32_t to_hal_speed(Speed s) { + switch (s) { + case Speed::Low: + return GPIO_SPEED_FREQ_LOW; + case Speed::Medium: + return GPIO_SPEED_FREQ_MEDIUM; + case Speed::High: + return GPIO_SPEED_FREQ_HIGH; + case Speed::VeryHigh: + return GPIO_SPEED_FREQ_VERY_HIGH; + } } - - constexpr bool operator!=(const Pin &other) const { - return !(*this == other); + // Note: AF mapping is inverted: AF0 -> 15, AF1 -> 14, ..., AF15 -> 0, it is staticly casted and + // inverted later + enum class AlternateFunction : uint8_t { + NO_AF = 20, + AF0 = 15, + AF1 = 14, + AF2 = 13, + AF3 = 12, + AF4 = 11, + AF5 = 10, + AF6 = 9, + AF7 = 8, + AF8 = 7, + AF9 = 6, + AF10 = 5, + AF11 = 4, + AF12 = 3, + AF13 = 2, + AF14 = 1, + AF15 = 0 + }; + enum class Port : uint8_t { A, B, C, D, E, F, G, H }; + static inline GPIO_TypeDef* port_to_reg(Port p) { + switch (p) { + case Port::A: + return GPIOA; + case Port::B: + return GPIOB; + case Port::C: + return GPIOC; + case Port::D: + return GPIOD; + case Port::E: + return GPIOE; + case Port::F: + return GPIOF; + case Port::G: + return GPIOG; + case Port::H: + return GPIOH; + default: + return nullptr; + } } - }; - - struct Entry { - Port port; - uint32_t pin; - OperationMode mode; - Pull pull; - Speed speed; - AlternateFunction af; - }; - - struct GPIO { - using domain = GPIODomain; - - Entry e; - - consteval GPIO(const Pin &pin, OperationMode mode, Pull pull, Speed speed, - AlternateFunction af = AlternateFunction::NO_AF) - : e{pin.port, pin.pin, mode, pull, speed, af} { - if (!pin.valid_af(af)) { - compile_error("Alternate function not valid for this pin"); - } - - if ((mode == OperationMode::ALT_PP || mode == OperationMode::ALT_OD) && af == AlternateFunction::NO_AF) { - compile_error("Alternate function must be specified for alternate modes"); - } + static inline void enable_gpio_clock(Port port) { + switch (port) { + case Port::A: + __HAL_RCC_GPIOA_CLK_ENABLE(); + break; + + case Port::B: + __HAL_RCC_GPIOB_CLK_ENABLE(); + break; + + case Port::C: + __HAL_RCC_GPIOC_CLK_ENABLE(); + break; + + case Port::D: + __HAL_RCC_GPIOD_CLK_ENABLE(); + break; + + case Port::E: + __HAL_RCC_GPIOE_CLK_ENABLE(); + break; + + case Port::F: + __HAL_RCC_GPIOF_CLK_ENABLE(); + break; + + case Port::G: + __HAL_RCC_GPIOG_CLK_ENABLE(); + break; + + case Port::H: + __HAL_RCC_GPIOH_CLK_ENABLE(); + break; + } } - template consteval std::size_t inscribe(Ctx &ctx) const { - return ctx.template add(e, this); - } - }; + struct Pin { + GPIODomain::Port port; + uint32_t pin; + uint16_t afs; - static constexpr std::size_t max_instances{110}; + inline constexpr bool valid_af(const AlternateFunction af) const { + if (af == AlternateFunction::NO_AF) + return true; + return ((1 << static_cast(af)) & afs) != 0; + } - struct Config { - std::tuple init_data{}; - }; + constexpr bool operator==(const Pin& other) const { + return (port == other.port) && (pin == other.pin); + } - template - static consteval array build(span pins) { - array cfgs{}; - for (std::size_t i = 0; i < N; ++i) { - const auto &e = pins[i]; + constexpr bool operator!=(const Pin& other) const { return !(*this == other); } + }; + + struct Entry { + Port port; + uint32_t pin; + OperationMode mode; + Pull pull; + Speed speed; + AlternateFunction af; + }; + + struct GPIO { + using domain = GPIODomain; + + Entry e; + + consteval GPIO( + const Pin& pin, + OperationMode mode, + Pull pull, + Speed speed, + AlternateFunction af = AlternateFunction::NO_AF + ) + : e{pin.port, pin.pin, mode, pull, speed, af} { + if (!pin.valid_af(af)) { + compile_error("Alternate function not valid for this pin"); + } + + if ((mode == OperationMode::ALT_PP || mode == OperationMode::ALT_OD) && + af == AlternateFunction::NO_AF) { + compile_error("Alternate function must be specified for alternate modes"); + } + } - for (std::size_t j = 0; j < i; ++j) { - const auto &prev = pins[j]; - if (prev.pin == e.pin && prev.port == e.port) { - compile_error("GPIO already inscribed"); + template consteval std::size_t inscribe(Ctx& ctx) const { + return ctx.template add(e, this); + } + }; + + static constexpr std::size_t max_instances{110}; + + struct Config { + std::tuple init_data{}; + }; + + template static consteval array build(span pins) { + array cfgs{}; + for (std::size_t i = 0; i < N; ++i) { + const auto& e = pins[i]; + + for (std::size_t j = 0; j < i; ++j) { + const auto& prev = pins[j]; + if (prev.pin == e.pin && prev.port == e.port) { + compile_error("GPIO already inscribed"); + } + } + + GPIO_InitTypeDef GPIO_InitStruct{}; + GPIO_InitStruct.Pin = e.pin; + GPIO_InitStruct.Mode = to_hal_mode(e.mode); + GPIO_InitStruct.Pull = to_hal_pull(e.pull); + GPIO_InitStruct.Speed = to_hal_speed(e.speed); + if (e.mode == OperationMode::ALT_PP || e.mode == OperationMode::ALT_OD) { + GPIO_InitStruct.Alternate = + 15 - static_cast(e.af); // AF mapping inversion + } + + cfgs[i].init_data = std::make_tuple(e.port, GPIO_InitStruct); } - } - - GPIO_InitTypeDef GPIO_InitStruct{}; - GPIO_InitStruct.Pin = e.pin; - GPIO_InitStruct.Mode = to_hal_mode(e.mode); - GPIO_InitStruct.Pull = to_hal_pull(e.pull); - GPIO_InitStruct.Speed = to_hal_speed(e.speed); - if (e.mode == OperationMode::ALT_PP || e.mode == OperationMode::ALT_OD) { - GPIO_InitStruct.Alternate = 15 - static_cast(e.af); // AF mapping inversion - } - - cfgs[i].init_data = std::make_tuple(e.port, GPIO_InitStruct); - } - return cfgs; - } + return cfgs; + } - // Runtime object - struct Instance { - private: - GPIO_TypeDef *port; - uint32_t pin; + // Runtime object + struct Instance { + private: + GPIO_TypeDef* port; + uint32_t pin; - public: - constexpr Instance() : port{nullptr}, pin{0} {} - Instance(GPIO_TypeDef *p, uint32_t pin) - : port{p}, pin{static_cast(pin)} {} + public: + constexpr Instance() : port{nullptr}, pin{0} {} + Instance(GPIO_TypeDef* p, uint32_t pin) : port{p}, pin{static_cast(pin)} {} - void turn_on() { HAL_GPIO_WritePin(port, pin, GPIO_PIN_SET); } + void turn_on() { HAL_GPIO_WritePin(port, pin, GPIO_PIN_SET); } - void turn_off() { HAL_GPIO_WritePin(port, pin, GPIO_PIN_RESET); } + void turn_off() { HAL_GPIO_WritePin(port, pin, GPIO_PIN_RESET); } - void toggle() { HAL_GPIO_TogglePin(port, pin); } + void toggle() { HAL_GPIO_TogglePin(port, pin); } - GPIO_PinState read() { return HAL_GPIO_ReadPin(port, pin); } - }; + GPIO_PinState read() { return HAL_GPIO_ReadPin(port, pin); } + }; - template struct Init { - static inline std::array instances{}; + template struct Init { + static inline std::array instances{}; - static void init(std::span cfgs) { - for (std::size_t i = 0; i < N; ++i) { - const auto &e = cfgs[i]; - auto [port, gpio_init] = e.init_data; + static void init(std::span cfgs) { + for (std::size_t i = 0; i < N; ++i) { + const auto& e = cfgs[i]; + auto [port, gpio_init] = e.init_data; - enable_gpio_clock(port); - HAL_GPIO_Init(port_to_reg(port), &gpio_init); + enable_gpio_clock(port); + HAL_GPIO_Init(port_to_reg(port), &gpio_init); - instances[i] = Instance{port_to_reg(port), gpio_init.Pin}; - } - } - }; + instances[i] = Instance{port_to_reg(port), gpio_init.Pin}; + } + } + }; }; } // namespace ST_LIB diff --git a/Inc/HALAL/Models/HALconfig/HALconfig.hpp b/Inc/HALAL/Models/HALconfig/HALconfig.hpp index 54ee4928b..d8ea1fd3b 100644 --- a/Inc/HALAL/Models/HALconfig/HALconfig.hpp +++ b/Inc/HALAL/Models/HALconfig/HALconfig.hpp @@ -10,13 +10,10 @@ #include "stm32h7xx_hal.h" #include "ErrorHandler/ErrorHandler.hpp" -enum TARGET { - Nucleo, - Board -}; +enum TARGET { Nucleo, Board }; namespace HALconfig { void system_clock(); void peripheral_clock(); -} +} // namespace HALconfig diff --git a/Inc/HALAL/Models/IPV4/IPV4.hpp b/Inc/HALAL/Models/IPV4/IPV4.hpp index 3fa823038..43b6c15d3 100644 --- a/Inc/HALAL/Models/IPV4/IPV4.hpp +++ b/Inc/HALAL/Models/IPV4/IPV4.hpp @@ -14,21 +14,20 @@ #ifdef HAL_ETH_MODULE_ENABLED - -using std::stringstream; using std::getline; +using std::stringstream; -class IPV4{ +class IPV4 { public: - ip_addr_t address; - string string_address; + ip_addr_t address; + string string_address; - IPV4(); - IPV4(const char* address); - IPV4(string address); - IPV4(ip_addr_t address); + IPV4(); + IPV4(const char* address); + IPV4(string address); + IPV4(ip_addr_t address); - void operator=(const char* address); + void operator=(const char* address); }; #endif diff --git a/Inc/HALAL/Models/LowPowerTimer/LowPowerTimer.hpp b/Inc/HALAL/Models/LowPowerTimer/LowPowerTimer.hpp index e16f79567..eac345319 100644 --- a/Inc/HALAL/Models/LowPowerTimer/LowPowerTimer.hpp +++ b/Inc/HALAL/Models/LowPowerTimer/LowPowerTimer.hpp @@ -10,27 +10,31 @@ #include #include - #include "stm32h7xx_hal.h" #ifdef HAL_LPTIM_MODULE_ENABLED -using std::reference_wrapper; using std::map; +using std::reference_wrapper; using std::string; class LowPowerTimer { public: - LPTIM_TypeDef& instance; - LPTIM_HandleTypeDef& handle; - uint16_t period; - string name; - - LowPowerTimer() = default; - LowPowerTimer(LPTIM_TypeDef& instance, LPTIM_HandleTypeDef& handle, uint16_t period, string name) : - instance(instance), handle(handle), period(period), name(name) {}; - - void init(); + LPTIM_TypeDef& instance; + LPTIM_HandleTypeDef& handle; + uint16_t period; + string name; + + LowPowerTimer() = default; + LowPowerTimer( + LPTIM_TypeDef& instance, + LPTIM_HandleTypeDef& handle, + uint16_t period, + string name + ) + : instance(instance), handle(handle), period(period), name(name){}; + + void init(); }; #endif diff --git a/Inc/HALAL/Models/MAC/MAC.hpp b/Inc/HALAL/Models/MAC/MAC.hpp index 256274817..e436a9fbc 100644 --- a/Inc/HALAL/Models/MAC/MAC.hpp +++ b/Inc/HALAL/Models/MAC/MAC.hpp @@ -20,7 +20,7 @@ using std::getline; using std::stringstream; class MAC { - public: +public: uint8_t address[6]; string string_address; diff --git a/Inc/HALAL/Models/MDMA/MDMA.hpp b/Inc/HALAL/Models/MDMA/MDMA.hpp index 2ad70b5b0..91ceafd71 100644 --- a/Inc/HALAL/Models/MDMA/MDMA.hpp +++ b/Inc/HALAL/Models/MDMA/MDMA.hpp @@ -12,8 +12,6 @@ #include #include - - #ifdef MDMA #undef MDMA #endif @@ -21,88 +19,92 @@ #ifndef TRANSFER_QUEUE_MAX_SIZE #define TRANSFER_QUEUE_MAX_SIZE 50 #endif -class MDMA{ - public: +class MDMA { +public: /** * @brief A helper struct to create and manage MDMA linked list nodes. */ struct LinkedListNode { - template - LinkedListNode(T* source_ptr, void* dest_ptr) - { - init_node(source_ptr, dest_ptr, sizeof(T)); - } - - template - LinkedListNode(T* source_ptr, void* dest_ptr, size_t size) - { - init_node(source_ptr, dest_ptr, size); - } - - void set_next(MDMA_LinkNodeTypeDef* next_node) { node.CLAR = reinterpret_cast(next_node); } - void set_destination(void* destination) - { - uint32_t destination_address = reinterpret_cast(destination); - node.CDAR = destination_address; - - // TCM memories are accessed by AHBS bus - if ((destination_address < 0x00010000) || (destination_address >= 0x20000000 && destination_address < 0x20020000)) { - SET_BIT(node.CTBR, MDMA_CTBR_DBUS); - } else { - CLEAR_BIT(node.CTBR, MDMA_CTBR_DBUS); + template LinkedListNode(T* source_ptr, void* dest_ptr) { + init_node(source_ptr, dest_ptr, sizeof(T)); } - } - void set_source(void* source) { - uint32_t source_address = reinterpret_cast(source); - node.CSAR = source_address; - - // TCM memories are accessed by AHBS bus - if ((source_address < 0x00010000) || (source_address >= 0x20000000 && source_address < 0x20020000)) { - SET_BIT(node.CTBR, MDMA_CTBR_SBUS); - } else { - CLEAR_BIT(node.CTBR, MDMA_CTBR_SBUS); + + template LinkedListNode(T* source_ptr, void* dest_ptr, size_t size) { + init_node(source_ptr, dest_ptr, size); } - } - auto get_node() -> MDMA_LinkNodeTypeDef* { return &node; } - auto get_size() -> uint32_t { return node.CBNDTR; } - auto get_destination() -> uint32_t { return node.CDAR; } - auto get_source() -> uint32_t { return node.CSAR; } - auto get_next() -> MDMA_LinkNodeTypeDef* { return reinterpret_cast(node.CLAR); } -private: - alignas(8) MDMA_LinkNodeTypeDef node; - - void init_node(void* src, void* dst, size_t size) - { - MDMA_LinkNodeConfTypeDef nodeConfig{}; - nodeConfig.Init.DataAlignment = MDMA_DATAALIGN_RIGHT; - nodeConfig.Init.SourceBurst = MDMA_SOURCE_BURST_SINGLE; - nodeConfig.Init.DestBurst = MDMA_DEST_BURST_SINGLE; - nodeConfig.Init.BufferTransferLength = 128; - nodeConfig.Init.TransferTriggerMode = MDMA_FULL_TRANSFER; - nodeConfig.Init.SourceBlockAddressOffset = 0; - nodeConfig.Init.DestBlockAddressOffset = 0; - nodeConfig.BlockCount = 1; - nodeConfig.Init.Priority = MDMA_PRIORITY_VERY_HIGH; - nodeConfig.Init.Endianness = MDMA_LITTLE_ENDIANNESS_PRESERVE; - nodeConfig.Init.Request = MDMA_REQUEST_SW; - - this->node = {}; - nodeConfig.SrcAddress = reinterpret_cast(src); - nodeConfig.DstAddress = reinterpret_cast(dst); - nodeConfig.BlockDataLength = static_cast(size); - - uint32_t source_data_size; - uint32_t dest_data_size; - uint32_t source_inc; - uint32_t dest_inc; - - size_t effective_size = size; - if (effective_size == 2 && ((reinterpret_cast(src) | reinterpret_cast(dst)) & 1)) effective_size = 1; // Odd address, so fallback to byte-wise - else if (effective_size == 4 && ((reinterpret_cast(src) | reinterpret_cast(dst)) & 3)) effective_size = 1; // Not word-aligned, so fallback to byte-wise - else if (effective_size == 8 && ((reinterpret_cast(src) | reinterpret_cast(dst)) & 7)) effective_size = 1; // Not doubleword-aligned, so fallback to byte-wise - - switch(static_cast(effective_size)) { + void set_next(MDMA_LinkNodeTypeDef* next_node) { + node.CLAR = reinterpret_cast(next_node); + } + void set_destination(void* destination) { + uint32_t destination_address = reinterpret_cast(destination); + node.CDAR = destination_address; + + // TCM memories are accessed by AHBS bus + if ((destination_address < 0x00010000) || + (destination_address >= 0x20000000 && destination_address < 0x20020000)) { + SET_BIT(node.CTBR, MDMA_CTBR_DBUS); + } else { + CLEAR_BIT(node.CTBR, MDMA_CTBR_DBUS); + } + } + void set_source(void* source) { + uint32_t source_address = reinterpret_cast(source); + node.CSAR = source_address; + + // TCM memories are accessed by AHBS bus + if ((source_address < 0x00010000) || + (source_address >= 0x20000000 && source_address < 0x20020000)) { + SET_BIT(node.CTBR, MDMA_CTBR_SBUS); + } else { + CLEAR_BIT(node.CTBR, MDMA_CTBR_SBUS); + } + } + auto get_node() -> MDMA_LinkNodeTypeDef* { return &node; } + auto get_size() -> uint32_t { return node.CBNDTR; } + auto get_destination() -> uint32_t { return node.CDAR; } + auto get_source() -> uint32_t { return node.CSAR; } + auto get_next() -> MDMA_LinkNodeTypeDef* { + return reinterpret_cast(node.CLAR); + } + + private: + alignas(8) MDMA_LinkNodeTypeDef node; + + void init_node(void* src, void* dst, size_t size) { + MDMA_LinkNodeConfTypeDef nodeConfig{}; + nodeConfig.Init.DataAlignment = MDMA_DATAALIGN_RIGHT; + nodeConfig.Init.SourceBurst = MDMA_SOURCE_BURST_SINGLE; + nodeConfig.Init.DestBurst = MDMA_DEST_BURST_SINGLE; + nodeConfig.Init.BufferTransferLength = 128; + nodeConfig.Init.TransferTriggerMode = MDMA_FULL_TRANSFER; + nodeConfig.Init.SourceBlockAddressOffset = 0; + nodeConfig.Init.DestBlockAddressOffset = 0; + nodeConfig.BlockCount = 1; + nodeConfig.Init.Priority = MDMA_PRIORITY_VERY_HIGH; + nodeConfig.Init.Endianness = MDMA_LITTLE_ENDIANNESS_PRESERVE; + nodeConfig.Init.Request = MDMA_REQUEST_SW; + + this->node = {}; + nodeConfig.SrcAddress = reinterpret_cast(src); + nodeConfig.DstAddress = reinterpret_cast(dst); + nodeConfig.BlockDataLength = static_cast(size); + + uint32_t source_data_size; + uint32_t dest_data_size; + uint32_t source_inc; + uint32_t dest_inc; + + size_t effective_size = size; + if (effective_size == 2 && + ((reinterpret_cast(src) | reinterpret_cast(dst)) & 1)) + effective_size = 1; // Odd address, so fallback to byte-wise + else if (effective_size == 4 && ((reinterpret_cast(src) | reinterpret_cast(dst)) & 3)) + effective_size = 1; // Not word-aligned, so fallback to byte-wise + else if (effective_size == 8 && ((reinterpret_cast(src) | reinterpret_cast(dst)) & 7)) + effective_size = 1; // Not doubleword-aligned, so fallback to byte-wise + + switch (static_cast(effective_size)) { case 2: source_data_size = MDMA_SRC_DATASIZE_HALFWORD; dest_data_size = MDMA_DEST_DATASIZE_HALFWORD; @@ -127,47 +129,38 @@ class MDMA{ source_inc = MDMA_SRC_INC_BYTE; dest_inc = MDMA_DEST_INC_BYTE; break; - } + } - nodeConfig.Init.SourceDataSize = source_data_size; - nodeConfig.Init.DestDataSize = dest_data_size; - nodeConfig.Init.SourceInc = source_inc; - nodeConfig.Init.DestinationInc = dest_inc; + nodeConfig.Init.SourceDataSize = source_data_size; + nodeConfig.Init.DestDataSize = dest_data_size; + nodeConfig.Init.SourceInc = source_inc; + nodeConfig.Init.DestinationInc = dest_inc; - if (HAL_MDMA_LinkedList_CreateNode(&node, &nodeConfig) != HAL_OK) { - ErrorHandler("Error creating linked list in MDMA"); + if (HAL_MDMA_LinkedList_CreateNode(&node, &nodeConfig) != HAL_OK) { + ErrorHandler("Error creating linked list in MDMA"); + } } - } -}; + }; - private: +private: static D1_NC MDMA_LinkNodeTypeDef internal_nodes[8]; - struct Instance{ + struct Instance { public: MDMA_HandleTypeDef handle; uint8_t id; volatile bool* done; MDMA_LinkNodeTypeDef* transfer_node; - Instance() - : handle{} - , id(0U) - , done(nullptr) - , transfer_node(nullptr) - {} - - Instance(MDMA_HandleTypeDef handle_, - uint8_t id_, - volatile bool* done_, - MDMA_LinkNodeTypeDef* transfer_node_) - : handle(handle_) - , id(id_) - , done(done_) - , transfer_node(transfer_node_) - {} - + Instance() : handle{}, id(0U), done(nullptr), transfer_node(nullptr) {} + Instance( + MDMA_HandleTypeDef handle_, + uint8_t id_, + volatile bool* done_, + MDMA_LinkNodeTypeDef* transfer_node_ + ) + : handle(handle_), id(id_), done(done_), transfer_node(transfer_node_) {} }; static void prepare_transfer(Instance& instance, MDMA::LinkedListNode* first_node); static void prepare_transfer(Instance& instance, MDMA_LinkNodeTypeDef* first_node); @@ -175,48 +168,50 @@ class MDMA{ static MDMA_Channel_TypeDef* get_channel(uint8_t id); static uint8_t get_instance_id(MDMA_Channel_TypeDef* channel); - inline static std::array instances{}; + inline static std::array instances{}; static std::bitset<8> instance_free_map; - inline static Stack,50> transfer_queue{}; + inline static Stack, 50> transfer_queue{}; - static void TransferCompleteCallback(MDMA_HandleTypeDef *hmdma); - static void TransferErrorCallback(MDMA_HandleTypeDef *hmdma); + static void TransferCompleteCallback(MDMA_HandleTypeDef* hmdma); + static void TransferErrorCallback(MDMA_HandleTypeDef* hmdma); /** - * @brief A method to add MDMA channels in linked list mode. + * @brief A method to add MDMA channels in linked list mode. - * This method will be called internally for each channel during the MDMA::start() process. - * - * @param instance The reference to the MDMA instance to be initialised - */ - - static void inscribe(Instance& instance,uint8_t id); + * This method will be called internally for each channel during the MDMA::start() process. + * + * @param instance The reference to the MDMA instance to be initialised + */ - public: + static void inscribe(Instance& instance, uint8_t id); +public: static void start(); static void irq_handler(); static void update(); - - - /** * @brief A method to start a transfer from source to destination using MDMA linked list - * + * * @param source_address The source address for the transfer. * @param destination_address The destination address for the transfer. * @param data_length The length of data to be transferred. - * @param check A reference boolean that will be set to true if the transfer was successfully done, false otherwise. + * @param check A reference boolean that will be set to true if the transfer was successfully + * done, false otherwise. */ - static void transfer_data(uint8_t* source_address, uint8_t* destination_address, const uint32_t data_length,volatile bool* done=nullptr); + static void transfer_data( + uint8_t* source_address, + uint8_t* destination_address, + const uint32_t data_length, + volatile bool* done = nullptr + ); /** - * @brief A method to transfer using MDMA linked - * + * @brief A method to transfer using MDMA linked + * * @param first_node The linked list node representing the first node in the linked list. - * @param check A reference boolean that will be set to true if the transfer was successfully done, false otherwise. + * @param check A reference boolean that will be set to true if the transfer was successfully + * done, false otherwise. */ - static void transfer_list(MDMA::LinkedListNode* first_node,volatile bool* check=nullptr); - + static void transfer_list(MDMA::LinkedListNode* first_node, volatile bool* check = nullptr); }; diff --git a/Inc/HALAL/Models/MPU.hpp b/Inc/HALAL/Models/MPU.hpp index f3499c806..915a40634 100644 --- a/Inc/HALAL/Models/MPU.hpp +++ b/Inc/HALAL/Models/MPU.hpp @@ -30,7 +30,8 @@ #ifndef MPU_HPP #define MPU_HPP -#ifndef HALAL_MPUBUFFERS_MAX_INSTANCES // Define this in your build system if you need a different value +#ifndef HALAL_MPUBUFFERS_MAX_INSTANCES // Define this in your build system if you need a different + // value #define HALAL_MPUBUFFERS_MAX_INSTANCES 100 #endif @@ -77,19 +78,17 @@ extern "C" const char __mpu_d2_nc_end; extern "C" const char __mpu_d3_nc_start; extern "C" const char __mpu_d3_nc_end; - template concept mpu_buffer_request = requires(typename T::domain d) { typename T::buffer_type; - { T{ } } -> std::same_as; + { T{} } -> std::same_as; }; // POD-like buffers that can safely live in static storage without custom destruction. template -concept mpu_buffer_payload = - std::is_standard_layout_v && std::is_trivially_destructible_v; +concept mpu_buffer_payload = std::is_standard_layout_v && std::is_trivially_destructible_v; -extern void compile_error(const char *msg); +extern void compile_error(const char* msg); struct MPUDomain { enum class MemoryDomain : uint8_t { @@ -98,10 +97,7 @@ struct MPUDomain { D3 = 3 // SRAM 4 (0x38000000) }; - enum class MemoryType : bool { - Cached = true, - NonCached = false - }; + enum class MemoryType : bool { Cached = true, NonCached = false }; // Buffer Request struct Entry { @@ -112,8 +108,7 @@ struct MPUDomain { }; // Buffer Request Wrapper - template - struct Buffer { + template struct Buffer { using domain = MPUDomain; using buffer_type = T; Entry e; @@ -122,21 +117,25 @@ struct MPUDomain { * @brief Constructs a Buffer entry for a type T with explicit parameter. * @tparam T The type for which the buffer is requested. Must be a POD type. * @param type The memory type (Cached or NonCached). - * @param domain The memory domain where the buffer should be allocated (Defaults to D1, since it is the largest and fastest). - * @param force_cache_alignment If true, forces the buffer to be cache line aligned (32 bytes, takes the rest as padding). + * @param domain The memory domain where the buffer should be allocated (Defaults to D1, + * since it is the largest and fastest). + * @param force_cache_alignment If true, forces the buffer to be cache line aligned (32 + * bytes, takes the rest as padding). */ - consteval Buffer(MemoryType type = MemoryType::NonCached, - MemoryDomain mem_domain = MemoryDomain::D1, - bool force_cache_alignment = false) - : e{ - mem_domain, - type, - force_cache_alignment ? 32 : alignof(T), - sizeof(T)} - { - static_assert(alignof(T) <= 32, "Requested type has alignment greater than cache line size (32 bytes)."); - static_assert(std::ranges::find(alignments, alignof(T)) != std::ranges::end(alignments), - "Requested type has alignment not supported by MPU buffer system."); + consteval Buffer( + MemoryType type = MemoryType::NonCached, + MemoryDomain mem_domain = MemoryDomain::D1, + bool force_cache_alignment = false + ) + : e{mem_domain, type, force_cache_alignment ? 32 : alignof(T), sizeof(T)} { + static_assert( + alignof(T) <= 32, + "Requested type has alignment greater than cache line size (32 bytes)." + ); + static_assert( + std::ranges::find(alignments, alignof(T)) != std::ranges::end(alignments), + "Requested type has alignment not supported by MPU buffer system." + ); } /** @@ -144,26 +143,29 @@ struct MPUDomain { * @param entry The Entry with all buffer requirements specified. */ consteval Buffer(Entry entry) : e(entry) { - static_assert(entry.alignment <= 32, "Requested alignment greater than cache line size (32 bytes)."); - static_assert(std::ranges::find(alignments, entry.alignment) != std::ranges::end(alignments), - "Requested alignment not supported by MPU buffer system."); + static_assert( + entry.alignment <= 32, + "Requested alignment greater than cache line size (32 bytes)." + ); + static_assert( + std::ranges::find(alignments, entry.alignment) != std::ranges::end(alignments), + "Requested alignment not supported by MPU buffer system." + ); // Verify size matches sizeof(T) if (entry.size_in_bytes != sizeof(T)) { compile_error("Entry size_in_bytes must match sizeof(T)"); } } - template - consteval std::size_t inscribe(Ctx &ctx) const { + template consteval std::size_t inscribe(Ctx& ctx) const { return ctx.template add(e, this); } }; - static constexpr std::size_t max_instances = HALAL_MPUBUFFERS_MAX_INSTANCES; struct Config { - uint32_t offset; // Offset relative to the start of the domain buffer + uint32_t offset; // Offset relative to the start of the domain buffer std::size_t size; MemoryDomain domain; MemoryType type; @@ -183,31 +185,34 @@ struct MPUDomain { return (val + align - 1) & ~(align - 1); // Align up to 'align' leaving the minimum padding } - static consteval DomainSizes calculate_total_sizes(std::span configs) { DomainSizes sizes; for (const auto& cfg : configs) { size_t end = cfg.offset + cfg.size; if (cfg.domain == MemoryDomain::D1) { - if(cfg.type == MemoryType::NonCached) sizes.d1_nc_total = std::max(sizes.d1_nc_total, end); - else sizes.d1_c_total = std::max(sizes.d1_c_total, end); - } - else if (cfg.domain == MemoryDomain::D2) { - if(cfg.type == MemoryType::NonCached) sizes.d2_nc_total = std::max(sizes.d2_nc_total, end); - else sizes.d2_c_total = std::max(sizes.d2_c_total, end); - } - else if (cfg.domain == MemoryDomain::D3) { - if(cfg.type == MemoryType::NonCached) sizes.d3_nc_total = std::max(sizes.d3_nc_total, end); - else sizes.d3_c_total = std::max(sizes.d3_c_total, end); + if (cfg.type == MemoryType::NonCached) + sizes.d1_nc_total = std::max(sizes.d1_nc_total, end); + else + sizes.d1_c_total = std::max(sizes.d1_c_total, end); + } else if (cfg.domain == MemoryDomain::D2) { + if (cfg.type == MemoryType::NonCached) + sizes.d2_nc_total = std::max(sizes.d2_nc_total, end); + else + sizes.d2_c_total = std::max(sizes.d2_c_total, end); + } else if (cfg.domain == MemoryDomain::D3) { + if (cfg.type == MemoryType::NonCached) + sizes.d3_nc_total = std::max(sizes.d3_nc_total, end); + else + sizes.d3_c_total = std::max(sizes.d3_c_total, end); } } // Align totals to 32 bytes, just in case sizes.d1_nc_total = align_up(sizes.d1_nc_total, 32); - sizes.d1_c_total = align_up(sizes.d1_c_total, 32); + sizes.d1_c_total = align_up(sizes.d1_c_total, 32); sizes.d2_nc_total = align_up(sizes.d2_nc_total, 32); - sizes.d2_c_total = align_up(sizes.d2_c_total, 32); + sizes.d2_c_total = align_up(sizes.d2_c_total, 32); sizes.d3_nc_total = align_up(sizes.d3_nc_total, 32); - sizes.d3_c_total = align_up(sizes.d3_c_total, 32); + sizes.d3_c_total = align_up(sizes.d3_c_total, 32); return sizes; } @@ -217,11 +222,11 @@ struct MPUDomain { return {}; } else { std::array cfgs{}; - + uint32_t offsets_nc[3] = {}; // D1, D2, D3 uint32_t offsets_c[3] = {}; // D1, D2, D3 - uint32_t assigned_offsets[N]; - + uint32_t assigned_offsets[N]; + for (size_t align : alignments) { for (size_t i = 0; i < N; i++) { if (entries[i].alignment == align) { @@ -255,72 +260,83 @@ struct MPUDomain { void* ptr; std::size_t size; - template + template auto& construct(Args&&... args) { using T = typename std::remove_cvref_t::buffer_type; return *new (ptr) T(std::forward(args)...); } - template - auto* as() { + template auto* as() { using T = typename std::remove_cvref_t::buffer_type; return static_cast(ptr); } }; - template + template static auto& construct(Args&&... args) { - return Board::template instance_of().template construct(std::forward(args)...); + return Board::template instance_of().template construct( + std::forward(args)... + ); } - template - static auto* as() { + template static auto* as() { return Board::template instance_of().template as(); } - - template cfgs> - struct Init { + template cfgs> struct Init { static inline std::array instances{}; - + static constexpr auto Sizes = calculate_total_sizes(cfgs); // Sections defined in Linker Script (aligned to 32 bytes just in case) - __attribute__((section(".mpu_ram_d1_nc.buffer"))) alignas(32) - static inline uint8_t d1_nc_buffer[Sizes.d1_nc_total > 0 ? Sizes.d1_nc_total : 1]; - __attribute__((section(".ram_d1.buffer"))) alignas(32) - static inline uint8_t d1_c_buffer[Sizes.d1_c_total > 0 ? Sizes.d1_c_total : 1]; + __attribute__((section(".mpu_ram_d1_nc.buffer"))) alignas(32 + ) static inline uint8_t d1_nc_buffer[Sizes.d1_nc_total > 0 ? Sizes.d1_nc_total : 1]; + __attribute__((section(".ram_d1.buffer"))) alignas(32 + ) static inline uint8_t d1_c_buffer[Sizes.d1_c_total > 0 ? Sizes.d1_c_total : 1]; - __attribute__((section(".mpu_ram_d2_nc.buffer"))) alignas(32) - static inline uint8_t d2_nc_buffer[Sizes.d2_nc_total > 0 ? Sizes.d2_nc_total : 1]; - __attribute__((section(".ram_d2.buffer"))) alignas(32) - static inline uint8_t d2_c_buffer[Sizes.d2_c_total > 0 ? Sizes.d2_c_total : 1]; + __attribute__((section(".mpu_ram_d2_nc.buffer"))) alignas(32 + ) static inline uint8_t d2_nc_buffer[Sizes.d2_nc_total > 0 ? Sizes.d2_nc_total : 1]; + __attribute__((section(".ram_d2.buffer"))) alignas(32 + ) static inline uint8_t d2_c_buffer[Sizes.d2_c_total > 0 ? Sizes.d2_c_total : 1]; - __attribute__((section(".mpu_ram_d3_nc.buffer"))) alignas(32) - static inline uint8_t d3_nc_buffer[Sizes.d3_nc_total > 0 ? Sizes.d3_nc_total : 1]; - __attribute__((section(".ram_d3.buffer"))) alignas(32) - static inline uint8_t d3_c_buffer[Sizes.d3_c_total > 0 ? Sizes.d3_c_total : 1]; + __attribute__((section(".mpu_ram_d3_nc.buffer"))) alignas(32 + ) static inline uint8_t d3_nc_buffer[Sizes.d3_nc_total > 0 ? Sizes.d3_nc_total : 1]; + __attribute__((section(".ram_d3.buffer"))) alignas(32 + ) static inline uint8_t d3_c_buffer[Sizes.d3_c_total > 0 ? Sizes.d3_c_total : 1]; static void init() { HAL_MPU_Disable(); configure_static_regions(); // Dynamic Configuration based on Linker Symbols - configure_dynamic_region((uintptr_t)__mpu_d1_nc_start, (uintptr_t)__mpu_d1_nc_end, MPU_REGION_NUMBER3); - configure_dynamic_region((uintptr_t)__mpu_d2_nc_start, (uintptr_t)__mpu_d2_nc_end, MPU_REGION_NUMBER5); - configure_dynamic_region((uintptr_t)__mpu_d3_nc_start, (uintptr_t)__mpu_d3_nc_end, MPU_REGION_NUMBER7); + configure_dynamic_region( + (uintptr_t)__mpu_d1_nc_start, + (uintptr_t)__mpu_d1_nc_end, + MPU_REGION_NUMBER3 + ); + configure_dynamic_region( + (uintptr_t)__mpu_d2_nc_start, + (uintptr_t)__mpu_d2_nc_end, + MPU_REGION_NUMBER5 + ); + configure_dynamic_region( + (uintptr_t)__mpu_d3_nc_start, + (uintptr_t)__mpu_d3_nc_end, + MPU_REGION_NUMBER7 + ); // Assign pointers - uint8_t* bases_nc[3] = { &d1_nc_buffer[0], &d2_nc_buffer[0], &d3_nc_buffer[0] }; - uint8_t* bases_c[3] = { &d1_c_buffer[0], &d2_c_buffer[0], &d3_c_buffer[0] }; + uint8_t* bases_nc[3] = {&d1_nc_buffer[0], &d2_nc_buffer[0], &d3_nc_buffer[0]}; + uint8_t* bases_c[3] = {&d1_c_buffer[0], &d2_c_buffer[0], &d3_c_buffer[0]}; for (std::size_t i = 0; i < N; i++) { - const auto &cfg = cfgs[i]; - auto &inst = instances[i]; + const auto& cfg = cfgs[i]; + auto& inst = instances[i]; - if (cfg.domain == MemoryDomain::D1 || cfg.domain == MemoryDomain::D2 || cfg.domain == MemoryDomain::D3) { + if (cfg.domain == MemoryDomain::D1 || cfg.domain == MemoryDomain::D2 || + cfg.domain == MemoryDomain::D3) { size_t d_idx = static_cast(cfg.domain) - 1; - + if (cfg.type == MemoryType::NonCached) { inst.ptr = bases_nc[d_idx] + cfg.offset; } else { @@ -336,22 +352,31 @@ struct MPUDomain { } }; - private: +private: static constexpr std::size_t alignments[6] = {32, 16, 8, 4, 2, 1}; static void configure_dynamic_region(uintptr_t start, uintptr_t end, uint8_t region_num) { - if (end <= start) return; + if (end <= start) + return; // Dx NC (Normal, Non-Cacheable) // TEX=1, C=0, B=0: Normal, Non-Cacheable // Shareable since it can be accessed by multiple masters (CPU, DMA, etc) - configure_region(start, end - start, region_num, - MPU_TEX_LEVEL1, MPU_REGION_FULL_ACCESS, MPU_INSTRUCTION_ACCESS_DISABLE, - MPU_ACCESS_SHAREABLE, MPU_ACCESS_NOT_CACHEABLE, MPU_ACCESS_NOT_BUFFERABLE); + configure_region( + start, + end - start, + region_num, + MPU_TEX_LEVEL1, + MPU_REGION_FULL_ACCESS, + MPU_INSTRUCTION_ACCESS_DISABLE, + MPU_ACCESS_SHAREABLE, + MPU_ACCESS_NOT_CACHEABLE, + MPU_ACCESS_NOT_BUFFERABLE + ); } static void configure_static_regions() { MPU_Region_InitTypeDef MPU_InitStruct = {0}; - + // Background (No Access) - Covers all memory not explicitly defined by a further region // Doesn't use configure_region helper since that uses an uint32_t size, we need 4GB here... MPU_InitStruct.Enable = MPU_REGION_ENABLE; @@ -369,57 +394,122 @@ struct MPUDomain { // Peripherals (Device, Buffered) // Guarded against speculative execution and cache - configure_region(reinterpret_cast(&__peripheral_base), reinterpret_cast(&__peripheral_size), MPU_REGION_NUMBER8, - MPU_TEX_LEVEL0, MPU_REGION_FULL_ACCESS, MPU_INSTRUCTION_ACCESS_DISABLE, - MPU_ACCESS_SHAREABLE, MPU_ACCESS_NOT_CACHEABLE, MPU_ACCESS_BUFFERABLE); + configure_region( + reinterpret_cast(&__peripheral_base), + reinterpret_cast(&__peripheral_size), + MPU_REGION_NUMBER8, + MPU_TEX_LEVEL0, + MPU_REGION_FULL_ACCESS, + MPU_INSTRUCTION_ACCESS_DISABLE, + MPU_ACCESS_SHAREABLE, + MPU_ACCESS_NOT_CACHEABLE, + MPU_ACCESS_BUFFERABLE + ); // Flash (Normal, Cacheable) // TEX=0, C=1, B=0: Normal, Write-Through, No Read-Allocate (Read optimized) // Not Shareable to allow full caching - configure_region(reinterpret_cast(&__flash_base), reinterpret_cast(&__flash_size), MPU_REGION_NUMBER1, - MPU_TEX_LEVEL0, MPU_REGION_FULL_ACCESS, MPU_INSTRUCTION_ACCESS_ENABLE, - MPU_ACCESS_NOT_SHAREABLE, MPU_ACCESS_CACHEABLE, MPU_ACCESS_NOT_BUFFERABLE); + configure_region( + reinterpret_cast(&__flash_base), + reinterpret_cast(&__flash_size), + MPU_REGION_NUMBER1, + MPU_TEX_LEVEL0, + MPU_REGION_FULL_ACCESS, + MPU_INSTRUCTION_ACCESS_ENABLE, + MPU_ACCESS_NOT_SHAREABLE, + MPU_ACCESS_CACHEABLE, + MPU_ACCESS_NOT_BUFFERABLE + ); // DTCM (Normal, Cacheable) // TEX=1, C=1, B=1: Normal, Write-Back, Write and Read Allocate // TCMs are like Cache, so they are not really cacheable, and the MPU settings are ignored - configure_region(reinterpret_cast(&__dtcm_base), reinterpret_cast(&__dtcm_size), MPU_REGION_NUMBER10, - MPU_TEX_LEVEL1, MPU_REGION_FULL_ACCESS, MPU_INSTRUCTION_ACCESS_DISABLE, - MPU_ACCESS_NOT_SHAREABLE, MPU_ACCESS_CACHEABLE, MPU_ACCESS_BUFFERABLE); + configure_region( + reinterpret_cast(&__dtcm_base), + reinterpret_cast(&__dtcm_size), + MPU_REGION_NUMBER10, + MPU_TEX_LEVEL1, + MPU_REGION_FULL_ACCESS, + MPU_INSTRUCTION_ACCESS_DISABLE, + MPU_ACCESS_NOT_SHAREABLE, + MPU_ACCESS_CACHEABLE, + MPU_ACCESS_BUFFERABLE + ); // ITCM (Normal, Cacheable) // TEX=0, C=1, B=0: Normal, Write-Through, No Read-Allocate (Read optimized) // TCMs are like Cache, so they are not really cacheable, and the MPU settings are ignored - configure_region(reinterpret_cast(&__itcm_base), reinterpret_cast(&__itcm_size), MPU_REGION_NUMBER11, - MPU_TEX_LEVEL0, MPU_REGION_FULL_ACCESS, MPU_INSTRUCTION_ACCESS_ENABLE, - MPU_ACCESS_NOT_SHAREABLE, MPU_ACCESS_CACHEABLE, MPU_ACCESS_NOT_BUFFERABLE); + configure_region( + reinterpret_cast(&__itcm_base), + reinterpret_cast(&__itcm_size), + MPU_REGION_NUMBER11, + MPU_TEX_LEVEL0, + MPU_REGION_FULL_ACCESS, + MPU_INSTRUCTION_ACCESS_ENABLE, + MPU_ACCESS_NOT_SHAREABLE, + MPU_ACCESS_CACHEABLE, + MPU_ACCESS_NOT_BUFFERABLE + ); // D1 RAM Cached (Normal, WBWA) // TEX=1, C=1, B=1: Normal, Write-Back, Write-Allocate // Shareable since it can be accessed by multiple masters (CPU, DMA, etc) - configure_region(reinterpret_cast(&__ram_d1_base), reinterpret_cast(&__ram_d1_size), MPU_REGION_NUMBER2, - MPU_TEX_LEVEL1, MPU_REGION_FULL_ACCESS, MPU_INSTRUCTION_ACCESS_DISABLE, - MPU_ACCESS_SHAREABLE, MPU_ACCESS_CACHEABLE, MPU_ACCESS_BUFFERABLE); + configure_region( + reinterpret_cast(&__ram_d1_base), + reinterpret_cast(&__ram_d1_size), + MPU_REGION_NUMBER2, + MPU_TEX_LEVEL1, + MPU_REGION_FULL_ACCESS, + MPU_INSTRUCTION_ACCESS_DISABLE, + MPU_ACCESS_SHAREABLE, + MPU_ACCESS_CACHEABLE, + MPU_ACCESS_BUFFERABLE + ); // D2 RAM Cached (Normal, WBWA) // TEX=1, C=1, B=1: Normal, Write-Back, Write-Allocate // Shareable since it can be accessed by multiple masters (CPU, DMA, etc) - configure_region(reinterpret_cast(&__ram_d2_base), reinterpret_cast(&__ram_d2_size), MPU_REGION_NUMBER4, - MPU_TEX_LEVEL1, MPU_REGION_FULL_ACCESS, MPU_INSTRUCTION_ACCESS_DISABLE, - MPU_ACCESS_SHAREABLE, MPU_ACCESS_CACHEABLE, MPU_ACCESS_BUFFERABLE); + configure_region( + reinterpret_cast(&__ram_d2_base), + reinterpret_cast(&__ram_d2_size), + MPU_REGION_NUMBER4, + MPU_TEX_LEVEL1, + MPU_REGION_FULL_ACCESS, + MPU_INSTRUCTION_ACCESS_DISABLE, + MPU_ACCESS_SHAREABLE, + MPU_ACCESS_CACHEABLE, + MPU_ACCESS_BUFFERABLE + ); // D3 RAM Cached (Normal, WBWA) // TEX=1, C=1, B=1: Normal, Write-Back, Write-Allocate // Shareable since it can be accessed by multiple masters (CPU, DMA, etc) - configure_region(reinterpret_cast(&__ram_d3_base), reinterpret_cast(&__ram_d3_size), MPU_REGION_NUMBER6, - MPU_TEX_LEVEL1, MPU_REGION_FULL_ACCESS, MPU_INSTRUCTION_ACCESS_DISABLE, - MPU_ACCESS_SHAREABLE, MPU_ACCESS_CACHEABLE, MPU_ACCESS_BUFFERABLE); + configure_region( + reinterpret_cast(&__ram_d3_base), + reinterpret_cast(&__ram_d3_size), + MPU_REGION_NUMBER6, + MPU_TEX_LEVEL1, + MPU_REGION_FULL_ACCESS, + MPU_INSTRUCTION_ACCESS_DISABLE, + MPU_ACCESS_SHAREABLE, + MPU_ACCESS_CACHEABLE, + MPU_ACCESS_BUFFERABLE + ); } - static void configure_region(uintptr_t base, size_t size, uint8_t region_num, - uint8_t tex, uint8_t access, uint8_t no_exec, - uint8_t shareable, uint8_t cacheable, uint8_t bufferable) { - if (size == 0) return; + static void configure_region( + uintptr_t base, + size_t size, + uint8_t region_num, + uint8_t tex, + uint8_t access, + uint8_t no_exec, + uint8_t shareable, + uint8_t cacheable, + uint8_t bufferable + ) { + if (size == 0) + return; // Find smallest power of 2 >= size, starting at 32 bytes (MPU minimum) // Enforce MPU minimum (32 bytes = 2^(4+1)) @@ -435,14 +525,16 @@ struct MPUDomain { uint32_t sub_size = (1 << (mpu_size + 1)) / 8; // Number of subregions needed to cover 'size' uint8_t needed_subs = (size + sub_size - 1) / sub_size; - + // We want first 'needed_subs' enabled (0), rest disabled (1) uint8_t srd = 0xFF << needed_subs; MPU_Region_InitTypeDef MPU_InitStruct = {0}; MPU_InitStruct.Enable = MPU_REGION_ENABLE; MPU_InitStruct.Number = region_num; - MPU_InitStruct.BaseAddress = static_cast(base); // Should be uint32_t, but may be different when mocking, so this is just to make sure it accepts it + MPU_InitStruct.BaseAddress = static_cast(base + ); // Should be uint32_t, but may be different when mocking, so this is just to make sure it + // accepts it MPU_InitStruct.Size = mpu_size; MPU_InitStruct.SubRegionDisable = srd; MPU_InitStruct.TypeExtField = tex; diff --git a/Inc/HALAL/Models/MPUManager/MPUManager.hpp b/Inc/HALAL/Models/MPUManager/MPUManager.hpp index fd3ba8b49..0237e2c9b 100644 --- a/Inc/HALAL/Models/MPUManager/MPUManager.hpp +++ b/Inc/HALAL/Models/MPUManager/MPUManager.hpp @@ -5,25 +5,22 @@ #define NO_CACHED_RAM_MAXIMUM_SPACE 2048 -class MPUManager{ +class MPUManager { public: - static void* allocate_non_cached_memory(uint32_t size){ - void* buffer = (void*)((uint8_t*)no_cached_ram_start + no_cached_ram_occupied_bytes); - no_cached_ram_occupied_bytes = no_cached_ram_occupied_bytes + size; - if(no_cached_ram_occupied_bytes > NO_CACHED_RAM_MAXIMUM_SPACE){ - uint32_t excess_bytes = no_cached_ram_occupied_bytes - NO_CACHED_RAM_MAXIMUM_SPACE; - ErrorHandler("Maximum capacity on non cached ram heap exceeded by %d",excess_bytes); - return nullptr; - } - return buffer; - } + static void* allocate_non_cached_memory(uint32_t size) { + void* buffer = (void*)((uint8_t*)no_cached_ram_start + no_cached_ram_occupied_bytes); + no_cached_ram_occupied_bytes = no_cached_ram_occupied_bytes + size; + if (no_cached_ram_occupied_bytes > NO_CACHED_RAM_MAXIMUM_SPACE) { + uint32_t excess_bytes = no_cached_ram_occupied_bytes - NO_CACHED_RAM_MAXIMUM_SPACE; + ErrorHandler("Maximum capacity on non cached ram heap exceeded by %d", excess_bytes); + return nullptr; + } + return buffer; + } private: - static void* no_cached_ram_start; - static uint32_t no_cached_ram_occupied_bytes; + static void* no_cached_ram_start; + static uint32_t no_cached_ram_occupied_bytes; }; -template -class RAMBuffer{ - -}; +template class RAMBuffer {}; diff --git a/Inc/HALAL/Models/Packets/ForwardOrder.hpp b/Inc/HALAL/Models/Packets/ForwardOrder.hpp index 1a7fb5d2e..4f7634781 100644 --- a/Inc/HALAL/Models/Packets/ForwardOrder.hpp +++ b/Inc/HALAL/Models/Packets/ForwardOrder.hpp @@ -3,48 +3,39 @@ #include "HALAL/Services/Communication/Ethernet/LWIP/TCP/Socket.hpp" template - requires NotCallablePack + requires NotCallablePack class ForwardOrder : public StackPacket, public Order { public: - ForwardOrder(uint16_t id, Socket &forwarder, Types *...values) - : StackPacket(id, values...), - forwarding_socket{forwarder} { - orders[id] = this; - } + ForwardOrder(uint16_t id, Socket& forwarder, Types*... values) + : StackPacket(id, values...), forwarding_socket{forwarder} { + orders[id] = this; + } - void process() override { return; } + void process() override { return; } - void parse(uint8_t *data) override { return; } - void parse(OrderProtocol *socket, uint8_t *data) override { - struct pbuf *packet = pbuf_alloc(PBUF_TRANSPORT, get_size(), PBUF_POOL); - pbuf_take(packet, data, get_size()); - forwarding_socket.tx_packet_buffer.push(packet); - forwarding_socket.send(); - } - size_t get_size() override { - return StackPacket::get_size(); - } - uint16_t get_id() override { - return StackPacket::get_id(); - } + void parse(uint8_t* data) override { return; } + void parse(OrderProtocol* socket, uint8_t* data) override { + struct pbuf* packet = pbuf_alloc(PBUF_TRANSPORT, get_size(), PBUF_POOL); + pbuf_take(packet, data, get_size()); + forwarding_socket.tx_packet_buffer.push(packet); + forwarding_socket.send(); + } + size_t get_size() override { return StackPacket::get_size(); } + uint16_t get_id() override { return StackPacket::get_id(); } private: - void set_callback(void (*callback)(void)) override { return; } - uint8_t *build() override { - return StackPacket::build(); - } - void set_pointer(size_t index, void *pointer) override { - StackPacket::set_pointer(index, pointer); - } - // socket in charge of forwarding the order - Socket &forwarding_socket; + void set_callback(void (*callback)(void)) override { return; } + uint8_t* build() override { return StackPacket::build(); } + void set_pointer(size_t index, void* pointer) override { + StackPacket::set_pointer(index, pointer); + } + // socket in charge of forwarding the order + Socket& forwarding_socket; }; #if __cpp_deduction_guides >= 201606 template - requires NotCallablePack -ForwardOrder(uint16_t id, Socket &fwd, Types *...values) - -> ForwardOrder<(!has_container::value) * - total_sizeof::value, - Types...>; + requires NotCallablePack +ForwardOrder(uint16_t id, Socket& fwd, Types*... values) + -> ForwardOrder<(!has_container::value) * total_sizeof::value, Types...>; #endif diff --git a/Inc/HALAL/Models/Packets/I2CPacket.hpp b/Inc/HALAL/Models/Packets/I2CPacket.hpp index 72c56ef32..db9b2db20 100644 --- a/Inc/HALAL/Models/Packets/I2CPacket.hpp +++ b/Inc/HALAL/Models/Packets/I2CPacket.hpp @@ -15,28 +15,25 @@ * in bytes and the number of bytes. * */ -class I2CPacket -{ +class I2CPacket { uint32_t size; /**< Size in bytes of the data. */ - uint16_t id; /**< Slave id */ + uint16_t id; /**< Slave id */ uint8_t* data; /**< Data pointer. */ private: I2CPacket(){}; public: - /** * @brief Construct a new I2C Packet object. The constructor will create an empty packet * of the specified size. This constructor is intended for receive packets. - * + * * @param size Size in bytes of the packet data. */ I2CPacket(uint32_t size) { - this->size = size; - this->id = 0; - this->data = (uint8_t*)malloc(this->size); - + this->size = size; + this->id = 0; + this->data = (uint8_t*)malloc(this->size); } /** @@ -46,8 +43,7 @@ class I2CPacket * @tparam Type Generic data type. * @param data Data to be stored in the packet. */ - template - I2CPacket(uint16_t id, Type data) { + template I2CPacket(uint16_t id, Type data) { this->size = sizeof(Type); this->id = id; this->data = (uint8_t*)malloc(this->size); @@ -63,8 +59,7 @@ class I2CPacket * @param data Pointer to the data to be store. * @param size Size of the array. */ - template - I2CPacket(uint8_t id, Type* data, uint32_t size){ + template I2CPacket(uint8_t id, Type* data, uint32_t size) { this->size = size * sizeof(Type); this->id = id; this->data = (uint8_t*)malloc(this->size); @@ -85,34 +80,25 @@ class I2CPacket * * @return uint32_t Returns the data size in bytes. */ - uint32_t get_size() { - return size; - } + uint32_t get_size() { return size; } /** - * @brief Gets the id. - * - * @return uint32_t Returns the data size in bytes. - */ - uint16_t get_id(){ - return id; - } + * @brief Gets the id. + * + * @return uint32_t Returns the data size in bytes. + */ + uint16_t get_id() { return id; } /** * @brief Get the pointer to the data. * * @return uint8_t* Returns a pointer to the packet data. */ - uint8_t* get_data() { - return data; - } + uint8_t* get_data() { return data; } /** - * @brief Set new data to the packet. - * + * @brief Set new data to the packet. + * ¡ */ - void set_data(uint8_t* data){ - this->data = data; - } - + void set_data(uint8_t* data) { this->data = data; } }; diff --git a/Inc/HALAL/Models/Packets/MdmaPacket.hpp b/Inc/HALAL/Models/Packets/MdmaPacket.hpp index acaa516e3..44fbd8907 100644 --- a/Inc/HALAL/Models/Packets/MdmaPacket.hpp +++ b/Inc/HALAL/Models/Packets/MdmaPacket.hpp @@ -1,9 +1,9 @@ /* -* MdmaPacket.hpp -* -* Created on: 06 nov. 2025 -* Author: Boris -*/ + * MdmaPacket.hpp + * + * Created on: 06 nov. 2025 + * Author: Boris + */ #ifndef MDMA_PACKET_HPP #define MDMA_PACKET_HPP @@ -30,8 +30,8 @@ struct MdmaPacketDomain { }; struct Instance { - uint8_t *packet_buffer; - MDMA::LinkedListNode *nodes; + uint8_t* packet_buffer; + MDMA::LinkedListNode* nodes; }; static constexpr size_t max_instances = MDMA_PACKET_MAX_INSTANCES; @@ -46,35 +46,38 @@ struct MdmaPacketDomain { return cfgs; } - template - struct Init { + template struct Init { static inline std::array instances{}; template - static void init(const std::array& cfgs, std::array& mpu_instances) { + static void init( + const std::array& cfgs, + std::array& mpu_instances + ) { for (std::size_t i = 0; i < N; i++) { - instances[i].packet_buffer = static_cast(mpu_instances[cfgs[i].packet_mpu_index].ptr); - instances[i].nodes = static_cast(mpu_instances[cfgs[i].nodes_mpu_index].ptr); + instances[i].packet_buffer = + static_cast(mpu_instances[cfgs[i].packet_mpu_index].ptr); + instances[i].nodes = + static_cast(mpu_instances[cfgs[i].nodes_mpu_index].ptr); } } }; /** - * @brief A Packet class that uses MDMA for building and parsing packets. - * @tparam Types The types of the values in the packet. - * @note It uses non-cached memory for MDMA operations. - */ + * @brief A Packet class that uses MDMA for building and parsing packets. + * @tparam Types The types of the values in the packet. + * @note It uses non-cached memory for MDMA operations. + */ class MdmaPacketBase : public Packet { public: - virtual uint8_t* build(bool *done, uint8_t *destination_address = nullptr) = 0; + virtual uint8_t* build(bool* done, uint8_t* destination_address = nullptr) = 0; using Packet::build; }; - template - class MdmaPacket : public MdmaPacketBase { + template class MdmaPacket : public MdmaPacketBase { public: uint16_t id; - uint8_t *buffer; + uint8_t* buffer; size_t size; std::tuple value_pointers; @@ -93,75 +96,92 @@ struct MdmaPacketDomain { MPUDomain::Buffer packet_req; MPUDomain::Buffer nodes_req; - consteval Request() : - packet_req(MPUDomain::MemoryType::NonCached, MPUDomain::MemoryDomain::D1), - nodes_req(MPUDomain::MemoryType::NonCached, MPUDomain::MemoryDomain::D1) - {} + consteval Request() + : packet_req(MPUDomain::MemoryType::NonCached, MPUDomain::MemoryDomain::D1), + nodes_req(MPUDomain::MemoryType::NonCached, MPUDomain::MemoryDomain::D1) {} - template - consteval void inscribe(Ctx &ctx) const { + template consteval void inscribe(Ctx& ctx) const { size_t p_idx = packet_req.inscribe(ctx); size_t n_idx = nodes_req.inscribe(ctx); ctx.template add({p_idx, n_idx}, this); } }; - MdmaPacket(Instance& instance, uint16_t id, Types*... values) - : id(id), size((sizeof(Types) + ...) + sizeof(uint16_t)) , value_pointers(&this->id, values...) { - + MdmaPacket(Instance& instance, uint16_t id, Types*... values) + : id(id), size((sizeof(Types) + ...) + sizeof(uint16_t)), + value_pointers(&this->id, values...) { + packets[this->id] = this; this->buffer = instance.packet_buffer; - MDMA::LinkedListNode *nodes = instance.nodes; + MDMA::LinkedListNode* nodes = instance.nodes; - MDMA::LinkedListNode *prev_node = nullptr; + MDMA::LinkedListNode* prev_node = nullptr; uint32_t offset = 0; uint32_t idx = 0; size_t node_idx = 0; - std::apply([&](auto&&... args) { (([&]() { - using PointerType = std::decay_t; - using UnderlyingType = std::remove_pointer_t; - - constexpr size_t type_size = sizeof(UnderlyingType); - MDMA::LinkedListNode *node = new (&nodes[node_idx++]) MDMA::LinkedListNode(args, buffer + offset,type_size); // Placement new - build_nodes[idx++] = node; - offset += type_size; - - if (prev_node != nullptr) { - prev_node->set_next(node->get_node()); - } - prev_node = node; - }()), ...); }, value_pointers); + std::apply( + [&](auto&&... args) { + (([&]() { + using PointerType = std::decay_t; + using UnderlyingType = std::remove_pointer_t; + + constexpr size_t type_size = sizeof(UnderlyingType); + MDMA::LinkedListNode* node = new (&nodes[node_idx++] + ) MDMA::LinkedListNode(args, buffer + offset, type_size); // Placement new + build_nodes[idx++] = node; + offset += type_size; + + if (prev_node != nullptr) { + prev_node->set_next(node->get_node()); + } + prev_node = node; + }()), + ...); + }, + value_pointers + ); prev_node = nullptr; offset = 0; idx = 0; - std::apply([&](auto&&... args) { (([&]() { - using PointerType = std::decay_t; - using UnderlyingType = std::remove_pointer_t; - - constexpr size_t type_size = sizeof(UnderlyingType); - MDMA::LinkedListNode *node = new (&nodes[node_idx++]) MDMA::LinkedListNode(buffer + offset, args, type_size); // Placement new - parse_nodes[idx++] = node; - offset += type_size; - - if (prev_node != nullptr) { - prev_node->set_next(node->get_node()); - } - prev_node = node; - }()), ...); }, value_pointers); - - build_transfer_node = new (&nodes[node_idx++]) MDMA::LinkedListNode(buffer, nullptr, size); - parse_transfer_node = new (&nodes[node_idx++]) MDMA::LinkedListNode(buffer, buffer, size); + std::apply( + [&](auto&&... args) { + (([&]() { + using PointerType = std::decay_t; + using UnderlyingType = std::remove_pointer_t; + + constexpr size_t type_size = sizeof(UnderlyingType); + MDMA::LinkedListNode* node = new (&nodes[node_idx++] + ) MDMA::LinkedListNode(buffer + offset, args, type_size); // Placement new + parse_nodes[idx++] = node; + offset += type_size; + + if (prev_node != nullptr) { + prev_node->set_next(node->get_node()); + } + prev_node = node; + }()), + ...); + }, + value_pointers + ); + + build_transfer_node = + new (&nodes[node_idx++]) MDMA::LinkedListNode(buffer, nullptr, size); + parse_transfer_node = + new (&nodes[node_idx++]) MDMA::LinkedListNode(buffer, buffer, size); } /** - * @brief Build the packet and transfer data into non-cached buffer using MDMA - * @param destination_address Optional destination address for the built packet (should be non-cached, else you will need to manage cache coherency). It isn't optional here because there's a specific overload without parameters for compliance with Packet interface. - * @return Pointer to the built packet data (internal buffer or destination address) - */ - uint8_t* build(uint8_t *destination_address) { + * @brief Build the packet and transfer data into non-cached buffer using MDMA + * @param destination_address Optional destination address for the built packet (should be + * non-cached, else you will need to manage cache coherency). It isn't optional here because + * there's a specific overload without parameters for compliance with Packet interface. + * @return Pointer to the built packet data (internal buffer or destination address) + */ + uint8_t* build(uint8_t* destination_address) { set_build_destination(destination_address); bool done = false; MDMA::transfer_list(build_nodes[0], &done); @@ -172,12 +192,14 @@ struct MdmaPacketDomain { } /** - * @brief Build the packet and transfer data into non-cached buffer using MDMA with a promise - * @param done Promise to be fulfilled upon transfer completion - * @param destination_address Optional destination address for the built packet (should be non-cached, else you will need to manage cache coherency) - * @return Pointer to the built packet data (internal buffer or destination address) - */ - uint8_t* build(bool *done, uint8_t *destination_address = nullptr) { + * @brief Build the packet and transfer data into non-cached buffer using MDMA with a + * promise + * @param done Promise to be fulfilled upon transfer completion + * @param destination_address Optional destination address for the built packet (should be + * non-cached, else you will need to manage cache coherency) + * @return Pointer to the built packet data (internal buffer or destination address) + */ + uint8_t* build(bool* done, uint8_t* destination_address = nullptr) { set_build_destination(destination_address); MDMA::transfer_list(build_nodes[0], done); return destination_address ? destination_address : buffer; @@ -185,16 +207,18 @@ struct MdmaPacketDomain { // Just for interface compliance uint8_t* build() override { - uint8_t *destination_address = nullptr; + uint8_t* destination_address = nullptr; return build(destination_address); } /** * @brief Parse the packet data from non-cached buffer using MDMA - * @param data Optional source data address to parse from (should be non-cached, else you will need to manage cache coherency). It isn't optional here becasue there's a specific overload without parameters for compliance with Packet interface. + * @param data Optional source data address to parse from (should be non-cached, else you + * will need to manage cache coherency). It isn't optional here becasue there's a specific + * overload without parameters for compliance with Packet interface. * @param done Optional pointer to a boolean that will be set to true when parsing is done. */ - void parse(uint8_t *data) override { + void parse(uint8_t* data) override { bool done = false; auto source_node = set_parse_source(data); MDMA::transfer_list(source_node, &done); @@ -206,40 +230,43 @@ struct MdmaPacketDomain { /** * @brief Parse the packet data from non-cached buffer using MDMA with a promise * @param done Pointer to a boolean that will be set to true when parsing is done. - * @param data Optional source data address to parse from (should be non-cached, else you will need to manage cache coherency). + * @param data Optional source data address to parse from (should be non-cached, else you + * will need to manage cache coherency). */ - void parse(bool *done, uint8_t *data = nullptr) { + void parse(bool* done, uint8_t* data = nullptr) { auto source_node = set_parse_source(data); MDMA::transfer_list(source_node, done); } - size_t get_size() override { - return size; - } + size_t get_size() override { return size; } - uint16_t get_id() override { - return id; - } + uint16_t get_id() override { return id; } // Just for interface compliance, this is not efficient for MdmaPacket as it is. - // Could be optimized by using a map of index to pointer or similar structure created at compile time, but doesn't seem worthy now. + // Could be optimized by using a map of index to pointer or similar structure created at + // compile time, but doesn't seem worthy now. void set_pointer(size_t index, void* pointer) override { size_t current_idx = 0; - std::apply([&](auto&&... args) { - ((current_idx++ == index ? - (args = reinterpret_cast>(pointer)) - : nullptr), ...); - }, value_pointers); + std::apply( + [&](auto&&... args) { + ((current_idx++ == index + ? (args = + reinterpret_cast>(pointer)) + : nullptr), + ...); + }, + value_pointers + ); } private: - MDMA::LinkedListNode *build_transfer_node; // Node used for destination address - MDMA::LinkedListNode *parse_transfer_node; // Node used for source address - MDMA::LinkedListNode *build_nodes[sizeof...(Types) + 1]; - MDMA::LinkedListNode *parse_nodes[sizeof...(Types) + 1]; + MDMA::LinkedListNode* build_transfer_node; // Node used for destination address + MDMA::LinkedListNode* parse_transfer_node; // Node used for source address + MDMA::LinkedListNode* build_nodes[sizeof...(Types) + 1]; + MDMA::LinkedListNode* parse_nodes[sizeof...(Types) + 1]; - void set_build_destination(uint8_t *external_buffer) { + void set_build_destination(uint8_t* external_buffer) { if (external_buffer != nullptr) { build_transfer_node->set_destination(external_buffer); build_nodes[sizeof...(Types)]->set_next(build_transfer_node->get_node()); @@ -248,7 +275,7 @@ struct MdmaPacketDomain { } } - MDMA::LinkedListNode* set_parse_source(uint8_t *external_buffer) { + MDMA::LinkedListNode* set_parse_source(uint8_t* external_buffer) { if (external_buffer != nullptr) { parse_transfer_node->set_source(external_buffer); parse_transfer_node->set_next(parse_nodes[0]->get_node()); diff --git a/Inc/HALAL/Models/Packets/Order.hpp b/Inc/HALAL/Models/Packets/Order.hpp index d8d0e9597..69eeed0ea 100644 --- a/Inc/HALAL/Models/Packets/Order.hpp +++ b/Inc/HALAL/Models/Packets/Order.hpp @@ -9,104 +9,92 @@ #include "HALAL/Models/Packets/Packet.hpp" #include "HALAL/Models/Packets/OrderProtocol.hpp" -class Order : public Packet{ -public: - string *remote_ip; - static map orders; - virtual void set_callback(void(*callback)(void)) = 0; +class Order : public Packet { +public: + string* remote_ip; + static map orders; + virtual void set_callback(void (*callback)(void)) = 0; virtual void process() = 0; virtual void parse(OrderProtocol* socket, uint8_t* data) = 0; - void store_ip_order(string &ip){ - remote_ip = &ip; - } - void parse(uint8_t* data) override { - parse(nullptr, data); - } + void store_ip_order(string& ip) { remote_ip = &ip; } + void parse(uint8_t* data) override { parse(nullptr, data); } static void process_by_id(uint16_t id) { - if (orders.find(id) != orders.end()) orders[id]->process(); + if (orders.find(id) != orders.end()) + orders[id]->process(); } static void process_data(OrderProtocol* socket, uint8_t* data) { uint16_t id = Packet::get_id(data); - if (orders.contains(id)){ + if (orders.contains(id)) { orders[id]->parse(socket, data); orders[id]->process(); } - } + } }; -template requires NotCallablePack -class StackOrder : public StackPacket, public Order{ +template + requires NotCallablePack +class StackOrder : public StackPacket, public Order { public: - StackOrder(uint16_t id,void(*callback)(void), Types*... values) : StackPacket(id,values...), callback(callback) {orders[id] = this; } - StackOrder(uint16_t id, Types*... values) : StackPacket(id,values...) {orders[id] = this;} - void(*callback)(void) = nullptr; - void set_callback(void(*callback)(void)) override { - this->callback = callback; - } - void process() override { - if (callback != nullptr) callback(); - } - uint8_t* build() override { - return StackPacket::build(); - } - void parse(uint8_t* data) override { - StackPacket::parse(data); - } - void parse(OrderProtocol* socket, uint8_t* data) override{ - parse(data); - } - size_t get_size() override { - return StackPacket::get_size(); + StackOrder(uint16_t id, void (*callback)(void), Types*... values) + : StackPacket(id, values...), callback(callback) { + orders[id] = this; } - uint16_t get_id() override { - return StackPacket::get_id(); + StackOrder(uint16_t id, Types*... values) : StackPacket(id, values...) { + orders[id] = this; } + void (*callback)(void) = nullptr; + void set_callback(void (*callback)(void)) override { this->callback = callback; } + void process() override { + if (callback != nullptr) + callback(); + } + uint8_t* build() override { return StackPacket::build(); } + void parse(uint8_t* data) override { StackPacket::parse(data); } + void parse(OrderProtocol* socket, uint8_t* data) override { parse(data); } + size_t get_size() override { return StackPacket::get_size(); } + uint16_t get_id() override { return StackPacket::get_id(); } - void set_pointer(size_t index, void* pointer) override{ - StackPacket::set_pointer(index, pointer); - } + void set_pointer(size_t index, void* pointer) override { + StackPacket::set_pointer(index, pointer); + } }; #if __cpp_deduction_guides >= 201606 -template requires NotCallablePack -StackOrder(uint16_t id,void(*callback)(void), Types*... values)->StackOrder<(!has_container::value)*total_sizeof::value, Types...>; +template + requires NotCallablePack +StackOrder(uint16_t id, void (*callback)(void), Types*... values) + -> StackOrder<(!has_container::value) * total_sizeof::value, Types...>; -template requires NotCallablePack -StackOrder(uint16_t id, Types*... values)->StackOrder<(!has_container::value)*total_sizeof::value, Types...>; +template + requires NotCallablePack +StackOrder(uint16_t id, Types*... values) + -> StackOrder<(!has_container::value) * total_sizeof::value, Types...>; #endif -class HeapOrder : public HeapPacket, public Order{ +class HeapOrder : public HeapPacket, public Order { public: - template - HeapOrder(uint16_t id,void(*callback)(void), Types*... values) : HeapPacket(id,values...), callback(callback) {orders[id] = this;} - - template - HeapOrder(uint16_t id, Types*... values) : HeapPacket(id,values...) {orders[id] = this;} + template + HeapOrder(uint16_t id, void (*callback)(void), Types*... values) + : HeapPacket(id, values...), callback(callback) { + orders[id] = this; + } - void(*callback)(void) = nullptr; - void set_callback(void(*callback)(void)) override { - this->callback = callback; + template HeapOrder(uint16_t id, Types*... values) : HeapPacket(id, values...) { + orders[id] = this; } + + void (*callback)(void) = nullptr; + void set_callback(void (*callback)(void)) override { this->callback = callback; } void process() override { - if (callback != nullptr) callback(); + if (callback != nullptr) + callback(); } - uint8_t* build() override { - return HeapPacket::build(); + uint8_t* build() override { return HeapPacket::build(); } + void parse(uint8_t* data) override { HeapPacket::parse(data); } + void parse(OrderProtocol* socket, uint8_t* data) { parse(data); } + size_t get_size() override { return HeapPacket::get_size(); } + uint16_t get_id() override { return HeapPacket::get_id(); } + void set_pointer(size_t index, void* pointer) override { + HeapPacket::set_pointer(index, pointer); } - void parse(uint8_t* data) override { - HeapPacket::parse(data); - } - void parse(OrderProtocol* socket, uint8_t* data){ - parse(data); - } - size_t get_size() override { - return HeapPacket::get_size(); - } - uint16_t get_id() override { - return HeapPacket::get_id(); - } - void set_pointer(size_t index, void* pointer) override{ - HeapPacket::set_pointer(index, pointer); - } - }; diff --git a/Inc/HALAL/Models/Packets/OrderProtocol.hpp b/Inc/HALAL/Models/Packets/OrderProtocol.hpp index 566fd7a85..5e07d39f4 100644 --- a/Inc/HALAL/Models/Packets/OrderProtocol.hpp +++ b/Inc/HALAL/Models/Packets/OrderProtocol.hpp @@ -3,7 +3,7 @@ class Order; -class OrderProtocol{ +class OrderProtocol { public: virtual bool send_order(Order& order) = 0; static vector sockets; diff --git a/Inc/HALAL/Models/Packets/Packet.hpp b/Inc/HALAL/Models/Packets/Packet.hpp index 1da9559dd..e76491b7e 100644 --- a/Inc/HALAL/Models/Packets/Packet.hpp +++ b/Inc/HALAL/Models/Packets/Packet.hpp @@ -3,51 +3,47 @@ #include "HALAL/Models/Packets/PacketValue.hpp" #include "HALAL/Models/DataStructures/StackTuple.hpp" -class Packet{ +class Packet { public: size_t size; virtual uint8_t* build() = 0; virtual void parse(uint8_t* data) = 0; virtual size_t get_size() = 0; - virtual uint16_t get_id() = 0; + virtual uint16_t get_id() = 0; virtual void set_pointer(size_t index, void* pointer) = 0; - static uint16_t get_id(uint8_t* data) { - return *((uint16_t*)data); - } - static void parse_data(uint8_t* data){ + static uint16_t get_id(uint8_t* data) { return *((uint16_t*)data); } + static void parse_data(uint8_t* data) { uint16_t id = get_id(data); - if(packets.contains(id)) packets[id]->parse(data); + if (packets.contains(id)) + packets[id]->parse(data); } + protected: static map packets; }; -template -class StackPacket : public Packet{ +template class StackPacket : public Packet { public: uint16_t id; PacketValue<>* values[sizeof...(Types)]; uint8_t buffer[BufferLength + sizeof(id)]; size_t& size = Packet::size; stack_tuple...> packetvalue_warehouse; - StackPacket(uint16_t id, Types*... values): id(id), packetvalue_warehouse{PacketValue(values)...} { + StackPacket(uint16_t id, Types*... values) + : id(id), packetvalue_warehouse{PacketValue(values)...} { int i = 0; - packetvalue_warehouse.for_each([this, &i](auto& value) { - this->values[i++] = &value; - }); + packetvalue_warehouse.for_each([this, &i](auto& value) { this->values[i++] = &value; }); packets[id] = this; size = BufferLength + sizeof(id); } void parse(uint8_t* data) override { - data+=sizeof(id); + data += sizeof(id); for (PacketValue<>* value : values) { value->parse(data); data += value->get_size(); } } - size_t get_size() override { - return size; - } + size_t get_size() override { return size; } uint8_t* build() override { uint8_t* data = buffer; memcpy(data, &id, sizeof(id)); @@ -59,17 +55,12 @@ class StackPacket : public Packet{ return buffer; } - uint16_t get_id() override { - return id; - } + uint16_t get_id() override { return id; } - void set_pointer(size_t index, void* pointer) override{ - values[index]->set_pointer(pointer); - } + void set_pointer(size_t index, void* pointer) override { values[index]->set_pointer(pointer); } }; -template -class StackPacket<0, Types...> : public Packet{ +template class StackPacket<0, Types...> : public Packet { public: uint16_t id; PacketValue<>* values[sizeof...(Types)]; @@ -78,23 +69,22 @@ class StackPacket<0, Types...> : public Packet{ size_t& data_size = Packet::size; uint8_t* buffer = nullptr; - StackPacket(uint16_t id, Types*... values): id(id), packetvalue_warehouse{PacketValue(values)...} { + StackPacket(uint16_t id, Types*... values) + : id(id), packetvalue_warehouse{PacketValue(values)...} { int i = 0; - packetvalue_warehouse.for_each([this, &i](auto& value) { - this->values[i++] = &value; - }); + packetvalue_warehouse.for_each([this, &i](auto& value) { this->values[i++] = &value; }); packets[id] = this; } void parse(uint8_t* data) override { - data+=sizeof(id); + data += sizeof(id); for (PacketValue<>* value : values) { value->parse(data); data += value->get_size(); } } - size_t get_size()override { + size_t get_size() override { size_t new_size = 0; for (PacketValue<>* value : values) { new_size += value->get_size(); @@ -104,12 +94,11 @@ class StackPacket<0, Types...> : public Packet{ uint8_t* build() override { data_size = get_size(); - if (buffer != nullptr && (data_size > buffer_size || data_size < buffer_size/2)){ + if (buffer != nullptr && (data_size > buffer_size || data_size < buffer_size / 2)) { delete[] buffer; - buffer = new uint8_t[data_size]; + buffer = new uint8_t[data_size]; buffer_size = data_size; - } - else if (buffer == nullptr) { + } else if (buffer == nullptr) { buffer = new uint8_t[data_size]; buffer_size = data_size; } @@ -124,49 +113,39 @@ class StackPacket<0, Types...> : public Packet{ return buffer; } - uint16_t get_id() override { - return id; - } + uint16_t get_id() override { return id; } - void set_pointer(size_t index, void* pointer) override{ - values[index]->set_pointer(pointer); - } + void set_pointer(size_t index, void* pointer) override { values[index]->set_pointer(pointer); } ~StackPacket() { - if (buffer != nullptr) delete[] buffer; + if (buffer != nullptr) + delete[] buffer; } }; -template<> -class StackPacket<0> : public Packet{ +template <> class StackPacket<0> : public Packet { public: uint16_t id; const size_t size = sizeof(id); StackPacket() = default; - StackPacket(uint16_t id) : id(id){packets[id] = this;} + StackPacket(uint16_t id) : id(id) { packets[id] = this; } void parse(uint8_t* data) override {} - uint8_t* build() override { - return (uint8_t*)&id; - } - uint16_t get_id() override { - return id; - } - size_t get_size() override { - return sizeof(id); - } + uint8_t* build() override { return (uint8_t*)&id; } + uint16_t get_id() override { return id; } + size_t get_size() override { return sizeof(id); } - void set_pointer(size_t index, void* pointer) override {}; + void set_pointer(size_t index, void* pointer) override{}; }; - #if __cpp_deduction_guides >= 201606 -template -StackPacket(uint16_t, Types*... values)->StackPacket<(!has_container::value)*total_sizeof::value, Types...>; +template +StackPacket(uint16_t, Types*... values) + -> StackPacket<(!has_container::value) * total_sizeof::value, Types...>; -StackPacket()->StackPacket<0>; +StackPacket() -> StackPacket<0>; #endif -class HeapPacket : public Packet{ +class HeapPacket : public Packet { public: uint16_t id; vector>> values; @@ -175,10 +154,9 @@ class HeapPacket : public Packet{ size_t buffer_size = 0; HeapPacket() = default; - template - HeapPacket(uint16_t id, Types*... values): id(id) { - (this->values.emplace_back(unique_ptr>(new PacketValue(values))) , ...); - packets[id] = this; + template HeapPacket(uint16_t id, Types*... values) : id(id) { + (this->values.emplace_back(unique_ptr>(new PacketValue(values))), ...); + packets[id] = this; } void parse(uint8_t* data) override { @@ -199,12 +177,11 @@ class HeapPacket : public Packet{ uint8_t* build() override { data_size = get_size(); - if (buffer != nullptr && (data_size > buffer_size || data_size < buffer_size/2)){ + if (buffer != nullptr && (data_size > buffer_size || data_size < buffer_size / 2)) { delete[] buffer; - buffer = new uint8_t[data_size]; + buffer = new uint8_t[data_size]; buffer_size = data_size; - } - else if (buffer == nullptr) { + } else if (buffer == nullptr) { buffer = new uint8_t[data_size]; buffer_size = data_size; } @@ -217,15 +194,12 @@ class HeapPacket : public Packet{ } return buffer; } - uint16_t get_id() override { - return id; - } + uint16_t get_id() override { return id; } - void set_pointer(size_t index, void* pointer) override{ - values[index]->set_pointer(pointer); - } + void set_pointer(size_t index, void* pointer) override { values[index]->set_pointer(pointer); } ~HeapPacket() { - if(buffer != nullptr) delete[] buffer; + if (buffer != nullptr) + delete[] buffer; } }; diff --git a/Inc/HALAL/Models/Packets/PacketValue.hpp b/Inc/HALAL/Models/Packets/PacketValue.hpp index b713ef7a2..529000d93 100644 --- a/Inc/HALAL/Models/Packets/PacketValue.hpp +++ b/Inc/HALAL/Models/Packets/PacketValue.hpp @@ -7,8 +7,7 @@ struct empty_type {}; template class PacketValue; -template<> -class PacketValue<> { +template <> class PacketValue<> { public: using value_type = empty_type; PacketValue() = default; @@ -20,190 +19,128 @@ class PacketValue<> { virtual void copy_to(uint8_t* data) = 0; }; -template requires NotContainer -class PacketValue: public PacketValue<> { +template + requires NotContainer +class PacketValue : public PacketValue<> { public: using value_type = Type; Type* src = nullptr; PacketValue() = default; - PacketValue(Type* src): src(src) {} + PacketValue(Type* src) : src(src) {} ~PacketValue() = default; - void* get_pointer() override { - return src; - } + void* get_pointer() override { return src; } - void set_pointer(void* pointer){ - src = (Type*)pointer; - } + void set_pointer(void* pointer) { src = (Type*)pointer; } - size_t get_size() override { - return sizeof(Type); - } - void parse(uint8_t* data) override { - *src = *((Type*)data); - } - void copy_to(uint8_t* data) override { - *((Type*)data) = *src; - } + size_t get_size() override { return sizeof(Type); } + void parse(uint8_t* data) override { *src = *((Type*)data); } + void copy_to(uint8_t* data) override { *((Type*)data) = *src; } }; - -template<> -class PacketValue: public PacketValue<> { +template <> class PacketValue : public PacketValue<> { public: using value_type = double; double* src = nullptr; PacketValue() = default; - PacketValue(double* src): src(src) {} + PacketValue(double* src) : src(src) {} ~PacketValue() = default; - void* get_pointer() override { - return src; - } + void* get_pointer() override { return src; } - void set_pointer(void* pointer){ - src = (double*)pointer; - } + void set_pointer(void* pointer) { src = (double*)pointer; } - size_t get_size() override { - return sizeof(double); - } - void parse(uint8_t* data) override { - *src = *((double*)data); - } - void copy_to(uint8_t* data) override { - memcpy(data,src,get_size()); - } + size_t get_size() override { return sizeof(double); } + void parse(uint8_t* data) override { *src = *((double*)data); } + void copy_to(uint8_t* data) override { memcpy(data, src, get_size()); } }; #if __cpp_deduction_guides >= 201606 PacketValue(double*) -> PacketValue; #endif -template requires Container -class PacketValue: public PacketValue<> { +template + requires Container +class PacketValue : public PacketValue<> { public: using value_type = Type; Type* src = nullptr; PacketValue() = default; - PacketValue(Type* src): src(src) {} + PacketValue(Type* src) : src(src) {} ~PacketValue() = default; - void* get_pointer() override { - return src->data(); - } + void* get_pointer() override { return src->data(); } - void set_pointer(void* pointer){ - src = (Type*)pointer; - } + void set_pointer(void* pointer) { src = (Type*)pointer; } - size_t get_size() override { - return src->size()*sizeof(typename Type::value_type); - } - void parse(uint8_t* data) override { - memcpy(src->data(), data, get_size()); - } - void copy_to(uint8_t* data) override { - memcpy(data, src->data(), get_size()); - } + size_t get_size() override { return src->size() * sizeof(typename Type::value_type); } + void parse(uint8_t* data) override { memcpy(src->data(), data, get_size()); } + void copy_to(uint8_t* data) override { memcpy(data, src->data(), get_size()); } }; #if __cpp_deduction_guides >= 201606 -template -PacketValue(Type*)->PacketValue; +template PacketValue(Type*) -> PacketValue; #endif -template<> -class PacketValue : public PacketValue<>{ +template <> class PacketValue : public PacketValue<> { public: using value_type = string; string* src = nullptr; PacketValue() = default; - PacketValue(string* src): src(src) {} + PacketValue(string* src) : src(src) {} ~PacketValue() = default; - void* get_pointer() override { - return src->data(); - } + void* get_pointer() override { return src->data(); } - void set_pointer(void* pointer){ - src = (string*)pointer; - } + void set_pointer(void* pointer) { src = (string*)pointer; } - size_t get_size() override { - return strlen(src->c_str()) + 1; - } - void parse(uint8_t* data) override { - memcpy(src->data(), data, get_size()); - } - void copy_to(uint8_t* data) override { - memcpy(data, src->data(), get_size()); - } + size_t get_size() override { return strlen(src->c_str()) + 1; } + void parse(uint8_t* data) override { memcpy(src->data(), data, get_size()); } + void copy_to(uint8_t* data) override { memcpy(data, src->data(), get_size()); } }; -template -class PacketValue: public PacketValue<> { +template class PacketValue : public PacketValue<> { public: using value_type = Type; - Type(*src )[N] = nullptr; + Type (*src)[N] = nullptr; PacketValue() = default; - PacketValue(Type(*src)[N]): src(src) {} + PacketValue(Type (*src)[N]) : src(src) {} ~PacketValue() = default; - void* get_pointer() override { - return src; - } + void* get_pointer() override { return src; } - void set_pointer(void* pointer){ - src = (Type(*)[N])pointer; - } + void set_pointer(void* pointer) { src = (Type(*)[N])pointer; } - size_t get_size() override { - return N*sizeof(Type); - } - void parse(uint8_t* data) override { - memcpy(src, data, get_size()); - } - void copy_to(uint8_t* data) override { - memcpy(data, src, get_size()); - } + size_t get_size() override { return N * sizeof(Type); } + void parse(uint8_t* data) override { memcpy(src, data, get_size()); } + void copy_to(uint8_t* data) override { memcpy(data, src, get_size()); } }; #if __cpp_deduction_guides >= 201606 -template -PacketValue(Type(*)[N])->PacketValue; +template PacketValue(Type (*)[N]) -> PacketValue; #endif -template -class PacketValue: public PacketValue<> { +template class PacketValue : public PacketValue<> { public: using value_type = Type*; - Type*(*src )[N] = nullptr; + Type* (*src)[N] = nullptr; PacketValue() = default; - PacketValue(Type*(*src)[N]): src(src) {} + PacketValue(Type* (*src)[N]) : src(src) {} ~PacketValue() = default; - void* get_pointer() override { - return src; - } + void* get_pointer() override { return src; } - void set_pointer(void* pointer){ - src = (Type*(*)[N])pointer; - } + void set_pointer(void* pointer) { src = (Type * (*)[N]) pointer; } - size_t get_size() override { - return N*sizeof(Type); - } + size_t get_size() override { return N * sizeof(Type); } void parse(uint8_t* data) override { - for(Type* i : *src) { + for (Type* i : *src) { *i = *(Type*)data; data += sizeof(Type); } } void copy_to(uint8_t* data) override { - for(Type* i : *src) { + for (Type* i : *src) { *(Type*)data = *i; data += sizeof(Type); } } }; -#if __cpp_deduction_guides >= 201606 -template -PacketValue(Type*(*)[N])->PacketValue; +#if __cpp_deduction_guides >= 201606 +template PacketValue(Type* (*)[N]) -> PacketValue; #endif diff --git a/Inc/HALAL/Models/Packets/RawPacket.hpp b/Inc/HALAL/Models/Packets/RawPacket.hpp index 753f7dff2..360f39f10 100644 --- a/Inc/HALAL/Models/Packets/RawPacket.hpp +++ b/Inc/HALAL/Models/Packets/RawPacket.hpp @@ -15,8 +15,7 @@ * in bytes and the number of bytes. * */ -class RawPacket -{ +class RawPacket { uint32_t size; /**< Size in bytes of the data. */ uint8_t* data; /**< Data pointer. */ @@ -24,16 +23,15 @@ class RawPacket RawPacket(){}; public: - /** * @brief Construct a new Raw Packet object. The constructor will create an empty packet * of the specified size. This constructor is intended for receive packets. - * + * * @param size Size in bytes of the packet data. */ RawPacket(uint32_t size) { - this->size = size; - this->data = (uint8_t*)malloc(this->size); + this->size = size; + this->data = (uint8_t*)malloc(this->size); } /** @@ -43,8 +41,7 @@ class RawPacket * @tparam Type Generic data type. * @param data Data to be stored in the packet. */ - template - RawPacket(Type* data) { + template RawPacket(Type* data) { this->size = sizeof(Type); this->data = (uint8_t*)malloc(this->size); @@ -59,8 +56,7 @@ class RawPacket * @param data Pointer to the data to be store. * @param size Size of the array. */ - template - RawPacket(Type* data, uint32_t size){ + template RawPacket(Type* data, uint32_t size) { this->size = size * sizeof(Type); this->data = (uint8_t*)malloc(this->size); memcpy(this->data, data, this->size); @@ -80,16 +76,12 @@ class RawPacket * * @return uint32_t Returns the data size in bytes. */ - uint32_t get_size() { - return size; - } + uint32_t get_size() { return size; } /** * @brief Get the pointer to the data. * * @return uint8_t* Returns a pointer to the packet data. */ - uint8_t* get_data() { - return data; - } + uint8_t* get_data() { return data; } }; diff --git a/Inc/HALAL/Models/Packets/SPIOrder.hpp b/Inc/HALAL/Models/Packets/SPIOrder.hpp index f7b743d51..845fe7ce0 100644 --- a/Inc/HALAL/Models/Packets/SPIOrder.hpp +++ b/Inc/HALAL/Models/Packets/SPIOrder.hpp @@ -5,201 +5,218 @@ * Author: ricardo */ - #include "HALAL/Models/Packets/SPIPacket.hpp" -#define ALIGN_NUMBER_TO_32(x) (((x + 31)/32)*32) +#define ALIGN_NUMBER_TO_32(x) (((x + 31) / 32) * 32) #define SPI_ID_SIZE 2 #define PAYLOAD_OVERHEAD SPI_ID_SIZE #define PAYLOAD_TAIL SPI_ID_SIZE #define SPI_MAXIMUM_PAYLOAD_SIZE_BYTES 120 -#define SPI_MAXIMUM_PACKET_SIZE_BYTES (SPI_MAXIMUM_PAYLOAD_SIZE_BYTES + PAYLOAD_OVERHEAD + PAYLOAD_TAIL) +#define SPI_MAXIMUM_PACKET_SIZE_BYTES \ + (SPI_MAXIMUM_PAYLOAD_SIZE_BYTES + PAYLOAD_OVERHEAD + PAYLOAD_TAIL) #define NO_ORDER_ID 0 #define ERROR_ORDER_ID 1 -#define CASTED_ERROR_ORDER_ID (uint16_t) 0b100000000 - +#define CASTED_ERROR_ORDER_ID (uint16_t)0b100000000 /** * @brief a Base for all Order classes. Any Order class can be stored on a pointer of this type. */ -class SPIBaseOrder{ +class SPIBaseOrder { public: - static map SPIOrdersByID; - - - uint16_t id; /**< Number of the Order ID, saved on the first two bytes of both payloads, even if the values sent are unused*/ - uint8_t* MISO_payload; /**< Byte Buffer for the slave DMA output*/ - uint8_t* MOSI_payload; /**< Byte Buffer for the master DMA output*/ + static map SPIOrdersByID; + + uint16_t id; /**< Number of the Order ID, saved on the first two bytes of both payloads, even if + the values sent are unused*/ + uint8_t* MISO_payload; /**< Byte Buffer for the slave DMA output*/ + uint8_t* MOSI_payload; /**< Byte Buffer for the master DMA output*/ + + uint16_t payload_size; /**< Size in bytes of both DMA buffers (both have to be the same)*/ + uint32_t CRC_index; + + uint8_t* master_data; /**< Pointer to the master Byte buffer after the header (the space where + the data is saved)*/ + uint16_t master_data_size; + uint8_t* slave_data; /**< Pointer to the slave Byte buffer after the header (the space where the + data is saved)*/ + uint16_t slave_data_size; + uint16_t greater_data_size; + void (*callback)(void + ) = nullptr; /**< callback function called at the end of the Order transaction*/ - uint16_t payload_size; /**< Size in bytes of both DMA buffers (both have to be the same)*/ - uint32_t CRC_index; - - uint8_t* master_data; /**< Pointer to the master Byte buffer after the header (the space where the data is saved)*/ - uint16_t master_data_size; - uint8_t* slave_data; /**< Pointer to the slave Byte buffer after the header (the space where the data is saved)*/ - uint16_t slave_data_size; - uint16_t greater_data_size; - void(*callback)(void) = nullptr; /**< callback function called at the end of the Order transaction*/ - - /** - * @brief function used to change the callback function of the Order - */ - virtual void set_callback(void(*callback)(void)) { - this->callback = callback; - } + /** + * @brief function used to change the callback function of the Order + */ + virtual void set_callback(void (*callback)(void)) { this->callback = callback; } - virtual void master_prepare_buffer(uint8_t* tx_buffer){/**< function used in SPI callback*/ - load_dma_buffer(tx_buffer, master_data, master_data_size + PAYLOAD_TAIL, 0); + virtual void master_prepare_buffer(uint8_t* tx_buffer) { /**< function used in SPI callback*/ + load_dma_buffer(tx_buffer, master_data, master_data_size + PAYLOAD_TAIL, 0); } - virtual void slave_prepare_buffer(uint8_t* tx_buffer){/**< function used in SPI callback*/ - load_dma_buffer(tx_buffer, MISO_payload, payload_size, 0); + virtual void slave_prepare_buffer(uint8_t* tx_buffer) { /**< function used in SPI callback*/ + load_dma_buffer(tx_buffer, MISO_payload, payload_size, 0); } - virtual void master_process_callback(uint8_t* rx_buffer){/**< function used in SPI callback*/ - load_dma_buffer(slave_data, rx_buffer, slave_data_size, 0); - if (callback != nullptr) callback(); + virtual void master_process_callback(uint8_t* rx_buffer) { /**< function used in SPI callback*/ + load_dma_buffer(slave_data, rx_buffer, slave_data_size, 0); + if (callback != nullptr) + callback(); } - virtual void slave_process_callback(uint8_t* rx_buffer){/**< function used in SPI callback*/ - load_dma_buffer(master_data, rx_buffer, master_data_size, PAYLOAD_OVERHEAD); - if (callback != nullptr) callback(); + virtual void slave_process_callback(uint8_t* rx_buffer) { /**< function used in SPI callback*/ + load_dma_buffer(master_data, rx_buffer, master_data_size, PAYLOAD_OVERHEAD); + if (callback != nullptr) + callback(); } protected: /** - * @brief the base constructor for all SPIOrders, that creates the structure used by the DMA to send and receive messages, along with having virtual functions + * @brief the base constructor for all SPIOrders, that creates the structure used by the DMA to + * send and receive messages, along with having virtual functions */ - SPIBaseOrder(uint16_t id, uint16_t master_data_size, uint16_t slave_data_size) : id(id), master_data_size(master_data_size), slave_data_size(slave_data_size){ - if(id == 0){ - ErrorHandler("Cannot use 0 as the SPIOrderID, as it is reserved to the no Order ready signal"); - } - if(master_data_size > slave_data_size){ - payload_size = master_data_size+PAYLOAD_OVERHEAD+PAYLOAD_TAIL; - }else{ - payload_size = slave_data_size+PAYLOAD_OVERHEAD+PAYLOAD_TAIL; - } - if(payload_size > SPI_MAXIMUM_PAYLOAD_SIZE_BYTES){ - ErrorHandler("Cannot declare SPIOrder %d as its size surpasses the maximum data size",id); - } - MISO_payload = new uint8_t[payload_size]{0}; - MOSI_payload = new uint8_t[payload_size]{0}; - master_data = &MOSI_payload[PAYLOAD_OVERHEAD]; - slave_data = &MISO_payload[PAYLOAD_OVERHEAD]; - MISO_payload[0] = (uint8_t) id; - MISO_payload[1] = (uint8_t) (id>>8); - MOSI_payload[0] = (uint8_t) id; - MOSI_payload[1] = (uint8_t) (id>>8); - - CRC_index = payload_size - 2; - MISO_payload[CRC_index] = (uint8_t) id; - MISO_payload[CRC_index+1] = (uint8_t) (id>>8); - MOSI_payload[CRC_index] = (uint8_t) id; - MOSI_payload[CRC_index+1] = (uint8_t) (id>>8); - SPIBaseOrder::SPIOrdersByID[id] = this; - } - - SPIBaseOrder(SPIBaseOrder& baseOrder) = default; - virtual ~SPIBaseOrder(); - - void read_dma_buffer(uint8_t* dma_buffer, uint8_t* spi_buffer, uint16_t buffer_size, uint8_t start_copy_byte){ - memcpy(spi_buffer, dma_buffer + start_copy_byte, buffer_size); + SPIBaseOrder(uint16_t id, uint16_t master_data_size, uint16_t slave_data_size) + : id(id), master_data_size(master_data_size), slave_data_size(slave_data_size) { + if (id == 0) { + ErrorHandler( + "Cannot use 0 as the SPIOrderID, as it is reserved to the no Order ready signal" + ); + } + if (master_data_size > slave_data_size) { + payload_size = master_data_size + PAYLOAD_OVERHEAD + PAYLOAD_TAIL; + } else { + payload_size = slave_data_size + PAYLOAD_OVERHEAD + PAYLOAD_TAIL; + } + if (payload_size > SPI_MAXIMUM_PAYLOAD_SIZE_BYTES) { + ErrorHandler( + "Cannot declare SPIOrder %d as its size surpasses the maximum data size", + id + ); + } + MISO_payload = new uint8_t[payload_size]{0}; + MOSI_payload = new uint8_t[payload_size]{0}; + master_data = &MOSI_payload[PAYLOAD_OVERHEAD]; + slave_data = &MISO_payload[PAYLOAD_OVERHEAD]; + MISO_payload[0] = (uint8_t)id; + MISO_payload[1] = (uint8_t)(id >> 8); + MOSI_payload[0] = (uint8_t)id; + MOSI_payload[1] = (uint8_t)(id >> 8); + + CRC_index = payload_size - 2; + MISO_payload[CRC_index] = (uint8_t)id; + MISO_payload[CRC_index + 1] = (uint8_t)(id >> 8); + MOSI_payload[CRC_index] = (uint8_t)id; + MOSI_payload[CRC_index + 1] = (uint8_t)(id >> 8); + SPIBaseOrder::SPIOrdersByID[id] = this; } - void load_dma_buffer(uint8_t* dma_buffer, uint8_t* spi_buffer, uint16_t buffer_size, uint8_t start_copy_byte){ - memcpy(dma_buffer, spi_buffer + start_copy_byte, buffer_size); + SPIBaseOrder(SPIBaseOrder& baseOrder) = default; + virtual ~SPIBaseOrder(); + + void read_dma_buffer( + uint8_t* dma_buffer, + uint8_t* spi_buffer, + uint16_t buffer_size, + uint8_t start_copy_byte + ) { + memcpy(spi_buffer, dma_buffer + start_copy_byte, buffer_size); } + void load_dma_buffer( + uint8_t* dma_buffer, + uint8_t* spi_buffer, + uint16_t buffer_size, + uint8_t start_copy_byte + ) { + memcpy(dma_buffer, spi_buffer + start_copy_byte, buffer_size); + } }; - - /** * @brief An order that simply sends the binary data inside the data arrays */ -class SPIBinaryOrder : public SPIBaseOrder{ +class SPIBinaryOrder : public SPIBaseOrder { public: - uint8_t* master_array; - uint8_t* slave_array; - - SPIBinaryOrder(uint16_t id, uint16_t master_data_size, uint16_t slave_data_size) : SPIBaseOrder(id, master_data_size, slave_data_size) - { - master_array = SPIBaseOrder::master_data; - slave_array = SPIBaseOrder::slave_data; - } -}; - + uint8_t* master_array; + uint8_t* slave_array; + SPIBinaryOrder(uint16_t id, uint16_t master_data_size, uint16_t slave_data_size) + : SPIBaseOrder(id, master_data_size, slave_data_size) { + master_array = SPIBaseOrder::master_data; + slave_array = SPIBaseOrder::slave_data; + } +}; /** * @brief SPIWordOrder works like SPIBinaryOrder but using full words of data instead of bytes * - * While SPIWordOrder could be emulated easily by the SPIStackOrder, the use of this specific case is - * common enough to have interest on increasing the efficiency of its execution - * This version is simply faster than an SPIStackOrder of words as it doesn t need to copy or prepare buffers. - * The tradeoff is that it lacks the comodity of reading pointers to external variables - * and the values have to be copied inside of the master_array and slave_array, specifically. - * Copying into the master_data or slave_data will result in the data being send in bytes instead of words, - * which will result in undesired data as the memory on stm microcontrollers is on Big endian. - * Its existence is also a warning on the effects of Big endian when sending buffers of non size 1 data. + * While SPIWordOrder could be emulated easily by the SPIStackOrder, the use of this specific case + * is common enough to have interest on increasing the efficiency of its execution This version is + * simply faster than an SPIStackOrder of words as it doesn t need to copy or prepare buffers. The + * tradeoff is that it lacks the comodity of reading pointers to external variables and the values + * have to be copied inside of the master_array and slave_array, specifically. Copying into the + * master_data or slave_data will result in the data being send in bytes instead of words, which + * will result in undesired data as the memory on stm microcontrollers is on Big endian. Its + * existence is also a warning on the effects of Big endian when sending buffers of non size 1 data. */ -class SPIWordOrder : public SPIBaseOrder{ +class SPIWordOrder : public SPIBaseOrder { public: - uint32_t* master_array; - uint32_t* slave_array; - - SPIWordOrder(uint16_t id, uint16_t master_word_count, uint16_t slave_word_count) : SPIBaseOrder(id, 4*master_word_count, 4*slave_word_count) - { - master_array = (uint32_t*)SPIBaseOrder::master_data; - slave_array = (uint32_t*)SPIBaseOrder::slave_data; - } -}; - + uint32_t* master_array; + uint32_t* slave_array; + SPIWordOrder(uint16_t id, uint16_t master_word_count, uint16_t slave_word_count) + : SPIBaseOrder(id, 4 * master_word_count, 4 * slave_word_count) { + master_array = (uint32_t*)SPIBaseOrder::master_data; + slave_array = (uint32_t*)SPIBaseOrder::slave_data; + } +}; /** - * @brief version of the SPIOrder that receives packet structures and uses the defined variables in there to communicate + * @brief version of the SPIOrder that receives packet structures and uses the defined variables in + * there to communicate * - * To use it the user will need to code first the SPIPackets for the master and slave and then give those structures - * in the constructor of this class. The class will access to the pointers defined inside the Packets and will use them - * to store received data and send its values. - * A master comunicator will perform read operations on the master_packet and write on the slave_packet, - * while the slave will perform reads on slave_packet to send the values inside of it, and will write on the master_packet variables. + * To use it the user will need to code first the SPIPackets for the master and slave and then give + * those structures in the constructor of this class. The class will access to the pointers defined + * inside the Packets and will use them to store received data and send its values. A master + * comunicator will perform read operations on the master_packet and write on the slave_packet, + * while the slave will perform reads on slave_packet to send the values inside of it, and will + * write on the master_packet variables. */ -class SPIStackOrder : public SPIBaseOrder{ +class SPIStackOrder : public SPIBaseOrder { public: - SPIBasePacket& master_packet; - SPIBasePacket& slave_packet; - - SPIStackOrder(uint16_t id, SPIBasePacket& master_packet, SPIBasePacket& slave_packet) : - SPIBaseOrder(id, (uint16_t)master_packet.size, (uint16_t)slave_packet.size), - master_packet(master_packet), slave_packet(slave_packet) - { - - } - - void master_prepare_buffer(uint8_t* tx_buffer) override{ - memcpy(SPIBaseOrder::master_data, master_packet.build(), master_packet.size); - load_dma_buffer(tx_buffer, MOSI_payload + PAYLOAD_OVERHEAD, payload_size - PAYLOAD_OVERHEAD, 0); - } - - void slave_prepare_buffer(uint8_t* tx_buffer) override{ - memcpy(SPIBaseOrder::slave_data, slave_packet.build(), slave_packet.size); - load_dma_buffer(tx_buffer, MISO_payload, payload_size, 0); - } - - void master_process_callback(uint8_t* rx_buffer) override{ - read_dma_buffer(rx_buffer, slave_data, slave_data_size, 0); - slave_packet.parse(SPIBaseOrder::slave_data); - if (callback != nullptr) callback(); - } - - void slave_process_callback(uint8_t* rx_buffer) override{ - read_dma_buffer(rx_buffer, master_data, master_data_size, PAYLOAD_OVERHEAD); - master_packet.parse(SPIBaseOrder::master_data); - if (callback != nullptr) callback(); - } + SPIBasePacket& master_packet; + SPIBasePacket& slave_packet; + + SPIStackOrder(uint16_t id, SPIBasePacket& master_packet, SPIBasePacket& slave_packet) + : SPIBaseOrder(id, (uint16_t)master_packet.size, (uint16_t)slave_packet.size), + master_packet(master_packet), slave_packet(slave_packet) {} + + void master_prepare_buffer(uint8_t* tx_buffer) override { + memcpy(SPIBaseOrder::master_data, master_packet.build(), master_packet.size); + load_dma_buffer( + tx_buffer, + MOSI_payload + PAYLOAD_OVERHEAD, + payload_size - PAYLOAD_OVERHEAD, + 0 + ); + } + + void slave_prepare_buffer(uint8_t* tx_buffer) override { + memcpy(SPIBaseOrder::slave_data, slave_packet.build(), slave_packet.size); + load_dma_buffer(tx_buffer, MISO_payload, payload_size, 0); + } + void master_process_callback(uint8_t* rx_buffer) override { + read_dma_buffer(rx_buffer, slave_data, slave_data_size, 0); + slave_packet.parse(SPIBaseOrder::slave_data); + if (callback != nullptr) + callback(); + } + + void slave_process_callback(uint8_t* rx_buffer) override { + read_dma_buffer(rx_buffer, master_data, master_data_size, PAYLOAD_OVERHEAD); + master_packet.parse(SPIBaseOrder::master_data); + if (callback != nullptr) + callback(); + } }; diff --git a/Inc/HALAL/Models/Packets/SPIPacket.hpp b/Inc/HALAL/Models/Packets/SPIPacket.hpp index 1789e2272..b17a58114 100644 --- a/Inc/HALAL/Models/Packets/SPIPacket.hpp +++ b/Inc/HALAL/Models/Packets/SPIPacket.hpp @@ -11,41 +11,39 @@ #include "HALAL/Models/Packets/PacketValue.hpp" #include "HALAL/Models/DataStructures/StackTuple.hpp" - /** - * @brief Base structure of SPIPacket used to store them without need of knowing its template definition + * @brief Base structure of SPIPacket used to store them without need of knowing its template + * definition */ -class SPIBasePacket{ +class SPIBasePacket { public: - uint8_t *buffer;/**< pointer for the variables casted into binary data*/ - size_t size;/** -class SPIPacket : public SPIBasePacket{ +template class SPIPacket : public SPIBasePacket { public: PacketValue<>* values[sizeof...(Types)]; uint8_t buffer[BufferLength]; stack_tuple...> packetvalue_warehouse; size_t& size = SPIBasePacket::size; - SPIPacket(Types*... values) : packetvalue_warehouse{PacketValue(values)...}{ + SPIPacket(Types*... values) : packetvalue_warehouse{PacketValue(values)...} { int i = 0; - packetvalue_warehouse.for_each([this, &i](auto& value) { - this->values[i++] = &value; - }); + packetvalue_warehouse.for_each([this, &i](auto& value) { this->values[i++] = &value; }); size = BufferLength; SPIBasePacket::buffer = buffer; } @@ -67,20 +65,12 @@ class SPIPacket : public SPIBasePacket{ } }; -template<> -class SPIPacket<0> : public SPIBasePacket{ +template <> class SPIPacket<0> : public SPIBasePacket { public: size_t& size = SPIBasePacket::size; - SPIPacket() { - size = 0; - } + SPIPacket() { size = 0; } - void parse(uint8_t* data) override { - } + void parse(uint8_t* data) override {} - uint8_t* build() override { - return nullptr; - } + uint8_t* build() override { return nullptr; } }; - - diff --git a/Inc/HALAL/Models/PinModel/Pin.hpp b/Inc/HALAL/Models/PinModel/Pin.hpp index 06b2b1b38..c4f2eea4c 100644 --- a/Inc/HALAL/Models/PinModel/Pin.hpp +++ b/Inc/HALAL/Models/PinModel/Pin.hpp @@ -78,7 +78,7 @@ enum PinState : uint8_t { OFF, ON }; enum TRIGGER : uint8_t { RISING_EDGE = 1, FAILING_EDGE = 0, BOTH_EDGES = 2 }; class Pin { - public: +public: GPIO_TypeDef* port; GPIOPin gpio_pin; AlternativeFunction alternative_function; @@ -98,25 +98,23 @@ class Pin { } bool operator<(const Pin& other) const { - if (port == other.port) return gpio_pin < other.gpio_pin; + if (port == other.port) + return gpio_pin < other.gpio_pin; return port < other.port; } }; namespace std { -template <> -struct hash { +template <> struct hash { std::size_t operator()(const Pin& k) const { using std::hash; using std::size_t; using std::string; - return ((hash()(k.gpio_pin) ^ - (hash()((size_t)(k.port)) << 1)) >> - 1); + return ((hash()(k.gpio_pin) ^ (hash()((size_t)(k.port)) << 1)) >> 1); } }; -} // namespace std +} // namespace std extern Pin PA0; extern Pin PA1; diff --git a/Inc/HALAL/Models/SPI/SPI2.hpp b/Inc/HALAL/Models/SPI/SPI2.hpp index dbcd23507..18ab84c96 100644 --- a/Inc/HALAL/Models/SPI/SPI2.hpp +++ b/Inc/HALAL/Models/SPI/SPI2.hpp @@ -15,35 +15,35 @@ #include "HALAL/Models/DMA/DMA2.hpp" #include "HALAL/Models/SPI/SPIConfig.hpp" -using ST_LIB::GPIODomain; using ST_LIB::DMA_Domain; +using ST_LIB::GPIODomain; using ST_LIB::SPIConfigTypes; // Forward declaration of IRQ handlers and HAL callbacks extern "C" { - void SPI1_IRQHandler(void); - void SPI2_IRQHandler(void); - void SPI3_IRQHandler(void); - void SPI4_IRQHandler(void); - void SPI5_IRQHandler(void); - void SPI6_IRQHandler(void); - - void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi); - void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi); - void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi); - void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi); +void SPI1_IRQHandler(void); +void SPI2_IRQHandler(void); +void SPI3_IRQHandler(void); +void SPI4_IRQHandler(void); +void SPI5_IRQHandler(void); +void SPI6_IRQHandler(void); + +void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef* hspi); +void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef* hspi); +void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef* hspi); +void HAL_SPI_ErrorCallback(SPI_HandleTypeDef* hspi); } namespace ST_LIB { -extern void compile_error(const char *msg); +extern void compile_error(const char* msg); struct SPIDomain { -/** - * ========================================= - * Internal working things - * ========================================= - */ + /** + * ========================================= + * Internal working things + * ========================================= + */ // Configuration types using ClockPolarity = SPIConfigTypes::ClockPolarity; @@ -71,35 +71,54 @@ struct SPIDomain { SLAVE = false, }; - static consteval bool compare_pin(const GPIODomain::Pin &p1, const GPIODomain::Pin &p2) { + static consteval bool compare_pin(const GPIODomain::Pin& p1, const GPIODomain::Pin& p2) { return (p1.port == p2.port) && (p1.pin == p2.pin); } - static consteval GPIODomain::AlternateFunction get_af(const GPIODomain::Pin &pin, SPIPeripheral peripheral) { + static consteval GPIODomain::AlternateFunction + get_af(const GPIODomain::Pin& pin, SPIPeripheral peripheral) { if (peripheral == SPIPeripheral::spi2) { - if (compare_pin(pin, PB4)) return GPIODomain::AlternateFunction::AF7; + if (compare_pin(pin, PB4)) + return GPIODomain::AlternateFunction::AF7; } if (peripheral == SPIPeripheral::spi3) { - if (compare_pin(pin, PA4)) return GPIODomain::AlternateFunction::AF6; - if (compare_pin(pin, PA15)) return GPIODomain::AlternateFunction::AF6; - if (compare_pin(pin, PB2)) return GPIODomain::AlternateFunction::AF7; - if (compare_pin(pin, PB3)) return GPIODomain::AlternateFunction::AF6; - if (compare_pin(pin, PB4)) return GPIODomain::AlternateFunction::AF6; - if (compare_pin(pin, PB5)) return GPIODomain::AlternateFunction::AF7; - if (compare_pin(pin, PC10)) return GPIODomain::AlternateFunction::AF6; - if (compare_pin(pin, PC11)) return GPIODomain::AlternateFunction::AF6; - if (compare_pin(pin, PC12)) return GPIODomain::AlternateFunction::AF6; + if (compare_pin(pin, PA4)) + return GPIODomain::AlternateFunction::AF6; + if (compare_pin(pin, PA15)) + return GPIODomain::AlternateFunction::AF6; + if (compare_pin(pin, PB2)) + return GPIODomain::AlternateFunction::AF7; + if (compare_pin(pin, PB3)) + return GPIODomain::AlternateFunction::AF6; + if (compare_pin(pin, PB4)) + return GPIODomain::AlternateFunction::AF6; + if (compare_pin(pin, PB5)) + return GPIODomain::AlternateFunction::AF7; + if (compare_pin(pin, PC10)) + return GPIODomain::AlternateFunction::AF6; + if (compare_pin(pin, PC11)) + return GPIODomain::AlternateFunction::AF6; + if (compare_pin(pin, PC12)) + return GPIODomain::AlternateFunction::AF6; } if (peripheral == SPIPeripheral::spi6) { - if (compare_pin(pin, PA4)) return GPIODomain::AlternateFunction::AF8; - if (compare_pin(pin, PA5)) return GPIODomain::AlternateFunction::AF8; - if (compare_pin(pin, PA6)) return GPIODomain::AlternateFunction::AF8; - if (compare_pin(pin, PA7)) return GPIODomain::AlternateFunction::AF8; - if (compare_pin(pin, PA15)) return GPIODomain::AlternateFunction::AF7; - if (compare_pin(pin, PB3)) return GPIODomain::AlternateFunction::AF8; - if (compare_pin(pin, PB4)) return GPIODomain::AlternateFunction::AF8; - if (compare_pin(pin, PB5)) return GPIODomain::AlternateFunction::AF8; + if (compare_pin(pin, PA4)) + return GPIODomain::AlternateFunction::AF8; + if (compare_pin(pin, PA5)) + return GPIODomain::AlternateFunction::AF8; + if (compare_pin(pin, PA6)) + return GPIODomain::AlternateFunction::AF8; + if (compare_pin(pin, PA7)) + return GPIODomain::AlternateFunction::AF8; + if (compare_pin(pin, PA15)) + return GPIODomain::AlternateFunction::AF7; + if (compare_pin(pin, PB3)) + return GPIODomain::AlternateFunction::AF8; + if (compare_pin(pin, PB4)) + return GPIODomain::AlternateFunction::AF8; + if (compare_pin(pin, PB5)) + return GPIODomain::AlternateFunction::AF8; } return GPIODomain::AlternateFunction::AF5; // Default AF for everything else @@ -107,21 +126,29 @@ struct SPIDomain { static constexpr uint32_t get_prescaler_flag(uint32_t prescaler) { switch (prescaler) { - case 2: return SPI_BAUDRATEPRESCALER_2; - case 4: return SPI_BAUDRATEPRESCALER_4; - case 8: return SPI_BAUDRATEPRESCALER_8; - case 16: return SPI_BAUDRATEPRESCALER_16; - case 32: return SPI_BAUDRATEPRESCALER_32; - case 64: return SPI_BAUDRATEPRESCALER_64; - case 128: return SPI_BAUDRATEPRESCALER_128; - case 256: return SPI_BAUDRATEPRESCALER_256; - default: - if consteval { - compile_error("Invalid prescaler value"); - } else { - ErrorHandler("Invalid prescaler value"); - return SPI_BAUDRATEPRESCALER_256; - } + case 2: + return SPI_BAUDRATEPRESCALER_2; + case 4: + return SPI_BAUDRATEPRESCALER_4; + case 8: + return SPI_BAUDRATEPRESCALER_8; + case 16: + return SPI_BAUDRATEPRESCALER_16; + case 32: + return SPI_BAUDRATEPRESCALER_32; + case 64: + return SPI_BAUDRATEPRESCALER_64; + case 128: + return SPI_BAUDRATEPRESCALER_128; + case 256: + return SPI_BAUDRATEPRESCALER_256; + default: + if consteval { + compile_error("Invalid prescaler value"); + } else { + ErrorHandler("Invalid prescaler value"); + return SPI_BAUDRATEPRESCALER_256; + } } } @@ -162,14 +189,12 @@ struct SPIDomain { SPIConfig config; // User-defined SPI configuration }; - -/** - * ========================================= - * Request Object - * ========================================= - */ - template - struct Device { + /** + * ========================================= + * Request Object + * ========================================= + */ + template struct Device { using domain = SPIDomain; SPIPeripheral peripheral; @@ -183,62 +208,113 @@ struct SPIDomain { std::optional nss_gpio; DMA_Domain::DMA dma_rx_tx; - - consteval Device(SPIMode mode, SPIPeripheral peripheral, uint32_t max_baudrate, - GPIODomain::Pin sck_pin, GPIODomain::Pin miso_pin, - GPIODomain::Pin mosi_pin, GPIODomain::Pin nss_pin, - SPIConfig config = SPIConfig{}) - : peripheral{peripheral}, mode{mode}, max_baudrate{max_baudrate}, - config{config}, - sck_gpio(sck_pin, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, get_af(sck_pin, peripheral)), - miso_gpio(miso_pin, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, get_af(miso_pin, peripheral)), - mosi_gpio(mosi_pin, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, get_af(mosi_pin, peripheral)), - nss_gpio(GPIODomain::GPIO(nss_pin, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, get_af(nss_pin, peripheral))), - dma_rx_tx(dma_peripheral(peripheral)) - { + + consteval Device( + SPIMode mode, + SPIPeripheral peripheral, + uint32_t max_baudrate, + GPIODomain::Pin sck_pin, + GPIODomain::Pin miso_pin, + GPIODomain::Pin mosi_pin, + GPIODomain::Pin nss_pin, + SPIConfig config = SPIConfig{} + ) + : peripheral{peripheral}, mode{mode}, max_baudrate{max_baudrate}, config{config}, + sck_gpio( + sck_pin, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + get_af(sck_pin, peripheral) + ), + miso_gpio( + miso_pin, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + get_af(miso_pin, peripheral) + ), + mosi_gpio( + mosi_pin, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + get_af(mosi_pin, peripheral) + ), + nss_gpio(GPIODomain::GPIO( + nss_pin, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + get_af(nss_pin, peripheral) + )), + dma_rx_tx(dma_peripheral(peripheral)) { config.validate(); if (config.nss_mode == NSSMode::SOFTWARE) { - compile_error("Use NSSMode::SOFTWARE, and omit NSS pin for software NSS management, it is handled externally"); + compile_error("Use NSSMode::SOFTWARE, and omit NSS pin for software NSS " + "management, it is handled externally"); } validate_nss_pin(peripheral, nss_pin); - + validate_spi_pins(peripheral, sck_pin, miso_pin, mosi_pin); } - + // Constructor without NSS pin (for software NSS mode) - consteval Device(SPIMode mode, SPIPeripheral peripheral, uint32_t max_baudrate, - GPIODomain::Pin sck_pin, GPIODomain::Pin miso_pin, - GPIODomain::Pin mosi_pin, - SPIConfig config) - : peripheral{peripheral}, mode{mode}, max_baudrate{max_baudrate}, - config{config}, - sck_gpio(sck_pin, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, get_af(sck_pin, peripheral)), - miso_gpio(miso_pin, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, get_af(miso_pin, peripheral)), - mosi_gpio(mosi_pin, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, get_af(mosi_pin, peripheral)), - nss_gpio(std::nullopt), // No NSS GPIO - dma_rx_tx(dma_peripheral(peripheral)) - { + consteval Device( + SPIMode mode, + SPIPeripheral peripheral, + uint32_t max_baudrate, + GPIODomain::Pin sck_pin, + GPIODomain::Pin miso_pin, + GPIODomain::Pin mosi_pin, + SPIConfig config + ) + : peripheral{peripheral}, mode{mode}, max_baudrate{max_baudrate}, config{config}, + sck_gpio( + sck_pin, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + get_af(sck_pin, peripheral) + ), + miso_gpio( + miso_pin, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + get_af(miso_pin, peripheral) + ), + mosi_gpio( + mosi_pin, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + get_af(mosi_pin, peripheral) + ), + nss_gpio(std::nullopt), // No NSS GPIO + dma_rx_tx(dma_peripheral(peripheral)) { config.validate(); - + if (config.nss_mode == NSSMode::HARDWARE) { - compile_error("NSS pin must be provided for hardware NSS mode, or use NSSMode::SOFTWARE"); + compile_error( + "NSS pin must be provided for hardware NSS mode, or use NSSMode::SOFTWARE" + ); } - + validate_spi_pins(peripheral, sck_pin, miso_pin, mosi_pin); } - template - consteval std::size_t inscribe(Ctx &ctx) const { + template consteval std::size_t inscribe(Ctx& ctx) const { auto dma_indices = dma_rx_tx.inscribe(ctx); - + // Conditionally add NSS GPIO if provided std::optional nss_idx = std::nullopt; if (nss_gpio.has_value()) { nss_idx = nss_gpio.value().inscribe(ctx); } - + Entry e{ .peripheral = peripheral, .mode = mode, @@ -257,76 +333,64 @@ struct SPIDomain { private: // Helper function to validate SPI pins (SCK, MISO, MOSI only) - static consteval void validate_spi_pins(SPIPeripheral peripheral, - GPIODomain::Pin sck_pin, - GPIODomain::Pin miso_pin, - GPIODomain::Pin mosi_pin) { + static consteval void validate_spi_pins( + SPIPeripheral peripheral, + GPIODomain::Pin sck_pin, + GPIODomain::Pin miso_pin, + GPIODomain::Pin mosi_pin + ) { switch (peripheral) { case SPIPeripheral::spi1: - if (!compare_pin(sck_pin, PB3) && - !compare_pin(sck_pin, PG11) && + if (!compare_pin(sck_pin, PB3) && !compare_pin(sck_pin, PG11) && !compare_pin(sck_pin, PA5)) { compile_error("Invalid SCK pin for SPI1"); } - if (!compare_pin(miso_pin, PB4) && - !compare_pin(miso_pin, PG9) && + if (!compare_pin(miso_pin, PB4) && !compare_pin(miso_pin, PG9) && !compare_pin(miso_pin, PA6)) { compile_error("Invalid MISO pin for SPI1"); } - if (!compare_pin(mosi_pin, PB5) && - !compare_pin(mosi_pin, PD7) && + if (!compare_pin(mosi_pin, PB5) && !compare_pin(mosi_pin, PD7) && !compare_pin(mosi_pin, PA7)) { compile_error("Invalid MOSI pin for SPI1"); } break; case SPIPeripheral::spi2: - if (!compare_pin(sck_pin, PD3) && - !compare_pin(sck_pin, PA12) && - !compare_pin(sck_pin, PA9) && - !compare_pin(sck_pin, PB13) && + if (!compare_pin(sck_pin, PD3) && !compare_pin(sck_pin, PA12) && + !compare_pin(sck_pin, PA9) && !compare_pin(sck_pin, PB13) && !compare_pin(sck_pin, PB10)) { compile_error("Invalid SCK pin for SPI2"); } - if (!compare_pin(miso_pin, PC2) && - !compare_pin(miso_pin, PB14)) { + if (!compare_pin(miso_pin, PC2) && !compare_pin(miso_pin, PB14)) { compile_error("Invalid MISO pin for SPI2"); } - if (!compare_pin(mosi_pin, PC3) && - !compare_pin(mosi_pin, PC1) && + if (!compare_pin(mosi_pin, PC3) && !compare_pin(mosi_pin, PC1) && !compare_pin(mosi_pin, PB15)) { compile_error("Invalid MOSI pin for SPI2"); } break; - + case SPIPeripheral::spi3: - if (!compare_pin(sck_pin, PB3) && - !compare_pin(sck_pin, PC10)) { + if (!compare_pin(sck_pin, PB3) && !compare_pin(sck_pin, PC10)) { compile_error("Invalid SCK pin for SPI3"); - } - if (!compare_pin(miso_pin, PB4) && - !compare_pin(miso_pin, PC11)) { + } + if (!compare_pin(miso_pin, PB4) && !compare_pin(miso_pin, PC11)) { compile_error("Invalid MISO pin for SPI3"); } - if (!compare_pin(mosi_pin, PB5) && - !compare_pin(mosi_pin, PD6) && - !compare_pin(mosi_pin, PC12) && - !compare_pin(mosi_pin, PB2)) { + if (!compare_pin(mosi_pin, PB5) && !compare_pin(mosi_pin, PD6) && + !compare_pin(mosi_pin, PC12) && !compare_pin(mosi_pin, PB2)) { compile_error("Invalid MOSI pin for SPI3"); } break; case SPIPeripheral::spi4: - if (!compare_pin(sck_pin, PE2) && - !compare_pin(sck_pin, PE12)) { + if (!compare_pin(sck_pin, PE2) && !compare_pin(sck_pin, PE12)) { compile_error("Invalid SCK pin for SPI4"); } - if (!compare_pin(miso_pin, PE5) && - !compare_pin(miso_pin, PE13)) { + if (!compare_pin(miso_pin, PE5) && !compare_pin(miso_pin, PE13)) { compile_error("Invalid MISO pin for SPI4"); } - if (!compare_pin(mosi_pin, PE6) && - !compare_pin(mosi_pin, PE14)) { + if (!compare_pin(mosi_pin, PE6) && !compare_pin(mosi_pin, PE14)) { compile_error("Invalid MOSI pin for SPI4"); } break; @@ -338,26 +402,21 @@ struct SPIDomain { if (!compare_pin(miso_pin, PF8)) { compile_error("Invalid MISO pin for SPI5"); } - if (!compare_pin(mosi_pin, PF9) && - !compare_pin(mosi_pin, PF11)) { + if (!compare_pin(mosi_pin, PF9) && !compare_pin(mosi_pin, PF11)) { compile_error("Invalid MOSI pin for SPI5"); } break; - + case SPIPeripheral::spi6: - if (!compare_pin(sck_pin, PB3) && - !compare_pin(sck_pin, PG13) && - !compare_pin(sck_pin, PC10) && - !compare_pin(sck_pin, PA7)) { + if (!compare_pin(sck_pin, PB3) && !compare_pin(sck_pin, PG13) && + !compare_pin(sck_pin, PC10) && !compare_pin(sck_pin, PA7)) { compile_error("Invalid SCK pin for SPI6"); } - if (!compare_pin(miso_pin, PB4) && - !compare_pin(miso_pin, PG12) && + if (!compare_pin(miso_pin, PB4) && !compare_pin(miso_pin, PG12) && !compare_pin(miso_pin, PA6)) { compile_error("Invalid MISO pin for SPI6"); } - if (!compare_pin(mosi_pin, PB5) && - !compare_pin(mosi_pin, PG14) && + if (!compare_pin(mosi_pin, PB5) && !compare_pin(mosi_pin, PG14) && !compare_pin(mosi_pin, PA7)) { compile_error("Invalid MOSI pin for SPI6"); } @@ -372,32 +431,27 @@ struct SPIDomain { static consteval void validate_nss_pin(SPIPeripheral peripheral, GPIODomain::Pin nss_pin) { switch (peripheral) { case SPIPeripheral::spi1: - if (!compare_pin(nss_pin, PA4) && - !compare_pin(nss_pin, PA15) && + if (!compare_pin(nss_pin, PA4) && !compare_pin(nss_pin, PA15) && !compare_pin(nss_pin, PG10)) { compile_error("Invalid NSS pin for SPI1"); } break; case SPIPeripheral::spi2: - if (!compare_pin(nss_pin, PA11) && - !compare_pin(nss_pin, PB9) && - !compare_pin(nss_pin, PB4) && - !compare_pin(nss_pin, PB12)) { + if (!compare_pin(nss_pin, PA11) && !compare_pin(nss_pin, PB9) && + !compare_pin(nss_pin, PB4) && !compare_pin(nss_pin, PB12)) { compile_error("Invalid NSS pin for SPI2"); } break; case SPIPeripheral::spi3: - if (!compare_pin(nss_pin, PA15) && - !compare_pin(nss_pin, PA4)) { + if (!compare_pin(nss_pin, PA15) && !compare_pin(nss_pin, PA4)) { compile_error("Invalid NSS pin for SPI3"); } break; case SPIPeripheral::spi4: - if (!compare_pin(nss_pin, PE4) && - !compare_pin(nss_pin, PE11)) { + if (!compare_pin(nss_pin, PE4) && !compare_pin(nss_pin, PE11)) { compile_error("Invalid NSS pin for SPI4"); } break; @@ -409,10 +463,8 @@ struct SPIDomain { break; case SPIPeripheral::spi6: - if (!compare_pin(nss_pin, PA0) && - !compare_pin(nss_pin, PA15) && - !compare_pin(nss_pin, PG8) && - !compare_pin(nss_pin, PA4)) { + if (!compare_pin(nss_pin, PA0) && !compare_pin(nss_pin, PA15) && + !compare_pin(nss_pin, PG8) && !compare_pin(nss_pin, PA4)) { compile_error("Invalid NSS pin for SPI6"); } break; @@ -424,32 +476,31 @@ struct SPIDomain { static consteval DMA_Domain::Peripheral dma_peripheral(SPIPeripheral peripheral) { switch (peripheral) { - case SPIPeripheral::spi1: - return DMA_Domain::Peripheral::spi1; - case SPIPeripheral::spi2: - return DMA_Domain::Peripheral::spi2; - case SPIPeripheral::spi3: - return DMA_Domain::Peripheral::spi3; - case SPIPeripheral::spi4: - return DMA_Domain::Peripheral::spi4; - case SPIPeripheral::spi5: - return DMA_Domain::Peripheral::spi5; - case SPIPeripheral::spi6: - return DMA_Domain::Peripheral::spi6; - default: - compile_error("Invalid SPI peripheral specified in SPIDomain::Device"); + case SPIPeripheral::spi1: + return DMA_Domain::Peripheral::spi1; + case SPIPeripheral::spi2: + return DMA_Domain::Peripheral::spi2; + case SPIPeripheral::spi3: + return DMA_Domain::Peripheral::spi3; + case SPIPeripheral::spi4: + return DMA_Domain::Peripheral::spi4; + case SPIPeripheral::spi5: + return DMA_Domain::Peripheral::spi5; + case SPIPeripheral::spi6: + return DMA_Domain::Peripheral::spi6; + default: + compile_error("Invalid SPI peripheral specified in SPIDomain::Device"); } } }; - -/** - * ========================================= - * Instance (state holder) - * ========================================= - */ - template struct Init; // Forward declaration - template struct SPIWrapper; // Forward declaration + /** + * ========================================= + * Instance (state holder) + * ========================================= + */ + template struct Init; // Forward declaration + template struct SPIWrapper; // Forward declaration struct Instance { template friend struct Init; template friend struct SPIWrapper; @@ -459,50 +510,57 @@ struct SPIDomain { friend void ::SPI4_IRQHandler(void); friend void ::SPI5_IRQHandler(void); friend void ::SPI6_IRQHandler(void); - friend void ::HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi); - friend void ::HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi); - friend void ::HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi); - friend void ::HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi); + friend void ::HAL_SPI_TxCpltCallback(SPI_HandleTypeDef* hspi); + friend void ::HAL_SPI_RxCpltCallback(SPI_HandleTypeDef* hspi); + friend void ::HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef* hspi); + friend void ::HAL_SPI_ErrorCallback(SPI_HandleTypeDef* hspi); - private: + private: SPI_HandleTypeDef hspi; SPI_TypeDef* instance; volatile bool* operation_flag = nullptr; }; - static inline Instance* spi_instances[max_instances]; -/** - * ========================================= - * Wrapper, public API - * ========================================= - */ + /** + * ========================================= + * Wrapper, public API + * ========================================= + */ // SPI Wrapper primary template - template + template struct SPIWrapper; /** * @brief SPI Wrapper for Master mode operations. */ - template - struct SPIWrapper { - static constexpr uint32_t data_bits = static_cast(device_request.config.data_size); + template struct SPIWrapper { + static constexpr uint32_t data_bits = + static_cast(device_request.config.data_size); static constexpr uint32_t frame_size = (data_bits <= 8) ? 1 : ((data_bits <= 16) ? 2 : 4); - SPIWrapper(Instance &instance) : spi_instance{instance} {} + SPIWrapper(Instance& instance) : spi_instance{instance} {} /** * @brief Sends data over SPI in blocking mode. */ - template - bool send(span data) { + template bool send(span data) { if (data.size_bytes() % frame_size != 0) { - ErrorHandler("SPI data size (%d) not aligned to frame size (%d)", data.size_bytes(), frame_size); + ErrorHandler( + "SPI data size (%d) not aligned to frame size (%d)", + data.size_bytes(), + frame_size + ); return false; } - auto error_code = HAL_SPI_Transmit(&spi_instance.hspi, (uint8_t*)data.data(), data.size_bytes() / frame_size, 10); + auto error_code = HAL_SPI_Transmit( + &spi_instance.hspi, + (uint8_t*)data.data(), + data.size_bytes() / frame_size, + 10 + ); return check_error_code(error_code); } @@ -510,25 +568,44 @@ struct SPIDomain { * @brief Sends a trivially copyable data type over SPI in blocking mode. */ template - bool send(const T& data) requires std::is_trivially_copyable_v { + bool send(const T& data) + requires std::is_trivially_copyable_v + { if (sizeof(T) % frame_size != 0) { - ErrorHandler("SPI data type size (%d) not aligned to frame size (%d)", sizeof(T), frame_size); + ErrorHandler( + "SPI data type size (%d) not aligned to frame size (%d)", + sizeof(T), + frame_size + ); return false; } - auto error_code = HAL_SPI_Transmit(&spi_instance.hspi, reinterpret_cast(&data), sizeof(T) / frame_size, 10); + auto error_code = HAL_SPI_Transmit( + &spi_instance.hspi, + reinterpret_cast(&data), + sizeof(T) / frame_size, + 10 + ); return check_error_code(error_code); } - + /** * @brief Receives data over SPI in blocking mode. */ - template - bool receive(span data) { + template bool receive(span data) { if (data.size_bytes() % frame_size != 0) { - ErrorHandler("SPI data size (%d) not aligned to frame size (%d)", data.size_bytes(), frame_size); + ErrorHandler( + "SPI data size (%d) not aligned to frame size (%d)", + data.size_bytes(), + frame_size + ); return false; } - auto error_code = HAL_SPI_Receive(&spi_instance.hspi, (uint8_t*)data.data(), data.size_bytes() / frame_size, 10); + auto error_code = HAL_SPI_Receive( + &spi_instance.hspi, + (uint8_t*)data.data(), + data.size_bytes() / frame_size, + 10 + ); return check_error_code(error_code); } @@ -536,12 +613,23 @@ struct SPIDomain { * @brief Receives a trivially copyable data type over SPI in blocking mode. */ template - bool receive(T& data) requires std::is_trivially_copyable_v { + bool receive(T& data) + requires std::is_trivially_copyable_v + { if (sizeof(T) % frame_size != 0) { - ErrorHandler("SPI data type size (%d) not aligned to frame size (%d)", sizeof(T), frame_size); + ErrorHandler( + "SPI data type size (%d) not aligned to frame size (%d)", + sizeof(T), + frame_size + ); return false; } - auto error_code = HAL_SPI_Receive(&spi_instance.hspi, reinterpret_cast(&data), sizeof(T) / frame_size, 10); + auto error_code = HAL_SPI_Receive( + &spi_instance.hspi, + reinterpret_cast(&data), + sizeof(T) / frame_size, + 10 + ); return check_error_code(error_code); } @@ -552,10 +640,20 @@ struct SPIDomain { bool transceive(span tx_data, span rx_data) { size_t size = std::min(tx_data.size_bytes(), rx_data.size_bytes()); if (size % frame_size != 0) { - ErrorHandler("SPI transaction size (%d) not aligned to frame size (%d)", size, frame_size); + ErrorHandler( + "SPI transaction size (%d) not aligned to frame size (%d)", + size, + frame_size + ); return false; } - auto error_code = HAL_SPI_TransmitReceive(&spi_instance.hspi, (uint8_t*)tx_data.data(), (uint8_t*)rx_data.data(), size / frame_size, 10); + auto error_code = HAL_SPI_TransmitReceive( + &spi_instance.hspi, + (uint8_t*)tx_data.data(), + (uint8_t*)rx_data.data(), + size / frame_size, + 10 + ); return check_error_code(error_code); } @@ -563,13 +661,25 @@ struct SPIDomain { * @brief Sends and receives a trivially copyable data type over SPI in blocking mode. */ template - bool transceive(span tx_data, T& rx_data) requires std::is_trivially_copyable_v { + bool transceive(span tx_data, T& rx_data) + requires std::is_trivially_copyable_v + { size_t size = std::min(tx_data.size_bytes(), sizeof(T)); - if (size % frame_size != 0) { - ErrorHandler("SPI transaction size (%d) not aligned to frame size (%d)", size, frame_size); + if (size % frame_size != 0) { + ErrorHandler( + "SPI transaction size (%d) not aligned to frame size (%d)", + size, + frame_size + ); return false; } - auto error_code = HAL_SPI_TransmitReceive(&spi_instance.hspi, (uint8_t*)tx_data.data(), reinterpret_cast(&rx_data), size / frame_size, 10); + auto error_code = HAL_SPI_TransmitReceive( + &spi_instance.hspi, + (uint8_t*)tx_data.data(), + reinterpret_cast(&rx_data), + size / frame_size, + 10 + ); return check_error_code(error_code); } @@ -577,13 +687,25 @@ struct SPIDomain { * @brief Sends and receives a trivially copyable data type over SPI in blocking mode. */ template - bool transceive(const T& tx_data, span rx_data) requires std::is_trivially_copyable_v { + bool transceive(const T& tx_data, span rx_data) + requires std::is_trivially_copyable_v + { size_t size = std::min(sizeof(T), rx_data.size_bytes()); - if (size % frame_size != 0) { - ErrorHandler("SPI transaction size (%d) not aligned to frame size (%d)", size, frame_size); + if (size % frame_size != 0) { + ErrorHandler( + "SPI transaction size (%d) not aligned to frame size (%d)", + size, + frame_size + ); return false; } - auto error_code = HAL_SPI_TransmitReceive(&spi_instance.hspi, reinterpret_cast(&tx_data), (uint8_t*)rx_data.data(), size / frame_size, 10); + auto error_code = HAL_SPI_TransmitReceive( + &spi_instance.hspi, + reinterpret_cast(&tx_data), + (uint8_t*)rx_data.data(), + size / frame_size, + 10 + ); return check_error_code(error_code); } @@ -591,133 +713,239 @@ struct SPIDomain { * @brief Sends and receives a trivially copyable data type over SPI in blocking mode. */ template - bool transceive(const T1& tx_data, T2& rx_data) requires (std::is_trivially_copyable_v && std::is_trivially_copyable_v) { + bool transceive(const T1& tx_data, T2& rx_data) + requires(std::is_trivially_copyable_v && std::is_trivially_copyable_v) + { size_t size = std::min(sizeof(T1), sizeof(T2)); - if (size % frame_size != 0) { - ErrorHandler("SPI transaction size (%d) not aligned to frame size (%d)", size, frame_size); + if (size % frame_size != 0) { + ErrorHandler( + "SPI transaction size (%d) not aligned to frame size (%d)", + size, + frame_size + ); return false; } - auto error_code = HAL_SPI_TransmitReceive(&spi_instance.hspi, reinterpret_cast(&tx_data), reinterpret_cast(&rx_data), size / frame_size, 10); + auto error_code = HAL_SPI_TransmitReceive( + &spi_instance.hspi, + reinterpret_cast(&tx_data), + reinterpret_cast(&rx_data), + size / frame_size, + 10 + ); return check_error_code(error_code); } /** - * @brief Sends data over SPI using DMA, uses an optional operation flag to signal completion. + * @brief Sends data over SPI using DMA, uses an optional operation flag to signal + * completion. */ template bool send_DMA(span data, volatile bool* operation_flag = nullptr) { spi_instance.operation_flag = operation_flag; if (data.size_bytes() % frame_size != 0) { - ErrorHandler("SPI data size (%d) not aligned to frame size (%d)", data.size_bytes(), frame_size); + ErrorHandler( + "SPI data size (%d) not aligned to frame size (%d)", + data.size_bytes(), + frame_size + ); return false; } - auto error_code = HAL_SPI_Transmit_DMA(&spi_instance.hspi, (uint8_t*)data.data(), data.size_bytes() / frame_size); + auto error_code = HAL_SPI_Transmit_DMA( + &spi_instance.hspi, + (uint8_t*)data.data(), + data.size_bytes() / frame_size + ); return check_error_code(error_code); } /** - * @brief Sends a trivially copyable data type over SPI using DMA, uses an optional operation flag to signal completion. + * @brief Sends a trivially copyable data type over SPI using DMA, uses an optional + * operation flag to signal completion. */ template - bool send_DMA(const T& data, volatile bool* operation_flag = nullptr) requires std::is_trivially_copyable_v { + bool send_DMA(const T& data, volatile bool* operation_flag = nullptr) + requires std::is_trivially_copyable_v + { spi_instance.operation_flag = operation_flag; - if (sizeof(T) % frame_size != 0) { - ErrorHandler("SPI data size (%d) not aligned to frame size (%d)", sizeof(T), frame_size); + if (sizeof(T) % frame_size != 0) { + ErrorHandler( + "SPI data size (%d) not aligned to frame size (%d)", + sizeof(T), + frame_size + ); return false; } - auto error_code = HAL_SPI_Transmit_DMA(&spi_instance.hspi, reinterpret_cast(&data), sizeof(T) / frame_size); + auto error_code = HAL_SPI_Transmit_DMA( + &spi_instance.hspi, + reinterpret_cast(&data), + sizeof(T) / frame_size + ); return check_error_code(error_code); } /** - * @brief Receives data over SPI using DMA, uses an optional operation flag to signal completion. + * @brief Receives data over SPI using DMA, uses an optional operation flag to signal + * completion. */ template bool receive_DMA(span data, volatile bool* operation_flag = nullptr) { spi_instance.operation_flag = operation_flag; if (data.size_bytes() % frame_size != 0) { - ErrorHandler("SPI data size (%d) not aligned to frame size (%d)", data.size_bytes(), frame_size); + ErrorHandler( + "SPI data size (%d) not aligned to frame size (%d)", + data.size_bytes(), + frame_size + ); return false; } - auto error_code = HAL_SPI_Receive_DMA(&spi_instance.hspi, (uint8_t*)data.data(), data.size_bytes() / frame_size); + auto error_code = HAL_SPI_Receive_DMA( + &spi_instance.hspi, + (uint8_t*)data.data(), + data.size_bytes() / frame_size + ); return check_error_code(error_code); } /** - * @brief Receives a trivially copyable data type over SPI using DMA, uses an optional operation flag to signal completion. + * @brief Receives a trivially copyable data type over SPI using DMA, uses an optional + * operation flag to signal completion. */ template - bool receive_DMA(T& data, volatile bool* operation_flag = nullptr) requires std::is_trivially_copyable_v { + bool receive_DMA(T& data, volatile bool* operation_flag = nullptr) + requires std::is_trivially_copyable_v + { spi_instance.operation_flag = operation_flag; - if (sizeof(T) % frame_size != 0) { - ErrorHandler("SPI data size (%d) not aligned to frame size (%d)", sizeof(T), frame_size); + if (sizeof(T) % frame_size != 0) { + ErrorHandler( + "SPI data size (%d) not aligned to frame size (%d)", + sizeof(T), + frame_size + ); return false; } - auto error_code = HAL_SPI_Receive_DMA(&spi_instance.hspi, reinterpret_cast(&data), sizeof(T) / frame_size); + auto error_code = HAL_SPI_Receive_DMA( + &spi_instance.hspi, + reinterpret_cast(&data), + sizeof(T) / frame_size + ); return check_error_code(error_code); } /** - * @brief Sends and receives data over SPI using DMA, uses an optional operation flag to signal completion. + * @brief Sends and receives data over SPI using DMA, uses an optional operation flag to + * signal completion. */ template - bool transceive_DMA(span tx_data, span rx_data, volatile bool* operation_flag = nullptr) { + bool transceive_DMA( + span tx_data, + span rx_data, + volatile bool* operation_flag = nullptr + ) { spi_instance.operation_flag = operation_flag; auto size = std::min(tx_data.size_bytes(), rx_data.size_bytes()); - if (size % frame_size != 0) { - ErrorHandler("SPI transaction size (%d) not aligned to frame size (%d)", size, frame_size); + if (size % frame_size != 0) { + ErrorHandler( + "SPI transaction size (%d) not aligned to frame size (%d)", + size, + frame_size + ); return false; } - auto error_code = HAL_SPI_TransmitReceive_DMA(&spi_instance.hspi, (uint8_t*)tx_data.data(), (uint8_t*)rx_data.data(), size / frame_size); + auto error_code = HAL_SPI_TransmitReceive_DMA( + &spi_instance.hspi, + (uint8_t*)tx_data.data(), + (uint8_t*)rx_data.data(), + size / frame_size + ); return check_error_code(error_code); } /** - * @brief Sends a span and receives a trivially copyable type over SPI using DMA, uses an optional operation flag to signal completion. + * @brief Sends a span and receives a trivially copyable type over SPI using DMA, uses an + * optional operation flag to signal completion. */ template - bool transceive_DMA(span tx_data, T& rx_data, volatile bool* operation_flag = nullptr) requires std::is_trivially_copyable_v { + bool transceive_DMA(span tx_data, T& rx_data, volatile bool* operation_flag = nullptr) + requires std::is_trivially_copyable_v + { spi_instance.operation_flag = operation_flag; auto size = std::min(tx_data.size_bytes(), sizeof(T)); - if (size % frame_size != 0) { - ErrorHandler("SPI transaction size (%d) not aligned to frame size (%d)", size, frame_size); + if (size % frame_size != 0) { + ErrorHandler( + "SPI transaction size (%d) not aligned to frame size (%d)", + size, + frame_size + ); return false; } - auto error_code = HAL_SPI_TransmitReceive_DMA(&spi_instance.hspi, (uint8_t*)tx_data.data(), reinterpret_cast(&rx_data), size / frame_size); + auto error_code = HAL_SPI_TransmitReceive_DMA( + &spi_instance.hspi, + (uint8_t*)tx_data.data(), + reinterpret_cast(&rx_data), + size / frame_size + ); return check_error_code(error_code); } /** - * @brief Sends a trivially copyable type and receives a span over SPI using DMA, uses an optional operation flag to signal completion. + * @brief Sends a trivially copyable type and receives a span over SPI using DMA, uses an + * optional operation flag to signal completion. */ template - bool transceive_DMA(const T& tx_data, span rx_data, volatile bool* operation_flag = nullptr) requires std::is_trivially_copyable_v { + bool transceive_DMA( + const T& tx_data, + span rx_data, + volatile bool* operation_flag = nullptr + ) + requires std::is_trivially_copyable_v + { spi_instance.operation_flag = operation_flag; auto size = std::min(sizeof(T), rx_data.size_bytes()); - if (size % frame_size != 0) { - ErrorHandler("SPI transaction size (%d) not aligned to frame size (%d)", size, frame_size); + if (size % frame_size != 0) { + ErrorHandler( + "SPI transaction size (%d) not aligned to frame size (%d)", + size, + frame_size + ); return false; } - auto error_code = HAL_SPI_TransmitReceive_DMA(&spi_instance.hspi, reinterpret_cast(&tx_data), (uint8_t*)rx_data.data(), size / frame_size); + auto error_code = HAL_SPI_TransmitReceive_DMA( + &spi_instance.hspi, + reinterpret_cast(&tx_data), + (uint8_t*)rx_data.data(), + size / frame_size + ); return check_error_code(error_code); } /** - * @brief Sends and receives a trivially copyable data type over SPI using DMA, uses an optional operation flag to signal completion. + * @brief Sends and receives a trivially copyable data type over SPI using DMA, uses an + * optional operation flag to signal completion. */ template - bool transceive_DMA(const T1& tx_data, T2& rx_data, volatile bool* operation_flag = nullptr) requires (std::is_trivially_copyable_v && std::is_trivially_copyable_v) { + bool transceive_DMA(const T1& tx_data, T2& rx_data, volatile bool* operation_flag = nullptr) + requires(std::is_trivially_copyable_v && std::is_trivially_copyable_v) + { spi_instance.operation_flag = operation_flag; auto size = std::min(sizeof(T1), sizeof(T2)); - if (size % frame_size != 0) { - ErrorHandler("SPI transaction size (%d) not aligned to frame size (%d)", size, frame_size); + if (size % frame_size != 0) { + ErrorHandler( + "SPI transaction size (%d) not aligned to frame size (%d)", + size, + frame_size + ); return false; } - auto error_code = HAL_SPI_TransmitReceive_DMA(&spi_instance.hspi, reinterpret_cast(&tx_data), reinterpret_cast(&rx_data), size / frame_size); + auto error_code = HAL_SPI_TransmitReceive_DMA( + &spi_instance.hspi, + reinterpret_cast(&tx_data), + reinterpret_cast(&rx_data), + size / frame_size + ); return check_error_code(error_code); } - private: + private: Instance& spi_instance; bool check_error_code(HAL_StatusTypeDef error_code) { if (error_code == HAL_OK) { @@ -734,138 +962,231 @@ struct SPIDomain { /** * @brief SPI Wrapper for Slave mode operations. Doesn't allow for blocking operations. */ - template - struct SPIWrapper { - static constexpr uint32_t data_bits = static_cast(device_request.config.data_size); + template struct SPIWrapper { + static constexpr uint32_t data_bits = + static_cast(device_request.config.data_size); static constexpr uint32_t frame_size = (data_bits <= 8) ? 1 : ((data_bits <= 16) ? 2 : 4); - SPIWrapper(Instance &instance) : spi_instance{instance} {} + SPIWrapper(Instance& instance) : spi_instance{instance} {} - void set_software_nss(bool selected) requires (device_request.config.nss_mode == SPIConfigTypes::NSSMode::SOFTWARE) { + void set_software_nss(bool selected) + requires(device_request.config.nss_mode == SPIConfigTypes::NSSMode::SOFTWARE) + { if (selected) { - CLEAR_BIT(spi_instance.instance->CR1, SPI_CR1_SSI); + CLEAR_BIT(spi_instance.instance->CR1, SPI_CR1_SSI); } else { - SET_BIT(spi_instance.instance->CR1, SPI_CR1_SSI); + SET_BIT(spi_instance.instance->CR1, SPI_CR1_SSI); } } /** - * @brief Listens for data over SPI using DMA, uses an optional operation flag to signal completion. + * @brief Listens for data over SPI using DMA, uses an optional operation flag to signal + * completion. */ template bool listen(span data, volatile bool* operation_flag = nullptr) { spi_instance.operation_flag = operation_flag; if (data.size_bytes() % frame_size != 0) { - ErrorHandler("SPI data size (%d) not aligned to frame size (%d)", data.size_bytes(), frame_size); + ErrorHandler( + "SPI data size (%d) not aligned to frame size (%d)", + data.size_bytes(), + frame_size + ); return false; } - auto error_code = HAL_SPI_Receive_DMA(&spi_instance.hspi, (uint8_t*)data.data(), data.size_bytes() / frame_size); + auto error_code = HAL_SPI_Receive_DMA( + &spi_instance.hspi, + (uint8_t*)data.data(), + data.size_bytes() / frame_size + ); return check_error_code(error_code); } /** - * @brief Listens for trivially copyable data type over SPI using DMA, uses an optional operation flag to signal completion. + * @brief Listens for trivially copyable data type over SPI using DMA, uses an optional + * operation flag to signal completion. */ template - bool listen(T& data, volatile bool* operation_flag = nullptr) requires std::is_trivially_copyable_v { + bool listen(T& data, volatile bool* operation_flag = nullptr) + requires std::is_trivially_copyable_v + { spi_instance.operation_flag = operation_flag; if (sizeof(T) % frame_size != 0) { - ErrorHandler("SPI data size (%d) not aligned to frame size (%d)", sizeof(T), frame_size); + ErrorHandler( + "SPI data size (%d) not aligned to frame size (%d)", + sizeof(T), + frame_size + ); return false; } - auto error_code = HAL_SPI_Receive_DMA(&spi_instance.hspi, reinterpret_cast(&data), sizeof(T) / frame_size); + auto error_code = HAL_SPI_Receive_DMA( + &spi_instance.hspi, + reinterpret_cast(&data), + sizeof(T) / frame_size + ); return check_error_code(error_code); } /** - * @brief Arms the SPI to send data over DMA when requested, uses an optional operation flag to signal completion. + * @brief Arms the SPI to send data over DMA when requested, uses an optional operation flag + * to signal completion. */ template bool arm(span tx_data, volatile bool* operation_flag = nullptr) { spi_instance.operation_flag = operation_flag; if (tx_data.size_bytes() % frame_size != 0) { - ErrorHandler("SPI data size (%d) not aligned to frame size (%d)", tx_data.size_bytes(), frame_size); + ErrorHandler( + "SPI data size (%d) not aligned to frame size (%d)", + tx_data.size_bytes(), + frame_size + ); return false; } - auto error_code = HAL_SPI_Transmit_DMA(&spi_instance.hspi, (uint8_t*)tx_data.data(), tx_data.size_bytes() / frame_size); + auto error_code = HAL_SPI_Transmit_DMA( + &spi_instance.hspi, + (uint8_t*)tx_data.data(), + tx_data.size_bytes() / frame_size + ); return check_error_code(error_code); } /** - * @brief Arms the SPI to send a trivially copyable data type over DMA when requested, uses an optional operation flag to signal completion. + * @brief Arms the SPI to send a trivially copyable data type over DMA when requested, uses + * an optional operation flag to signal completion. */ template - bool arm(const T& data, volatile bool* operation_flag = nullptr) requires std::is_trivially_copyable_v { + bool arm(const T& data, volatile bool* operation_flag = nullptr) + requires std::is_trivially_copyable_v + { spi_instance.operation_flag = operation_flag; if (sizeof(T) % frame_size != 0) { - ErrorHandler("SPI data size (%d) not aligned to frame size (%d)", sizeof(T), frame_size); + ErrorHandler( + "SPI data size (%d) not aligned to frame size (%d)", + sizeof(T), + frame_size + ); return false; } - auto error_code = HAL_SPI_Transmit_DMA(&spi_instance.hspi, reinterpret_cast(&data), sizeof(T) / frame_size); + auto error_code = HAL_SPI_Transmit_DMA( + &spi_instance.hspi, + reinterpret_cast(&data), + sizeof(T) / frame_size + ); return check_error_code(error_code); } /** - * @brief Sends and receives data over SPI using DMA, uses an optional operation flag to signal completion. + * @brief Sends and receives data over SPI using DMA, uses an optional operation flag to + * signal completion. */ template - bool transceive(span tx_data, span rx_data, volatile bool* operation_flag = nullptr) { + bool transceive( + span tx_data, + span rx_data, + volatile bool* operation_flag = nullptr + ) { spi_instance.operation_flag = operation_flag; auto size = std::min(tx_data.size_bytes(), rx_data.size_bytes()); if (size % frame_size != 0) { - ErrorHandler("SPI transaction size (%d) not aligned to frame size (%d)", size, frame_size); + ErrorHandler( + "SPI transaction size (%d) not aligned to frame size (%d)", + size, + frame_size + ); return false; } - auto error_code = HAL_SPI_TransmitReceive_DMA(&spi_instance.hspi, (uint8_t*)tx_data.data(), (uint8_t*)rx_data.data(), size / frame_size); + auto error_code = HAL_SPI_TransmitReceive_DMA( + &spi_instance.hspi, + (uint8_t*)tx_data.data(), + (uint8_t*)rx_data.data(), + size / frame_size + ); return check_error_code(error_code); } /** - * @brief Sends a span and receives a trivially copyable type over SPI using DMA, uses an optional operation flag to signal completion. + * @brief Sends a span and receives a trivially copyable type over SPI using DMA, uses an + * optional operation flag to signal completion. */ template - bool transceive(span tx_data, T& rx_data, volatile bool* operation_flag = nullptr) requires std::is_trivially_copyable_v { + bool transceive(span tx_data, T& rx_data, volatile bool* operation_flag = nullptr) + requires std::is_trivially_copyable_v + { spi_instance.operation_flag = operation_flag; auto size = std::min(tx_data.size_bytes(), sizeof(T)); - if (size % frame_size != 0) { - ErrorHandler("SPI transaction size (%d) not aligned to frame size (%d)", size, frame_size); + if (size % frame_size != 0) { + ErrorHandler( + "SPI transaction size (%d) not aligned to frame size (%d)", + size, + frame_size + ); return false; } - auto error_code = HAL_SPI_TransmitReceive_DMA(&spi_instance.hspi, (uint8_t*)tx_data.data(), reinterpret_cast(&rx_data), size / frame_size); + auto error_code = HAL_SPI_TransmitReceive_DMA( + &spi_instance.hspi, + (uint8_t*)tx_data.data(), + reinterpret_cast(&rx_data), + size / frame_size + ); return check_error_code(error_code); } /** - * @brief Sends a trivially copyable type and receives a span over SPI using DMA, uses an optional operation flag to signal completion. + * @brief Sends a trivially copyable type and receives a span over SPI using DMA, uses an + * optional operation flag to signal completion. */ template - bool transceive(const T& tx_data, span rx_data, volatile bool* operation_flag = nullptr) requires std::is_trivially_copyable_v { + bool + transceive(const T& tx_data, span rx_data, volatile bool* operation_flag = nullptr) + requires std::is_trivially_copyable_v + { spi_instance.operation_flag = operation_flag; auto size = std::min(sizeof(T), rx_data.size_bytes()); - if (size % frame_size != 0) { - ErrorHandler("SPI transaction size (%d) not aligned to frame size (%d)", size, frame_size); + if (size % frame_size != 0) { + ErrorHandler( + "SPI transaction size (%d) not aligned to frame size (%d)", + size, + frame_size + ); return false; } - auto error_code = HAL_SPI_TransmitReceive_DMA(&spi_instance.hspi, reinterpret_cast(&tx_data), (uint8_t*)rx_data.data(), size / frame_size); + auto error_code = HAL_SPI_TransmitReceive_DMA( + &spi_instance.hspi, + reinterpret_cast(&tx_data), + (uint8_t*)rx_data.data(), + size / frame_size + ); return check_error_code(error_code); } /** - * @brief Sends and receives a trivially copyable data type over SPI using DMA, uses an optional operation flag to signal completion. + * @brief Sends and receives a trivially copyable data type over SPI using DMA, uses an + * optional operation flag to signal completion. */ template - bool transceive(const T1& tx_data, T2& rx_data, volatile bool* operation_flag = nullptr) requires (std::is_trivially_copyable_v && std::is_trivially_copyable_v) { + bool transceive(const T1& tx_data, T2& rx_data, volatile bool* operation_flag = nullptr) + requires(std::is_trivially_copyable_v && std::is_trivially_copyable_v) + { spi_instance.operation_flag = operation_flag; auto size = std::min(sizeof(T1), sizeof(T2)); - if (size % frame_size != 0) { - ErrorHandler("SPI transaction size (%d) not aligned to frame size (%d)", size, frame_size); + if (size % frame_size != 0) { + ErrorHandler( + "SPI transaction size (%d) not aligned to frame size (%d)", + size, + frame_size + ); return false; } - auto error_code = HAL_SPI_TransmitReceive_DMA(&spi_instance.hspi, reinterpret_cast(&tx_data), reinterpret_cast(&rx_data), size / frame_size); + auto error_code = HAL_SPI_TransmitReceive_DMA( + &spi_instance.hspi, + reinterpret_cast(&tx_data), + reinterpret_cast(&rx_data), + size / frame_size + ); return check_error_code(error_code); } - private: + private: Instance& spi_instance; bool check_error_code(HAL_StatusTypeDef error_code) { if (error_code == HAL_OK) { @@ -879,14 +1200,12 @@ struct SPIDomain { } }; - -/** - * ========================================= - * Internal working things - * ========================================= - */ - template - static consteval array build(span entries) { + /** + * ========================================= + * Internal working things + * ========================================= + */ + template static consteval array build(span entries) { array cfgs{}; if (N == 0) { @@ -945,15 +1264,16 @@ struct SPIDomain { return cfgs; } - template - struct Init { + template struct Init { static inline std::array instances{}; - static void init(std::span cfgs, - std::span gpio_instances, - std::span dma_peripherals) { + static void init( + std::span cfgs, + std::span gpio_instances, + std::span dma_peripherals + ) { for (std::size_t i = 0; i < N; ++i) { - const auto &e = cfgs[i]; + const auto& e = cfgs[i]; SPIPeripheral peripheral = e.peripheral; instances[i].instance = reinterpret_cast(e.peripheral); @@ -1045,12 +1365,10 @@ struct SPIDomain { init.Mode = SPI_MODE_MASTER; // Baudrate prescaler calculation uint32_t pclk_freq; - if (peripheral == SPIPeripheral::spi1 || - peripheral == SPIPeripheral::spi2 || + if (peripheral == SPIPeripheral::spi1 || peripheral == SPIPeripheral::spi2 || peripheral == SPIPeripheral::spi3) { pclk_freq = HAL_RCCEx_GetPeriphCLKFreq(RCC_PERIPHCLK_SPI123); - } else if (peripheral == SPIPeripheral::spi4 || - peripheral == SPIPeripheral::spi5) { + } else if (peripheral == SPIPeripheral::spi4 || peripheral == SPIPeripheral::spi5) { pclk_freq = HAL_RCCEx_GetPeriphCLKFreq(RCC_PERIPHCLK_SPI45); } else { pclk_freq = HAL_RCCEx_GetPeriphCLKFreq(RCC_PERIPHCLK_SPI6); @@ -1059,28 +1377,38 @@ struct SPIDomain { } else { init.Mode = SPI_MODE_SLAVE; } - - init.NSS = SPIConfigTypes::translate_nss_mode(e.config.nss_mode, e.mode == SPIMode::MASTER); + + init.NSS = SPIConfigTypes::translate_nss_mode( + e.config.nss_mode, + e.mode == SPIMode::MASTER + ); init.Direction = SPIConfigTypes::translate_direction(e.config.direction); init.DataSize = SPIConfigTypes::translate_data_size(e.config.data_size); init.CLKPolarity = SPIConfigTypes::translate_clock_polarity(e.config.polarity); init.CLKPhase = SPIConfigTypes::translate_clock_phase(e.config.phase); init.FirstBit = SPIConfigTypes::translate_bit_order(e.config.bit_order); init.TIMode = SPIConfigTypes::translate_ti_mode(e.config.ti_mode); - init.CRCCalculation = SPIConfigTypes::translate_crc_calculation(e.config.crc_calculation); + init.CRCCalculation = + SPIConfigTypes::translate_crc_calculation(e.config.crc_calculation); if (e.config.crc_calculation) { init.CRCPolynomial = e.config.crc_polynomial; init.CRCLength = SPIConfigTypes::translate_crc_length(e.config.crc_length); } init.NSSPMode = SPIConfigTypes::translate_nss_pulse(e.config.nss_pulse); init.NSSPolarity = SPIConfigTypes::translate_nss_polarity(e.config.nss_polarity); - init.FifoThreshold = SPIConfigTypes::translate_fifo_threshold(e.config.fifo_threshold); + init.FifoThreshold = + SPIConfigTypes::translate_fifo_threshold(e.config.fifo_threshold); init.TxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN; init.RxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN; - init.MasterSSIdleness = SPIConfigTypes::translate_ss_idleness(e.config.master_ss_idleness); - init.MasterInterDataIdleness = SPIConfigTypes::translate_interdata_idleness(e.config.master_interdata_idleness); - init.MasterReceiverAutoSusp = SPIConfigTypes::translate_rx_autosusp(e.config.master_rx_autosusp); - init.MasterKeepIOState = SPIConfigTypes::translate_keep_io_state(e.config.keep_io_state); + init.MasterSSIdleness = + SPIConfigTypes::translate_ss_idleness(e.config.master_ss_idleness); + init.MasterInterDataIdleness = + SPIConfigTypes::translate_interdata_idleness(e.config.master_interdata_idleness + ); + init.MasterReceiverAutoSusp = + SPIConfigTypes::translate_rx_autosusp(e.config.master_rx_autosusp); + init.MasterKeepIOState = + SPIConfigTypes::translate_keep_io_state(e.config.keep_io_state); init.IOSwap = SPIConfigTypes::translate_io_swap(e.config.io_swap); if (HAL_SPI_Init(&hspi) != HAL_OK) { @@ -1130,5 +1458,4 @@ struct SPIDomain { } // namespace ST_LIB - #endif // SPI2_HPP \ No newline at end of file diff --git a/Inc/HALAL/Models/SPI/SPIConfig.hpp b/Inc/HALAL/Models/SPI/SPIConfig.hpp index 0215de56d..918363b21 100644 --- a/Inc/HALAL/Models/SPI/SPIConfig.hpp +++ b/Inc/HALAL/Models/SPI/SPIConfig.hpp @@ -14,25 +14,22 @@ namespace ST_LIB { struct SPIConfigTypes { - + enum class ClockPolarity : bool { - LOW = false, // Clock idle state is low (CPOL=0) - HIGH = true // Clock idle state is high (CPOL=1) + LOW = false, // Clock idle state is low (CPOL=0) + HIGH = true // Clock idle state is high (CPOL=1) }; enum class ClockPhase : bool { - FIRST_EDGE = false, // Data sampled on first clock edge (CPHA=0) - SECOND_EDGE = true // Data sampled on second clock edge (CPHA=1) + FIRST_EDGE = false, // Data sampled on first clock edge (CPHA=0) + SECOND_EDGE = true // Data sampled on second clock edge (CPHA=1) }; - enum class BitOrder : bool { - MSB_FIRST = false, - LSB_FIRST = true - }; + enum class BitOrder : bool { MSB_FIRST = false, LSB_FIRST = true }; enum class NSSMode { - SOFTWARE, // Software NSS management (manual control) - HARDWARE // Hardware NSS management (automatic) + SOFTWARE, // Software NSS management (manual control) + HARDWARE // Hardware NSS management (automatic) }; enum class DataSize : uint8_t { @@ -68,10 +65,10 @@ struct SPIConfigTypes { }; enum class Direction { - FULL_DUPLEX, // 2-line bidirectional (most common) - HALF_DUPLEX, // 1-line bidirectional - SIMPLEX_TX_ONLY, // Transmit only - SIMPLEX_RX_ONLY // Receive only + FULL_DUPLEX, // 2-line bidirectional (most common) + HALF_DUPLEX, // 1-line bidirectional + SIMPLEX_TX_ONLY, // Transmit only + SIMPLEX_RX_ONLY // Receive only }; enum class FIFOThreshold : uint8_t { @@ -93,13 +90,10 @@ struct SPIConfigTypes { THRESHOLD_16DATA = 16 }; - enum class NSSPolarity { - ACTIVE_LOW = false, - ACTIVE_HIGH = true - }; + enum class NSSPolarity { ACTIVE_LOW = false, ACTIVE_HIGH = true }; enum class CRCLength : uint8_t { - DATASIZE = 0, // CRC length matches data size + DATASIZE = 0, // CRC length matches data size CRC_4BIT = 4, CRC_5BIT = 5, CRC_6BIT = 6, @@ -140,40 +134,43 @@ struct SPIConfigTypes { ClockPhase phase = ClockPhase::FIRST_EDGE; BitOrder bit_order = BitOrder::MSB_FIRST; NSSMode nss_mode = NSSMode::HARDWARE; - + // Data format DataSize data_size = DataSize::SIZE_8BIT; Direction direction = Direction::FULL_DUPLEX; FIFOThreshold fifo_threshold = FIFOThreshold::THRESHOLD_01DATA; - + // NSS settings - bool nss_pulse = false; // NSS pulse between data frames (master only) + bool nss_pulse = false; // NSS pulse between data frames (master only) NSSPolarity nss_polarity = NSSPolarity::ACTIVE_LOW; - + // Master timing settings - uint8_t master_ss_idleness = 0; // Cycles (0-15) between NSS and first data - uint8_t master_interdata_idleness = 0; // Cycles (0-15) between data frames - + uint8_t master_ss_idleness = 0; // Cycles (0-15) between NSS and first data + uint8_t master_interdata_idleness = 0; // Cycles (0-15) between data frames + // Advanced options - bool keep_io_state = true; // Keep pin states when idle (prevents floating) - bool master_rx_autosusp = false; // Auto-suspend in master RX mode to prevent overrun - bool io_swap = false; // Swap MISO/MOSI pins - + bool keep_io_state = true; // Keep pin states when idle (prevents floating) + bool master_rx_autosusp = false; // Auto-suspend in master RX mode to prevent overrun + bool io_swap = false; // Swap MISO/MOSI pins + // Protocol options - bool ti_mode = false; // Enable TI synchronous serial frame format (Microwire compatible) - + bool ti_mode = false; // Enable TI synchronous serial frame format (Microwire compatible) + // CRC options (hardware CRC calculation) - bool crc_calculation = false; // Enable hardware CRC calculation - uint32_t crc_polynomial = 0x07; // CRC polynomial (must be odd, default for CRC-8) - CRCLength crc_length = CRCLength::DATASIZE; // CRC length (default: matches data size) - + bool crc_calculation = false; // Enable hardware CRC calculation + uint32_t crc_polynomial = 0x07; // CRC polynomial (must be odd, default for CRC-8) + CRCLength crc_length = CRCLength::DATASIZE; // CRC length (default: matches data size) + constexpr SPIConfig() = default; - - constexpr SPIConfig(ClockPolarity pol, ClockPhase ph, - BitOrder order = BitOrder::MSB_FIRST, - NSSMode nss = NSSMode::HARDWARE) + + constexpr SPIConfig( + ClockPolarity pol, + ClockPhase ph, + BitOrder order = BitOrder::MSB_FIRST, + NSSMode nss = NSSMode::HARDWARE + ) : polarity(pol), phase(ph), bit_order(order), nss_mode(nss) {} - + // Compile-time validation constexpr void validate() const { // Validate CRC polynomial if CRC is enabled @@ -185,7 +182,7 @@ struct SPIConfigTypes { compile_error("CRC polynomial must be odd"); } } - + // Validate timing parameters if (master_ss_idleness > 15) { compile_error("master_ss_idleness must be 0-15"); @@ -224,14 +221,14 @@ struct SPIConfigTypes { static constexpr uint32_t translate_direction(Direction dir) { switch (dir) { - case Direction::FULL_DUPLEX: - return SPI_DIRECTION_2LINES; - case Direction::HALF_DUPLEX: - return SPI_DIRECTION_1LINE; - case Direction::SIMPLEX_TX_ONLY: - return SPI_DIRECTION_2LINES_TXONLY; - case Direction::SIMPLEX_RX_ONLY: - return SPI_DIRECTION_2LINES_RXONLY; + case Direction::FULL_DUPLEX: + return SPI_DIRECTION_2LINES; + case Direction::HALF_DUPLEX: + return SPI_DIRECTION_1LINE; + case Direction::SIMPLEX_TX_ONLY: + return SPI_DIRECTION_2LINES_TXONLY; + case Direction::SIMPLEX_RX_ONLY: + return SPI_DIRECTION_2LINES_RXONLY; } return SPI_DIRECTION_2LINES; } @@ -251,14 +248,16 @@ struct SPIConfigTypes { static constexpr uint32_t translate_ss_idleness(uint8_t cycles) { // Clamp to valid range 0-15 - if (cycles > 15) cycles = 15; + if (cycles > 15) + cycles = 15; return cycles; } static constexpr uint32_t translate_interdata_idleness(uint8_t cycles) { // Clamp to valid range 0-15, shift to proper position - if (cycles > 15) cycles = 15; - return (cycles << 4); // These are in bits 7:4 of CFG2 + if (cycles > 15) + cycles = 15; + return (cycles << 4); // These are in bits 7:4 of CFG2 } static constexpr uint32_t translate_keep_io_state(bool enable) { diff --git a/Inc/HALAL/Models/TimerChannelModel/TimerChannel.hpp b/Inc/HALAL/Models/TimerChannelModel/TimerChannel.hpp index 864f4fcda..2fd1d107d 100644 --- a/Inc/HALAL/Models/TimerChannelModel/TimerChannel.hpp +++ b/Inc/HALAL/Models/TimerChannelModel/TimerChannel.hpp @@ -2,23 +2,26 @@ * TimerChannel.hpp * * Created on: Nov 1, 2022 - * Author: alejandro + * Author: alejandro */ #pragma once #ifdef HAL_TIM_MODULE_ENABLED struct TimerChannel { - TIM_HandleTypeDef* timer; - uint32_t channel; + TIM_HandleTypeDef* timer; + uint32_t channel; - bool operator== (const TimerChannel &other) const { - return (timer == other.timer && channel == other.channel); - } - - bool operator< (const TimerChannel &other) const { - if (timer == other.timer) { return channel < other.channel; } - else { return timer < other.timer; } - } + bool operator==(const TimerChannel& other) const { + return (timer == other.timer && channel == other.channel); + } + + bool operator<(const TimerChannel& other) const { + if (timer == other.timer) { + return channel < other.channel; + } else { + return timer < other.timer; + } + } }; #endif diff --git a/Inc/HALAL/Models/TimerDomain/TimerDomain.hpp b/Inc/HALAL/Models/TimerDomain/TimerDomain.hpp index 4ad500704..bd28923d4 100644 --- a/Inc/HALAL/Models/TimerDomain/TimerDomain.hpp +++ b/Inc/HALAL/Models/TimerDomain/TimerDomain.hpp @@ -7,7 +7,7 @@ #pragma once -#include "hal_wrapper.h" +#include "hal_wrapper.h" #ifdef HAL_TIM_MODULE_ENABLED @@ -20,26 +20,26 @@ #include "ErrorHandler/ErrorHandler.hpp" // NOTE: only works for static arrays -#define ARRAY_LENGTH(a) (sizeof(a)/sizeof(*a)) - -#define TimerXList \ - X(2, APB1LENR) \ - X(3, APB1LENR) \ - X(4, APB1LENR) \ - X(5, APB1LENR) \ - X(6, APB1LENR) \ - X(7, APB1LENR) \ - X(12, APB1LENR) \ - X(13, APB1LENR) \ - X(14, APB1LENR) \ - \ - X(23, APB1HENR) \ - X(24, APB1HENR) \ - \ - X(1, APB2ENR) \ - X(8, APB2ENR) \ - X(15, APB2ENR) \ - X(16, APB2ENR) \ +#define ARRAY_LENGTH(a) (sizeof(a) / sizeof(*a)) + +#define TimerXList \ + X(2, APB1LENR) \ + X(3, APB1LENR) \ + X(4, APB1LENR) \ + X(5, APB1LENR) \ + X(6, APB1LENR) \ + X(7, APB1LENR) \ + X(12, APB1LENR) \ + X(13, APB1LENR) \ + X(14, APB1LENR) \ + \ + X(23, APB1HENR) \ + X(24, APB1HENR) \ + \ + X(1, APB2ENR) \ + X(8, APB2ENR) \ + X(15, APB2ENR) \ + X(16, APB2ENR) \ X(17, APB2ENR) #define X(n, b) extern TIM_HandleTypeDef htim##n; @@ -47,862 +47,971 @@ TimerXList #undef X #if !defined(glue) -#define glue_(a,b) a##b -#define glue(a,b) glue_(a,b) +#define glue_(a, b) a##b +#define glue(a, b) glue_(a, b) #endif // !defined(glue) -/* Tim1 & Tim8 are advanced-control timers - * their ARR & prescaler are 16bit - * they have up to 6 independent channels for: - * - input capture (not channel 5 or 6) - * - output capture - * - PWM generation - * - One-pulse mode output - */ - -/* Timers {TIM2, TIM5, TIM23, TIM24} are the only 32-bit counter resolution timers, the rest are 16-bit */ -/* Timers 2, 3, 4, 5, 23, 24 are general-purpose timers - * Timers 12, 13, 14 are also general-purpose timers (but separate in the ref manual) - * Timers 15, 16, 17 are also general purpose timers (but separate in the ref manual) - */ + /* Tim1 & Tim8 are advanced-control timers + * their ARR & prescaler are 16bit + * they have up to 6 independent channels for: + * - input capture (not channel 5 or 6) + * - output capture + * - PWM generation + * - One-pulse mode output + */ + + /* Timers {TIM2, TIM5, TIM23, TIM24} are the only 32-bit counter resolution timers, the rest are + 16-bit */ + /* Timers 2, 3, 4, 5, 23, 24 are general-purpose timers + * Timers 12, 13, 14 are also general-purpose timers (but separate in the ref manual) + * Timers 15, 16, 17 are also general purpose timers (but separate in the ref manual) + */ + + /* Tim6 & Tim7 are basic timers. Features: + - 16-bit ARR upcounter + - 16-bit PSC + - Synchronization circuit to trigger the DAC + - Interrupt/DMA generation on the update event + */ + + /* advanced timer features: + - 16-bit ARR up/down counter + - 16-bit PSC + - Up to 6 independent channels for: + · Input capture (all channels but 5 and 6) + · Output compare + · PWM generation (Edge and Center aligned mode) + · One-pulse mode output + - Complementary outputs with programmable dead-time + - Synchronization circuit to control the timer with + external signals and to interconnect several timers together. + - Repetition counter to update the timer registers only after + a given number of cycles of the counter. + - 2 break inputs to put the timer’s output signals in a safe user selectable configuration. + - Interrupt/DMA generation on the following events: + · Update: counter overflow/underflow, counter initialization (by software or + internal/external trigger) · Trigger event (counter start, stop, initialization or count by + internal/external trigger) · Input capture · Output compare + - Supports incremental (quadrature) encoder and Hall-sensor circuitry for positioning purposes + - Trigger input for external clock or cycle-by-cycle current management + */ + namespace ST_LIB { + extern void compile_error(const char* msg); + + /* The number corresponds with the timer nº */ + enum TimerRequest : uint8_t { + AnyGeneralPurpose = 0, + Any32bit = 0xFF, + + Advanced_1 = 1, + Advanced_8 = 8, + + GeneralPurpose32bit_2 = 2, + GeneralPurpose32bit_5 = 5, + GeneralPurpose32bit_23 = 23, + GeneralPurpose32bit_24 = 24, + + GeneralPurpose_3 = 3, + GeneralPurpose_4 = 4, + + SlaveTimer_12 = 12, + SlaveTimer_13 = 13, + SlaveTimer_14 = 14, + + GeneralPurpose_15 = 15, + GeneralPurpose_16 = 16, + GeneralPurpose_17 = 17, + + Basic_6 = 6, + Basic_7 = 7, + }; -/* Tim6 & Tim7 are basic timers. Features: - - 16-bit ARR upcounter - - 16-bit PSC - - Synchronization circuit to trigger the DAC - - Interrupt/DMA generation on the update event -*/ + // Alternate functions for timers + enum class TimerAF { + None, + PWM, + Encoder, + InputCapture, + BreakInput, + BreakInputCompare, + }; -/* advanced timer features: - - 16-bit ARR up/down counter - - 16-bit PSC - - Up to 6 independent channels for: - · Input capture (all channels but 5 and 6) - · Output compare - · PWM generation (Edge and Center aligned mode) - · One-pulse mode output - - Complementary outputs with programmable dead-time - - Synchronization circuit to control the timer with - external signals and to interconnect several timers together. - - Repetition counter to update the timer registers only after - a given number of cycles of the counter. - - 2 break inputs to put the timer’s output signals in a safe user selectable configuration. - - Interrupt/DMA generation on the following events: - · Update: counter overflow/underflow, counter initialization (by software or internal/external trigger) - · Trigger event (counter start, stop, initialization or count by internal/external trigger) - · Input capture - · Output compare - - Supports incremental (quadrature) encoder and Hall-sensor circuitry for positioning purposes - - Trigger input for external clock or cycle-by-cycle current management -*/ -namespace ST_LIB { -extern void compile_error(const char *msg); - -/* The number corresponds with the timer nº */ -enum TimerRequest : uint8_t { - AnyGeneralPurpose = 0, - Any32bit = 0xFF, - - Advanced_1 = 1, - Advanced_8 = 8, - - GeneralPurpose32bit_2 = 2, - GeneralPurpose32bit_5 = 5, - GeneralPurpose32bit_23 = 23, - GeneralPurpose32bit_24 = 24, - - GeneralPurpose_3 = 3, - GeneralPurpose_4 = 4, - - SlaveTimer_12 = 12, - SlaveTimer_13 = 13, - SlaveTimer_14 = 14, - - GeneralPurpose_15 = 15, - GeneralPurpose_16 = 16, - GeneralPurpose_17 = 17, - - Basic_6 = 6, - Basic_7 = 7, -}; - -// Alternate functions for timers -enum class TimerAF { - None, - PWM, - Encoder, - InputCapture, - BreakInput, - BreakInputCompare, -}; - -enum class TimerChannel : uint8_t { - CHANNEL_1 = 1, - CHANNEL_2 = 2, - CHANNEL_3 = 3, - CHANNEL_4 = 4, - CHANNEL_5 = 5, - CHANNEL_6 = 6, - - CHANNEL_NEGATED_FLAG = 8, - - CHANNEL_1_NEGATED = 1 | CHANNEL_NEGATED_FLAG, - CHANNEL_2_NEGATED = 2 | CHANNEL_NEGATED_FLAG, - CHANNEL_3_NEGATED = 3 | CHANNEL_NEGATED_FLAG, -}; - -struct TimerPin { - ST_LIB::TimerAF af; - ST_LIB::GPIODomain::Pin pin; - ST_LIB::TimerChannel channel; -}; + enum class TimerChannel : uint8_t { + CHANNEL_1 = 1, + CHANNEL_2 = 2, + CHANNEL_3 = 3, + CHANNEL_4 = 4, + CHANNEL_5 = 5, + CHANNEL_6 = 6, -#ifndef DEFAULT_PWM_FREQUENCY_MODE -#define DEFAULT_PWM_FREQUENCY_MODE ST_LIB::PWM_Frequency_Mode::PRECISION -#endif + CHANNEL_NEGATED_FLAG = 8, -enum class PWM_Frequency_Mode { - PRECISION, - SPEED, -}; - -constexpr std::array create_timer_idxmap() { - std::array result{}; - - // invalid timers that don't exist - result[0] = -1; - result[9] = -1; result[10] = -1; result[11] = -1; - result[18] = -1; result[19] = -1; result[20] = -1; result[21] = -1; result[22] = -1; - - // general-purpose timers - result[2] = 0; result[3] = 1; result[4] = 2; - result[5] = 3; result[23] = 4; result[24] = 5; - - // more general-purpose timers - result[12] = 6; result[13] = 7; result[14] = 8; - - // more general-purpose timers - result[15] = 9; result[16] = 10; result[17] = 11; - - // basic timers - result[6] = 12; result[7] = 13; - - // advanced control timers - result[1] = 14; result[8] = 15; - - return result; -} - -static constexpr std::array timer_idxmap = create_timer_idxmap(); - -struct TimerDomain { - // There are 16 timers - static constexpr std::size_t max_instances = 16; - - struct Entry { - std::array name; /* max length = 7 */ - TimerRequest request; - uint8_t pin_count; - std::array pins; /* this won't be read in Timer constructor */ + CHANNEL_1_NEGATED = 1 | CHANNEL_NEGATED_FLAG, + CHANNEL_2_NEGATED = 2 | CHANNEL_NEGATED_FLAG, + CHANNEL_3_NEGATED = 3 | CHANNEL_NEGATED_FLAG, }; - struct Config { - uint8_t timer_idx; + struct TimerPin { + ST_LIB::TimerAF af; + ST_LIB::GPIODomain::Pin pin; + ST_LIB::TimerChannel channel; }; - static constexpr TIM_HandleTypeDef *hal_handles[16] = { - // general purpose timers - &htim2, &htim3, &htim4, &htim5, &htim23, &htim24, - &htim12, &htim13, &htim14, - &htim15, &htim16, &htim17, - - // basic timers - &htim6, &htim7, +#ifndef DEFAULT_PWM_FREQUENCY_MODE +#define DEFAULT_PWM_FREQUENCY_MODE ST_LIB::PWM_Frequency_Mode::PRECISION +#endif - // advanced control timers - &htim1, &htim8 + enum class PWM_Frequency_Mode { + PRECISION, + SPEED, }; -#ifdef SIM_ON - static TIM_TypeDef *cmsis_timers[16]; -#else - static constexpr TIM_TypeDef *cmsis_timers[16] = { - // general purpose timers - TIM2, TIM3, TIM4, TIM5, TIM23, TIM24, - TIM12, TIM13, TIM14, - TIM15, TIM16, TIM17, + constexpr std::array create_timer_idxmap() { + std::array result{}; + + // invalid timers that don't exist + result[0] = -1; + result[9] = -1; + result[10] = -1; + result[11] = -1; + result[18] = -1; + result[19] = -1; + result[20] = -1; + result[21] = -1; + result[22] = -1; + + // general-purpose timers + result[2] = 0; + result[3] = 1; + result[4] = 2; + result[5] = 3; + result[23] = 4; + result[24] = 5; + + // more general-purpose timers + result[12] = 6; + result[13] = 7; + result[14] = 8; + + // more general-purpose timers + result[15] = 9; + result[16] = 10; + result[17] = 11; // basic timers - TIM6, TIM7, + result[6] = 12; + result[7] = 13; // advanced control timers - TIM1, TIM8 - }; -#endif + result[1] = 14; + result[8] = 15; - static constexpr IRQn_Type timer_irqn[] = { - // general purpose timers 1 - TIM2_IRQn, TIM3_IRQn, TIM4_IRQn, TIM5_IRQn, TIM23_IRQn, TIM24_IRQn, - // slave timers - TIM8_BRK_TIM12_IRQn, TIM8_UP_TIM13_IRQn, TIM8_TRG_COM_TIM14_IRQn, - // general purpose timers 2 - TIM15_IRQn, TIM16_IRQn, TIM17_IRQn, + return result; + } - // basic timers - TIM6_DAC_IRQn, /* TIM6 global and DAC1&2 underrun error interrupts */ - TIM7_IRQn, + static constexpr std::array timer_idxmap = create_timer_idxmap(); - TIM1_UP_IRQn, TIM8_UP_TIM13_IRQn - }; + struct TimerDomain { + // There are 16 timers + static constexpr std::size_t max_instances = 16; - static inline void rcc_enable_timer(TIM_TypeDef *tim) { -#define X(n, b) \ - else if(tim == TIM##n) { SET_BIT(RCC->b, RCC_##b##_TIM##n##EN); } + struct Entry { + std::array name; /* max length = 7 */ + TimerRequest request; + uint8_t pin_count; + std::array pins; /* this won't be read in Timer constructor */ + }; - if(false) {} - TimerXList - else { - ErrorHandler("Invalid timer given to rcc_enable_timer"); - } -#undef X - } + struct Config { + uint8_t timer_idx; + }; - static constexpr std::array EMPTY_TIMER_NAME = {0,0,0,0, 0,0,0,0}; + static constexpr TIM_HandleTypeDef* hal_handles[16] = {// general purpose timers + &htim2, + &htim3, + &htim4, + &htim5, + &htim23, + &htim24, + &htim12, + &htim13, + &htim14, + &htim15, + &htim16, + &htim17, + + // basic timers + &htim6, + &htim7, + + // advanced control timers + &htim1, + &htim8 + }; - struct Timer { - using domain = TimerDomain; - //GPIODomain::GPIO gpios[4]; - Entry e; - GPIODomain::GPIO gpio0; - GPIODomain::GPIO gpio1; - GPIODomain::GPIO gpio2; - GPIODomain::GPIO gpio3; - GPIODomain::GPIO gpio4; - GPIODomain::GPIO gpio5; - GPIODomain::GPIO gpio6; +#ifdef SIM_ON + static TIM_TypeDef* cmsis_timers[16]; +#else + static constexpr TIM_TypeDef* cmsis_timers[16] = {// general purpose timers + TIM2, + TIM3, + TIM4, + TIM5, + TIM23, + TIM24, + TIM12, + TIM13, + TIM14, + TIM15, + TIM16, + TIM17, + + // basic timers + TIM6, + TIM7, + + // advanced control timers + TIM1, + TIM8 + }; +#endif - static consteval GPIODomain::AlternateFunction get_gpio_af(ST_LIB::TimerRequest req, ST_LIB::TimerPin pin); + static constexpr IRQn_Type timer_irqn[] = { + // general purpose timers 1 + TIM2_IRQn, + TIM3_IRQn, + TIM4_IRQn, + TIM5_IRQn, + TIM23_IRQn, + TIM24_IRQn, + // slave timers + TIM8_BRK_TIM12_IRQn, + TIM8_UP_TIM13_IRQn, + TIM8_TRG_COM_TIM14_IRQn, + // general purpose timers 2 + TIM15_IRQn, + TIM16_IRQn, + TIM17_IRQn, + + // basic timers + TIM6_DAC_IRQn, /* TIM6 global and DAC1&2 underrun error interrupts */ + TIM7_IRQn, + + TIM1_UP_IRQn, + TIM8_UP_TIM13_IRQn + }; - static consteval GPIODomain::OperationMode get_operation_mode(ST_LIB::TimerAF af) - { - switch(af) { - case TimerAF::None: return GPIODomain::OperationMode::INPUT; + static inline void rcc_enable_timer(TIM_TypeDef* tim) { +#define X(n, b) \ + else if (tim == TIM##n) { \ + SET_BIT(RCC->b, RCC_##b##_TIM##n##EN); \ + } - case TimerAF::PWM: return GPIODomain::OperationMode::ALT_PP; - case TimerAF::Encoder: return GPIODomain::OperationMode::ALT_PP; + if (false) { + } + TimerXList else { + ErrorHandler("Invalid timer given to rcc_enable_timer"); + } +#undef X + } + + static constexpr std::array EMPTY_TIMER_NAME = {0, 0, 0, 0, 0, 0, 0, 0}; + + struct Timer { + using domain = TimerDomain; + // GPIODomain::GPIO gpios[4]; + Entry e; + GPIODomain::GPIO gpio0; + GPIODomain::GPIO gpio1; + GPIODomain::GPIO gpio2; + GPIODomain::GPIO gpio3; + GPIODomain::GPIO gpio4; + GPIODomain::GPIO gpio5; + GPIODomain::GPIO gpio6; + + static consteval GPIODomain::AlternateFunction + get_gpio_af(ST_LIB::TimerRequest req, ST_LIB::TimerPin pin); + + static consteval GPIODomain::OperationMode get_operation_mode(ST_LIB::TimerAF af) { + switch (af) { + case TimerAF::None: + return GPIODomain::OperationMode::INPUT; + + case TimerAF::PWM: + return GPIODomain::OperationMode::ALT_PP; + case TimerAF::Encoder: + return GPIODomain::OperationMode::ALT_PP; // TODO: check what this really needs to be for each - case TimerAF::InputCapture: return GPIODomain::OperationMode::OUTPUT_OPENDRAIN; + case TimerAF::InputCapture: + return GPIODomain::OperationMode::OUTPUT_OPENDRAIN; - case TimerAF::BreakInput: return GPIODomain::OperationMode::OUTPUT_OPENDRAIN; - case TimerAF::BreakInputCompare: return GPIODomain::OperationMode::OUTPUT_OPENDRAIN; + case TimerAF::BreakInput: + return GPIODomain::OperationMode::OUTPUT_OPENDRAIN; + case TimerAF::BreakInputCompare: + return GPIODomain::OperationMode::OUTPUT_OPENDRAIN; + } } - } - static consteval GPIODomain::Pull get_pull(ST_LIB::TimerAF af) - { - switch(af) { - case TimerAF::None: return GPIODomain::Pull::None; + static consteval GPIODomain::Pull get_pull(ST_LIB::TimerAF af) { + switch (af) { + case TimerAF::None: + return GPIODomain::Pull::None; - case TimerAF::PWM: return GPIODomain::Pull::None; - case TimerAF::Encoder: return GPIODomain::Pull::Up; + case TimerAF::PWM: + return GPIODomain::Pull::None; + case TimerAF::Encoder: + return GPIODomain::Pull::Up; // TODO: check what this really needs to be for each - case TimerAF::InputCapture: return GPIODomain::Pull::Up; - - case TimerAF::BreakInput: return GPIODomain::Pull::None; - case TimerAF::BreakInputCompare: return GPIODomain::Pull::None; + case TimerAF::InputCapture: + return GPIODomain::Pull::Up; + + case TimerAF::BreakInput: + return GPIODomain::Pull::None; + case TimerAF::BreakInputCompare: + return GPIODomain::Pull::None; + } } - } - static consteval GPIODomain::Speed get_speed(ST_LIB::TimerAF af) - { - switch(af) { - case TimerAF::None: return GPIODomain::Speed::Low; + static consteval GPIODomain::Speed get_speed(ST_LIB::TimerAF af) { + switch (af) { + case TimerAF::None: + return GPIODomain::Speed::Low; - case TimerAF::PWM: return GPIODomain::Speed::Low; - case TimerAF::Encoder: return GPIODomain::Speed::Low; + case TimerAF::PWM: + return GPIODomain::Speed::Low; + case TimerAF::Encoder: + return GPIODomain::Speed::Low; // TODO: check what this really needs to be for each - case TimerAF::InputCapture: return GPIODomain::Speed::Low; - - case TimerAF::BreakInput: return GPIODomain::Speed::Low; - case TimerAF::BreakInputCompare: return GPIODomain::Speed::Low; + case TimerAF::InputCapture: + return GPIODomain::Speed::Low; + + case TimerAF::BreakInput: + return GPIODomain::Speed::Low; + case TimerAF::BreakInputCompare: + return GPIODomain::Speed::Low; + } } - } - static constexpr ST_LIB::TimerPin empty_pin = { - .af = TimerAF::None, - .pin = ST_LIB::PA0, - .channel = TimerChannel::CHANNEL_1, - }; + static constexpr ST_LIB::TimerPin empty_pin = { + .af = TimerAF::None, + .pin = ST_LIB::PA0, + .channel = TimerChannel::CHANNEL_1, + }; -#define GetPinFromIdx(pinargs, idx) \ +#define GetPinFromIdx(pinargs, idx) \ sizeof...(pinargs) > idx ? (ST_LIB::TimerPin[]){pinargs...}[idx] : empty_pin -#define GetGPIOFromIdx(pinargs, request, idx) \ - sizeof...(pinargs) > idx ? (ST_LIB::TimerPin[]){pinargs...}[idx].pin : ST_LIB::PA0, \ - get_operation_mode(sizeof...(pinargs) > idx ? (ST_LIB::TimerPin[]){pinargs...}[idx].af : TimerAF::None), \ - get_pull(sizeof...(pinargs) > idx ? (ST_LIB::TimerPin[]){pinargs...}[idx].af : TimerAF::None), \ - get_speed(sizeof...(pinargs) > idx ? (ST_LIB::TimerPin[]){pinargs...}[idx].af : TimerAF::None), \ - (sizeof...(pinargs) > idx ? get_gpio_af(request, (ST_LIB::TimerPin[]){pinargs...}[idx]) : GPIODomain::AlternateFunction::NO_AF) - - template - consteval Timer(TimerRequest request = TimerRequest::AnyGeneralPurpose, - std::array name = EMPTY_TIMER_NAME, T... pinargs) : - e(name, request, sizeof...(pinargs), - std::array( - {GetPinFromIdx(pinargs, 0), GetPinFromIdx(pinargs, 1), GetPinFromIdx(pinargs, 2), - GetPinFromIdx(pinargs, 3), GetPinFromIdx(pinargs, 4), GetPinFromIdx(pinargs, 5), - GetPinFromIdx(pinargs, 6)})), - gpio0(GetGPIOFromIdx(pinargs, request, 0)), - gpio1(GetGPIOFromIdx(pinargs, request, 1)), - gpio2(GetGPIOFromIdx(pinargs, request, 2)), - gpio3(GetGPIOFromIdx(pinargs, request, 3)), - gpio4(GetGPIOFromIdx(pinargs, request, 4)), - gpio5(GetGPIOFromIdx(pinargs, request, 5)), - gpio6(GetGPIOFromIdx(pinargs, request, 6)) - { - static_assert((std::is_same_v && ...), - "All template arguments must be of type TimerPin"); - if(sizeof...(pinargs) > 7) { - ST_LIB::compile_error("Max 7 pins per timer"); +#define GetGPIOFromIdx(pinargs, request, idx) \ + sizeof...(pinargs) > idx ? (ST_LIB::TimerPin[]){pinargs...}[idx].pin : ST_LIB::PA0, \ + get_operation_mode( \ + sizeof...(pinargs) > idx ? (ST_LIB::TimerPin[]){pinargs...}[idx].af : TimerAF::None \ + ), \ + get_pull( \ + sizeof...(pinargs) > idx ? (ST_LIB::TimerPin[]){pinargs...}[idx].af : TimerAF::None \ + ), \ + get_speed( \ + sizeof...(pinargs) > idx ? (ST_LIB::TimerPin[]){pinargs...}[idx].af : TimerAF::None \ + ), \ + (sizeof...(pinargs) > idx ? get_gpio_af(request, (ST_LIB::TimerPin[]){pinargs...}[idx]) \ + : GPIODomain::AlternateFunction::NO_AF) + + template + consteval Timer( + TimerRequest request = TimerRequest::AnyGeneralPurpose, + std::array name = EMPTY_TIMER_NAME, + T... pinargs + ) + : e(name, + request, + sizeof...(pinargs), + std::array( + {GetPinFromIdx(pinargs, 0), + GetPinFromIdx(pinargs, 1), + GetPinFromIdx(pinargs, 2), + GetPinFromIdx(pinargs, 3), + GetPinFromIdx(pinargs, 4), + GetPinFromIdx(pinargs, 5), + GetPinFromIdx(pinargs, 6)} + )), + gpio0(GetGPIOFromIdx(pinargs, request, 0)), + gpio1(GetGPIOFromIdx(pinargs, request, 1)), + gpio2(GetGPIOFromIdx(pinargs, request, 2)), + gpio3(GetGPIOFromIdx(pinargs, request, 3)), + gpio4(GetGPIOFromIdx(pinargs, request, 4)), + gpio5(GetGPIOFromIdx(pinargs, request, 5)), + gpio6(GetGPIOFromIdx(pinargs, request, 6)) { + static_assert( + (std::is_same_v && ...), + "All template arguments must be of type TimerPin" + ); + if (sizeof...(pinargs) > 7) { + ST_LIB::compile_error("Max 7 pins per timer"); + } } - } - // anything uninitialized will be 0 - template - consteval Timer(Entry ent, T... pinargs) : - e(ent.name, ent.request, sizeof...(pinargs), - std::array( - {GetPinFromIdx(pinargs, 0), GetPinFromIdx(pinargs, 1), GetPinFromIdx(pinargs, 2), - GetPinFromIdx(pinargs, 3), GetPinFromIdx(pinargs, 4), GetPinFromIdx(pinargs, 5), - GetPinFromIdx(pinargs, 6)})), - gpio0(GetGPIOFromIdx(pinargs, ent.request, 0)), - gpio1(GetGPIOFromIdx(pinargs, ent.request, 1)), - gpio2(GetGPIOFromIdx(pinargs, ent.request, 2)), - gpio3(GetGPIOFromIdx(pinargs, ent.request, 3)), - gpio4(GetGPIOFromIdx(pinargs, ent.request, 4)), - gpio5(GetGPIOFromIdx(pinargs, ent.request, 5)), - gpio6(GetGPIOFromIdx(pinargs, ent.request, 6)) - { - static_assert((std::is_same_v && ...), - "All template arguments must be of type TimerPin"); - if(sizeof...(pinargs) > 7) { - ST_LIB::compile_error("Max 7 pins per timer"); + // anything uninitialized will be 0 + template + consteval Timer(Entry ent, T... pinargs) + : e(ent.name, + ent.request, + sizeof...(pinargs), + std::array( + {GetPinFromIdx(pinargs, 0), + GetPinFromIdx(pinargs, 1), + GetPinFromIdx(pinargs, 2), + GetPinFromIdx(pinargs, 3), + GetPinFromIdx(pinargs, 4), + GetPinFromIdx(pinargs, 5), + GetPinFromIdx(pinargs, 6)} + )), + gpio0(GetGPIOFromIdx(pinargs, ent.request, 0)), + gpio1(GetGPIOFromIdx(pinargs, ent.request, 1)), + gpio2(GetGPIOFromIdx(pinargs, ent.request, 2)), + gpio3(GetGPIOFromIdx(pinargs, ent.request, 3)), + gpio4(GetGPIOFromIdx(pinargs, ent.request, 4)), + gpio5(GetGPIOFromIdx(pinargs, ent.request, 5)), + gpio6(GetGPIOFromIdx(pinargs, ent.request, 6)) { + static_assert( + (std::is_same_v && ...), + "All template arguments must be of type TimerPin" + ); + if (sizeof...(pinargs) > 7) { + ST_LIB::compile_error("Max 7 pins per timer"); + } } - } - - template - consteval void inscribe(Ctx &ctx) const { - if(e.pin_count > 0) { gpio0.inscribe(ctx); } - if(e.pin_count > 1) { gpio1.inscribe(ctx); } - if(e.pin_count > 2) { gpio2.inscribe(ctx); } - if(e.pin_count > 3) { gpio3.inscribe(ctx); } - if(e.pin_count > 4) { gpio4.inscribe(ctx); } - if(e.pin_count > 5) { gpio5.inscribe(ctx); } - if(e.pin_count > 6) { gpio6.inscribe(ctx); } - - TimerDomain::Entry local_entry = { - .name = e.name, - .request = e.request, - .pin_count = e.pin_count, - .pins = e.pins, - }; - ctx.template add(local_entry, this); - } - }; - template - static consteval std::array build(std::span requests) { - std::array cfgs{}; - uint16_t cfg_idx = 0; - bool used_timers[25] = {0}; + template consteval void inscribe(Ctx& ctx) const { + if (e.pin_count > 0) { + gpio0.inscribe(ctx); + } + if (e.pin_count > 1) { + gpio1.inscribe(ctx); + } + if (e.pin_count > 2) { + gpio2.inscribe(ctx); + } + if (e.pin_count > 3) { + gpio3.inscribe(ctx); + } + if (e.pin_count > 4) { + gpio4.inscribe(ctx); + } + if (e.pin_count > 5) { + gpio5.inscribe(ctx); + } + if (e.pin_count > 6) { + gpio6.inscribe(ctx); + } - if(requests.size() > max_instances) { - ST_LIB::compile_error("too many Timer requests, there are only 16 timers"); - } + TimerDomain::Entry local_entry = { + .name = e.name, + .request = e.request, + .pin_count = e.pin_count, + .pins = e.pins, + }; + ctx.template add(local_entry, this); + } + }; - int remaining_requests[max_instances] = {}; - int count_remaining_requests = (int)requests.size(); - for(int i = 0; i < (int)requests.size(); i++) remaining_requests[i] = i; + template + static consteval std::array build(std::span requests) { + std::array cfgs{}; + uint16_t cfg_idx = 0; + bool used_timers[25] = {0}; - for(int i = 0; i < (int)requests.size(); i++) { - if(requests[i].request != TimerRequest::AnyGeneralPurpose && - (requests[i].request < 1 || requests[i].request > 24 || - (requests[i].request > 17 && requests[i].request < 23))) - { - ST_LIB::compile_error("Invalid TimerRequest value for timer"); + if (requests.size() > max_instances) { + ST_LIB::compile_error("too many Timer requests, there are only 16 timers"); } - uint32_t used_channels = 0; - for(uint16_t pi = 0; pi < requests[i].pin_count; pi++) { - uint32_t channel_bit = (1 << static_cast(requests[i].pins[pi].channel)); - if(used_channels & channel_bit) { - ST_LIB::compile_error("Only one pin per channel for each timer"); - } - used_channels |= channel_bit; - } - } + int remaining_requests[max_instances] = {}; + int count_remaining_requests = (int)requests.size(); + for (int i = 0; i < (int)requests.size(); i++) + remaining_requests[i] = i; - // First find any that have requested a specific timer - for(std::size_t i = 0; i < N; i++) { - uint8_t reqint = static_cast(requests[i].request); - if((requests[i].request != TimerRequest::AnyGeneralPurpose) && - (requests[i].request != TimerRequest::Any32bit)) - { - if(used_timers[reqint]) { - ST_LIB::compile_error("Error: Timer already used"); + for (int i = 0; i < (int)requests.size(); i++) { + if (requests[i].request != TimerRequest::AnyGeneralPurpose && + (requests[i].request < 1 || requests[i].request > 24 || + (requests[i].request > 17 && requests[i].request < 23))) { + ST_LIB::compile_error("Invalid TimerRequest value for timer"); } - used_timers[reqint] = true; - Config cfg = { - .timer_idx = timer_idxmap[reqint], - }; - cfgs[cfg_idx++] = cfg; + uint32_t used_channels = 0; + for (uint16_t pi = 0; pi < requests[i].pin_count; pi++) { + uint32_t channel_bit = + (1 << static_cast(requests[i].pins[pi].channel)); + if (used_channels & channel_bit) { + ST_LIB::compile_error("Only one pin per channel for each timer"); + } + used_channels |= channel_bit; + } + } - // unordered remove (remaining requests is not used here so these are ordered) - count_remaining_requests--; - remaining_requests[i] = remaining_requests[count_remaining_requests]; + // First find any that have requested a specific timer + for (std::size_t i = 0; i < N; i++) { + uint8_t reqint = static_cast(requests[i].request); + if ((requests[i].request != TimerRequest::AnyGeneralPurpose) && + (requests[i].request != TimerRequest::Any32bit)) { + if (used_timers[reqint]) { + ST_LIB::compile_error("Error: Timer already used"); + } + used_timers[reqint] = true; + + Config cfg = { + .timer_idx = timer_idxmap[reqint], + }; + cfgs[cfg_idx++] = cfg; + + // unordered remove (remaining requests is not used here so these are ordered) + count_remaining_requests--; + remaining_requests[i] = remaining_requests[count_remaining_requests]; + } } - } - // 32 bit timers, very important for scheduler - uint8_t bits32_timers[] = {2, 5, 23, 24}; - uint8_t remaining_32bit_timers[4] = {0}; - uint8_t count_remaining_32bit_timers = 0; - uint8_t count_32bit_requests = 0; + // 32 bit timers, very important for scheduler + uint8_t bits32_timers[] = {2, 5, 23, 24}; + uint8_t remaining_32bit_timers[4] = {0}; + uint8_t count_remaining_32bit_timers = 0; + uint8_t count_32bit_requests = 0; - for(int i = 0; i < (int)ARRAY_LENGTH(bits32_timers); i++) { - if(!used_timers[bits32_timers[i]]) - remaining_32bit_timers[count_remaining_32bit_timers++] = bits32_timers[i]; - } + for (int i = 0; i < (int)ARRAY_LENGTH(bits32_timers); i++) { + if (!used_timers[bits32_timers[i]]) + remaining_32bit_timers[count_remaining_32bit_timers++] = bits32_timers[i]; + } - for(int i = 0; i < count_remaining_requests; ) { - const Entry &e = requests[remaining_requests[i]]; - if(e.request == TimerRequest::Any32bit) { - if(count_remaining_32bit_timers <= count_32bit_requests) { - ST_LIB::compile_error("No remaining 32 bit timers, there are only 4. Timers {2, 5, 23, 24}"); + for (int i = 0; i < count_remaining_requests;) { + const Entry& e = requests[remaining_requests[i]]; + if (e.request == TimerRequest::Any32bit) { + if (count_remaining_32bit_timers <= count_32bit_requests) { + ST_LIB::compile_error( + "No remaining 32 bit timers, there are only 4. Timers {2, 5, 23, 24}" + ); + } + + uint8_t reqint = remaining_32bit_timers[count_32bit_requests]; + Config cfg = { + .timer_idx = timer_idxmap[reqint], + }; + cfgs[cfg_idx++] = cfg; + + // unordered remove + count_remaining_requests--; + remaining_requests[i] = remaining_requests[count_remaining_requests]; + } else { + i++; } - - uint8_t reqint = remaining_32bit_timers[count_32bit_requests]; - Config cfg = { - .timer_idx = timer_idxmap[reqint], - }; - cfgs[cfg_idx++] = cfg; - - // unordered remove - count_remaining_requests--; - remaining_requests[i] = remaining_requests[count_remaining_requests]; - } else { - i++; } - } - // can use any CountingMode (32 bit timers can also but they are higher priority) - uint8_t up_down_updown_timers[] = {3, 4}; + // can use any CountingMode (32 bit timers can also but they are higher priority) + uint8_t up_down_updown_timers[] = {3, 4}; - // 16 bit timers - /* NOTE: timers {TIM12, TIM13, TIM14} are also 16 bit but - * they are used as slave timers to tim8 - */ - uint8_t bits16_timers[] = {15, 16, 17}; - uint8_t remaining_timers[15] = {0}; - uint8_t count_remaining_timers = 0; + // 16 bit timers + /* NOTE: timers {TIM12, TIM13, TIM14} are also 16 bit but + * they are used as slave timers to tim8 + */ + uint8_t bits16_timers[] = {15, 16, 17}; + uint8_t remaining_timers[15] = {0}; + uint8_t count_remaining_timers = 0; - for(int i = 0; i < (int)ARRAY_LENGTH(bits16_timers); i++) { - if(!used_timers[bits16_timers[i]]) - remaining_timers[count_remaining_timers++] = bits16_timers[i]; - } + for (int i = 0; i < (int)ARRAY_LENGTH(bits16_timers); i++) { + if (!used_timers[bits16_timers[i]]) + remaining_timers[count_remaining_timers++] = bits16_timers[i]; + } - for(int i = 0; i < (int)ARRAY_LENGTH(up_down_updown_timers); i++) { - if(!used_timers[up_down_updown_timers[i]]) - remaining_timers[count_remaining_timers++] = up_down_updown_timers[i]; - } + for (int i = 0; i < (int)ARRAY_LENGTH(up_down_updown_timers); i++) { + if (!used_timers[up_down_updown_timers[i]]) + remaining_timers[count_remaining_timers++] = up_down_updown_timers[i]; + } - for(int i = 0; i < (int)ARRAY_LENGTH(bits32_timers); i++) { - if(!used_timers[bits32_timers[i]]) - remaining_timers[count_remaining_timers++] = bits32_timers[i]; - } + for (int i = 0; i < (int)ARRAY_LENGTH(bits32_timers); i++) { + if (!used_timers[bits32_timers[i]]) + remaining_timers[count_remaining_timers++] = bits32_timers[i]; + } - if(count_remaining_requests > count_remaining_timers) { - ST_LIB::compile_error("This should not happen"); - } + if (count_remaining_requests > count_remaining_timers) { + ST_LIB::compile_error("This should not happen"); + } - for(int i = 0; i < count_remaining_requests; i++) { - const Entry &e = requests[remaining_requests[i]]; - if(e.request != TimerRequest::AnyGeneralPurpose) { - ST_LIB::compile_error("This only processes TimerRequest::AnyGeneralPurpose"); + for (int i = 0; i < count_remaining_requests; i++) { + const Entry& e = requests[remaining_requests[i]]; + if (e.request != TimerRequest::AnyGeneralPurpose) { + ST_LIB::compile_error("This only processes TimerRequest::AnyGeneralPurpose"); + } + uint8_t reqint = remaining_timers[i]; + Config cfg = { + .timer_idx = timer_idxmap[reqint], + }; + cfgs[cfg_idx++] = cfg; } - uint8_t reqint = remaining_timers[i]; - Config cfg = { - .timer_idx = timer_idxmap[reqint], - }; - cfgs[cfg_idx++] = cfg; - } - return cfgs; - } + return cfgs; + } - // Runtime object - struct Instance { - TIM_TypeDef *tim; - TIM_HandleTypeDef *hal_tim; - uint8_t timer_idx; - }; + // Runtime object + struct Instance { + TIM_TypeDef* tim; + TIM_HandleTypeDef* hal_tim; + uint8_t timer_idx; + }; - static void (*callbacks[TimerDomain::max_instances])(void*); - static void *callback_data[TimerDomain::max_instances]; - - template struct Init { - static inline std::array instances{}; - - static void init(std::span cfgs) { - for(std::size_t i = 0; i < N; i++) { - const Config &e = cfgs[i]; - - TIM_HandleTypeDef *handle = hal_handles[e.timer_idx]; - TIM_TypeDef *tim = cmsis_timers[e.timer_idx]; - handle->Instance = tim; - handle->Init.Period = 0; - handle->Init.Prescaler = 0; - handle->Init.CounterMode = TIM_COUNTERMODE_UP; - handle->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; - handle->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - handle->Init.RepetitionCounter = 0; - - handle->ChannelState[0] = HAL_TIM_CHANNEL_STATE_READY; - handle->ChannelState[1] = HAL_TIM_CHANNEL_STATE_READY; - handle->ChannelState[2] = HAL_TIM_CHANNEL_STATE_READY; - handle->ChannelState[3] = HAL_TIM_CHANNEL_STATE_READY; - handle->ChannelState[4] = HAL_TIM_CHANNEL_STATE_READY; - handle->ChannelState[5] = HAL_TIM_CHANNEL_STATE_READY; - - handle->ChannelNState[0] = HAL_TIM_CHANNEL_STATE_READY; - handle->ChannelNState[1] = HAL_TIM_CHANNEL_STATE_READY; - handle->ChannelNState[2] = HAL_TIM_CHANNEL_STATE_READY; - handle->ChannelNState[3] = HAL_TIM_CHANNEL_STATE_READY; - handle->DMABurstState = HAL_DMA_BURST_STATE_READY; - handle->Lock = HAL_UNLOCKED; - handle->State = HAL_TIM_STATE_READY; - - rcc_enable_timer(tim); - - Instance *inst = &instances[i]; - inst->tim = tim; - inst->hal_tim = handle; - inst->timer_idx = e.timer_idx; + static void (*callbacks[TimerDomain::max_instances])(void*); + static void* callback_data[TimerDomain::max_instances]; + + template struct Init { + static inline std::array instances{}; + + static void init(std::span cfgs) { + for (std::size_t i = 0; i < N; i++) { + const Config& e = cfgs[i]; + + TIM_HandleTypeDef* handle = hal_handles[e.timer_idx]; + TIM_TypeDef* tim = cmsis_timers[e.timer_idx]; + handle->Instance = tim; + handle->Init.Period = 0; + handle->Init.Prescaler = 0; + handle->Init.CounterMode = TIM_COUNTERMODE_UP; + handle->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + handle->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + handle->Init.RepetitionCounter = 0; + + handle->ChannelState[0] = HAL_TIM_CHANNEL_STATE_READY; + handle->ChannelState[1] = HAL_TIM_CHANNEL_STATE_READY; + handle->ChannelState[2] = HAL_TIM_CHANNEL_STATE_READY; + handle->ChannelState[3] = HAL_TIM_CHANNEL_STATE_READY; + handle->ChannelState[4] = HAL_TIM_CHANNEL_STATE_READY; + handle->ChannelState[5] = HAL_TIM_CHANNEL_STATE_READY; + + handle->ChannelNState[0] = HAL_TIM_CHANNEL_STATE_READY; + handle->ChannelNState[1] = HAL_TIM_CHANNEL_STATE_READY; + handle->ChannelNState[2] = HAL_TIM_CHANNEL_STATE_READY; + handle->ChannelNState[3] = HAL_TIM_CHANNEL_STATE_READY; + handle->DMABurstState = HAL_DMA_BURST_STATE_READY; + handle->Lock = HAL_UNLOCKED; + handle->State = HAL_TIM_STATE_READY; + + rcc_enable_timer(tim); + + Instance* inst = &instances[i]; + inst->tim = tim; + inst->hal_tim = handle; + inst->timer_idx = e.timer_idx; + } } - } - }; -}; - -consteval GPIODomain::AlternateFunction -TimerDomain::Timer::get_gpio_af(ST_LIB::TimerRequest req, ST_LIB::TimerPin pin) -{ - enum TimerAF_Use { - Channel_1 = 1, - Channel_2 = 2, - Channel_3 = 3, - Channel_4 = 4, - - NegatedChannelFlag = 8, - - Channel_1_Negated = 1 | NegatedChannelFlag, - Channel_2_Negated = 2 | NegatedChannelFlag, - Channel_3_Negated = 3 | NegatedChannelFlag, - Channel_4_Negated = 4 | NegatedChannelFlag, - - ExternalTriggerFilter, /* ETR */ - BreakInput_1, - BreakInput_2, - BreakInputCompare_1, - BreakInputCompare_2, + }; }; - struct TimerPossiblePin { - ST_LIB::GPIODomain::Pin pin; - ST_LIB::GPIODomain::AlternateFunction af; - TimerAF_Use use; - }; + consteval GPIODomain::AlternateFunction TimerDomain::Timer::get_gpio_af( + ST_LIB::TimerRequest req, + ST_LIB::TimerPin pin + ) { + enum TimerAF_Use { + Channel_1 = 1, + Channel_2 = 2, + Channel_3 = 3, + Channel_4 = 4, + + NegatedChannelFlag = 8, + + Channel_1_Negated = 1 | NegatedChannelFlag, + Channel_2_Negated = 2 | NegatedChannelFlag, + Channel_3_Negated = 3 | NegatedChannelFlag, + Channel_4_Negated = 4 | NegatedChannelFlag, + + ExternalTriggerFilter, /* ETR */ + BreakInput_1, + BreakInput_2, + BreakInputCompare_1, + BreakInputCompare_2, + }; + + struct TimerPossiblePin { + ST_LIB::GPIODomain::Pin pin; + ST_LIB::GPIODomain::AlternateFunction af; + TimerAF_Use use; + }; - // 4 capture-compare channels - // complementary output -#define Tim1PinsMacro \ - {ST_LIB::PE6, ST_LIB::GPIODomain::AlternateFunction::AF1, BreakInput_2}, \ - {ST_LIB::PE6, ST_LIB::GPIODomain::AlternateFunction::AF12, BreakInputCompare_2}, \ - \ - {ST_LIB::PA6, ST_LIB::GPIODomain::AlternateFunction::AF1, BreakInput_1}, \ - {ST_LIB::PA6, ST_LIB::GPIODomain::AlternateFunction::AF11, BreakInputCompare_1}, \ - \ - {ST_LIB::PA7, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1_Negated}, \ - {ST_LIB::PB0, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_2_Negated}, \ - {ST_LIB::PB1, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_3_Negated}, \ - {ST_LIB::PE7, ST_LIB::GPIODomain::AlternateFunction::AF1, ExternalTriggerFilter}, \ - {ST_LIB::PE8, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1_Negated}, \ - {ST_LIB::PE9, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1}, \ - {ST_LIB::PE10, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_2_Negated}, \ - {ST_LIB::PE11, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_2}, \ - {ST_LIB::PE12, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_3_Negated}, \ - {ST_LIB::PE13, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_3}, \ - {ST_LIB::PE14, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_4}, \ - \ - {ST_LIB::PE15, ST_LIB::GPIODomain::AlternateFunction::AF1, BreakInput_1}, \ - {ST_LIB::PE15, ST_LIB::GPIODomain::AlternateFunction::AF13, BreakInputCompare_1}, \ - \ - {ST_LIB::PB12, ST_LIB::GPIODomain::AlternateFunction::AF1, BreakInput_1}, \ - {ST_LIB::PB12, ST_LIB::GPIODomain::AlternateFunction::AF13, BreakInputCompare_1}, \ - \ - {ST_LIB::PB13, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1_Negated}, \ - {ST_LIB::PB14, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_2_Negated}, \ - {ST_LIB::PB15, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_3_Negated}, \ - \ - {ST_LIB::PG4, ST_LIB::GPIODomain::AlternateFunction::AF1, BreakInput_2}, \ - {ST_LIB::PG4, ST_LIB::GPIODomain::AlternateFunction::AF11, BreakInputCompare_2}, \ - \ - {ST_LIB::PG5, ST_LIB::GPIODomain::AlternateFunction::AF1, ExternalTriggerFilter}, \ - \ - {ST_LIB::PA8, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1}, \ - {ST_LIB::PA9, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_2}, \ - {ST_LIB::PA10, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_3}, \ - {ST_LIB::PA11, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_4}, \ - \ - {ST_LIB::PA12, ST_LIB::GPIODomain::AlternateFunction::AF1, ExternalTriggerFilter},\ - {ST_LIB::PA12, ST_LIB::GPIODomain::AlternateFunction::AF12, BreakInput_2}, - - // 4 capture-compare channels -#define Tim2PinsMacro \ - {ST_LIB::PA0, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1}, \ - {ST_LIB::PA0, ST_LIB::GPIODomain::AlternateFunction::AF1, ExternalTriggerFilter}, \ - {ST_LIB::PA1, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_2}, \ - {ST_LIB::PA2, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_3}, \ - {ST_LIB::PA3, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_4}, \ - \ - {ST_LIB::PA5, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1}, \ - {ST_LIB::PA5, ST_LIB::GPIODomain::AlternateFunction::AF1, ExternalTriggerFilter}, \ - \ - {ST_LIB::PA15, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1}, \ - {ST_LIB::PA15, ST_LIB::GPIODomain::AlternateFunction::AF1, ExternalTriggerFilter}, \ - {ST_LIB::PB3, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_2}, \ - {ST_LIB::PB10, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_3}, \ - {ST_LIB::PB11, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_4}, - - // 4 capture-compare channels -#define Tim3PinsMacro \ - {ST_LIB::PA6, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_1}, \ - {ST_LIB::PA7, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_2}, \ - {ST_LIB::PB0, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_3}, \ - {ST_LIB::PB1, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_4}, \ - \ - {ST_LIB::PB4, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_1}, \ - {ST_LIB::PB5, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_2}, \ - \ - {ST_LIB::PC6, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_1}, \ - {ST_LIB::PC7, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_2}, \ - {ST_LIB::PC8, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_3}, \ - {ST_LIB::PC9, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_4}, \ - \ - {ST_LIB::PD2, ST_LIB::GPIODomain::AlternateFunction::AF2, ExternalTriggerFilter}, - - // 4 capture-compare channels -#define Tim4PinsMacro \ - {ST_LIB::PB6, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_1}, \ - {ST_LIB::PB7, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_2}, \ - {ST_LIB::PB8, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_3}, \ - {ST_LIB::PB9, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_4}, \ - \ - {ST_LIB::PD12, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_1}, \ - {ST_LIB::PD13, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_2}, \ - {ST_LIB::PD14, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_3}, \ - {ST_LIB::PD15, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_4}, \ - \ - {ST_LIB::PE0, ST_LIB::GPIODomain::AlternateFunction::AF2, ExternalTriggerFilter}, - - // 4 capture-compare channels -#define Tim5PinsMacro \ - {ST_LIB::PA0, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_1}, \ - {ST_LIB::PA1, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_2}, \ - {ST_LIB::PA2, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_3}, \ - {ST_LIB::PA3, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_4}, \ - {ST_LIB::PA4, ST_LIB::GPIODomain::AlternateFunction::AF2, ExternalTriggerFilter}, - - // anything invalid, this doesn't get checked for basic timers because they have no pins -#define Tim6PinsMacro {ST_LIB::PA0, ST_LIB::GPIODomain::AlternateFunction::NO_AF, Channel_1} - // anything invalid, this doesn't get checked for basic timers because they have no pins -#define Tim7PinsMacro {ST_LIB::PA0, ST_LIB::GPIODomain::AlternateFunction::NO_AF, Channel_1} - - // 4 capture-compare channels - // complementary output -#define Tim8PinsMacro \ - {ST_LIB::PA0, ST_LIB::GPIODomain::AlternateFunction::AF3, ExternalTriggerFilter}, \ - {ST_LIB::PA5, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_1_Negated}, \ - \ - {ST_LIB::PA6, ST_LIB::GPIODomain::AlternateFunction::AF3, BreakInput_1}, \ - {ST_LIB::PA6, ST_LIB::GPIODomain::AlternateFunction::AF10, BreakInputCompare_1}, \ - \ - {ST_LIB::PA7, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_1_Negated}, \ - \ - {ST_LIB::PA8, ST_LIB::GPIODomain::AlternateFunction::AF3, BreakInput_2}, \ - {ST_LIB::PA8, ST_LIB::GPIODomain::AlternateFunction::AF12, BreakInputCompare_2}, \ - \ - {ST_LIB::PB0, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_2_Negated}, \ - {ST_LIB::PB1, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_3_Negated}, \ - {ST_LIB::PB14, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_2_Negated}, \ - {ST_LIB::PB15, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_3_Negated}, \ - \ - {ST_LIB::PC6, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_1}, \ - {ST_LIB::PC7, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_2}, \ - {ST_LIB::PC8, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_3}, \ - {ST_LIB::PC9, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_4}, \ - \ - {ST_LIB::PG2, ST_LIB::GPIODomain::AlternateFunction::AF3, BreakInput_1}, \ - {ST_LIB::PG2, ST_LIB::GPIODomain::AlternateFunction::AF11, BreakInputCompare_1}, \ - \ - {ST_LIB::PG3, ST_LIB::GPIODomain::AlternateFunction::AF3, BreakInput_2}, \ - {ST_LIB::PG3, ST_LIB::GPIODomain::AlternateFunction::AF11, BreakInputCompare_2}, \ - \ - {ST_LIB::PG8, ST_LIB::GPIODomain::AlternateFunction::AF3, ExternalTriggerFilter}, - - // 2 capture-compare channels -#define Tim12PinsMacro \ - {ST_LIB::PB14, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_1}, \ - {ST_LIB::PB15, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_2}, - - // 1 capture-compare channel -#define Tim13PinsMacro \ - {ST_LIB::PA6, ST_LIB::GPIODomain::AlternateFunction::AF9, Channel_1}, \ - {ST_LIB::PF8, ST_LIB::GPIODomain::AlternateFunction::AF9, Channel_1}, - - // 1 capture-compare channel -#define Tim14PinsMacro \ - {ST_LIB::PA7, ST_LIB::GPIODomain::AlternateFunction::AF9, Channel_1}, \ - {ST_LIB::PF9, ST_LIB::GPIODomain::AlternateFunction::AF9, Channel_1}, - - // 2 capture-compare channels -#define Tim15PinsMacro \ - {ST_LIB::PA0, ST_LIB::GPIODomain::AlternateFunction::AF4, BreakInput_1}, \ - \ - {ST_LIB::PA1, ST_LIB::GPIODomain::AlternateFunction::AF4, Channel_1_Negated}, \ - {ST_LIB::PA2, ST_LIB::GPIODomain::AlternateFunction::AF4, Channel_1}, \ - {ST_LIB::PA3, ST_LIB::GPIODomain::AlternateFunction::AF4, Channel_2}, \ - \ - {ST_LIB::PC12, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_1}, \ - {ST_LIB::PD2, ST_LIB::GPIODomain::AlternateFunction::AF4, Channel_2}, \ - \ - {ST_LIB::PE3, ST_LIB::GPIODomain::AlternateFunction::AF4, BreakInput_1}, \ - {ST_LIB::PE4, ST_LIB::GPIODomain::AlternateFunction::AF4, BreakInput_1}, \ - \ - {ST_LIB::PE4, ST_LIB::GPIODomain::AlternateFunction::AF4, Channel_1}, \ - {ST_LIB::PE4, ST_LIB::GPIODomain::AlternateFunction::AF4, Channel_2}, - - // 1 capture-compare channel -#define Tim16PinsMacro \ - {ST_LIB::PF6, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1}, \ - {ST_LIB::PF8, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1_Negated}, \ - {ST_LIB::PF10, ST_LIB::GPIODomain::AlternateFunction::AF1, BreakInput_1}, - - // 1 capture-compare channel -#define Tim17PinsMacro \ - {ST_LIB::PB5, ST_LIB::GPIODomain::AlternateFunction::AF1, BreakInput_1}, \ - {ST_LIB::PB7, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1_Negated}, \ - {ST_LIB::PB9, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1}, \ - {ST_LIB::PF7, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1}, \ - {ST_LIB::PF9, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1_Negated}, \ - {ST_LIB::PG6, ST_LIB::GPIODomain::AlternateFunction::AF1, BreakInput_1}, - - // 4 capture-compare channels -#define Tim23PinsMacro \ - {ST_LIB::PB2, ST_LIB::GPIODomain::AlternateFunction::AF13, ExternalTriggerFilter}, \ - {ST_LIB::PF0, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_1}, \ - {ST_LIB::PF1, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_2}, \ - {ST_LIB::PF2, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_3}, \ - {ST_LIB::PF3, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_4}, \ - \ - {ST_LIB::PF6, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_1}, \ - {ST_LIB::PF7, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_2}, \ - {ST_LIB::PF8, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_3}, \ - {ST_LIB::PF9, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_4}, \ - \ - {ST_LIB::PG3, ST_LIB::GPIODomain::AlternateFunction::AF13, ExternalTriggerFilter}, \ - {ST_LIB::PG12, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_1}, \ - {ST_LIB::PG13, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_2}, \ - {ST_LIB::PG14, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_3}, - - // 4 capture-compare channels -#define Tim24PinsMacro \ - {ST_LIB::PB3, ST_LIB::GPIODomain::AlternateFunction::AF14, ExternalTriggerFilter}, \ - {ST_LIB::PF11, ST_LIB::GPIODomain::AlternateFunction::AF14, Channel_1}, \ - {ST_LIB::PF12, ST_LIB::GPIODomain::AlternateFunction::AF14, Channel_2}, \ - {ST_LIB::PF13, ST_LIB::GPIODomain::AlternateFunction::AF14, Channel_3}, \ - {ST_LIB::PF14, ST_LIB::GPIODomain::AlternateFunction::AF14, Channel_4}, \ - {ST_LIB::PG2, ST_LIB::GPIODomain::AlternateFunction::AF14, ExternalTriggerFilter}, - -#define X(timx, ignore) \ - constexpr TimerPossiblePin glue(tim, glue(timx, pins))[31] = { \ - glue(Tim, glue(timx, PinsMacro)) \ - }; \ - constexpr std::size_t glue(tim, glue(timx, pin_count)) = ARRAY_LENGTH(glue(tim, glue(timx, pins))); - - TimerXList + // 4 capture-compare channels + // complementary output +#define Tim1PinsMacro \ + {ST_LIB::PE6, ST_LIB::GPIODomain::AlternateFunction::AF1, BreakInput_2}, \ + {ST_LIB::PE6, ST_LIB::GPIODomain::AlternateFunction::AF12, BreakInputCompare_2}, \ + \ + {ST_LIB::PA6, ST_LIB::GPIODomain::AlternateFunction::AF1, BreakInput_1}, \ + {ST_LIB::PA6, ST_LIB::GPIODomain::AlternateFunction::AF11, BreakInputCompare_1}, \ + \ + {ST_LIB::PA7, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1_Negated}, \ + {ST_LIB::PB0, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_2_Negated}, \ + {ST_LIB::PB1, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_3_Negated}, \ + {ST_LIB::PE7, ST_LIB::GPIODomain::AlternateFunction::AF1, ExternalTriggerFilter}, \ + {ST_LIB::PE8, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1_Negated}, \ + {ST_LIB::PE9, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1}, \ + {ST_LIB::PE10, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_2_Negated}, \ + {ST_LIB::PE11, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_2}, \ + {ST_LIB::PE12, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_3_Negated}, \ + {ST_LIB::PE13, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_3}, \ + {ST_LIB::PE14, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_4}, \ + \ + {ST_LIB::PE15, ST_LIB::GPIODomain::AlternateFunction::AF1, BreakInput_1}, \ + {ST_LIB::PE15, ST_LIB::GPIODomain::AlternateFunction::AF13, BreakInputCompare_1}, \ + \ + {ST_LIB::PB12, ST_LIB::GPIODomain::AlternateFunction::AF1, BreakInput_1}, \ + {ST_LIB::PB12, ST_LIB::GPIODomain::AlternateFunction::AF13, BreakInputCompare_1}, \ + \ + {ST_LIB::PB13, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1_Negated}, \ + {ST_LIB::PB14, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_2_Negated}, \ + {ST_LIB::PB15, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_3_Negated}, \ + \ + {ST_LIB::PG4, ST_LIB::GPIODomain::AlternateFunction::AF1, BreakInput_2}, \ + {ST_LIB::PG4, ST_LIB::GPIODomain::AlternateFunction::AF11, BreakInputCompare_2}, \ + \ + {ST_LIB::PG5, ST_LIB::GPIODomain::AlternateFunction::AF1, ExternalTriggerFilter}, \ + \ + {ST_LIB::PA8, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1}, \ + {ST_LIB::PA9, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_2}, \ + {ST_LIB::PA10, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_3}, \ + {ST_LIB::PA11, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_4}, \ + \ + {ST_LIB::PA12, ST_LIB::GPIODomain::AlternateFunction::AF1, ExternalTriggerFilter}, \ + {ST_LIB::PA12, ST_LIB::GPIODomain::AlternateFunction::AF12, BreakInput_2}, + + // 4 capture-compare channels +#define Tim2PinsMacro \ + {ST_LIB::PA0, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1}, \ + {ST_LIB::PA0, ST_LIB::GPIODomain::AlternateFunction::AF1, ExternalTriggerFilter}, \ + {ST_LIB::PA1, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_2}, \ + {ST_LIB::PA2, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_3}, \ + {ST_LIB::PA3, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_4}, \ + \ + {ST_LIB::PA5, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1}, \ + {ST_LIB::PA5, ST_LIB::GPIODomain::AlternateFunction::AF1, ExternalTriggerFilter}, \ + \ + {ST_LIB::PA15, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1}, \ + {ST_LIB::PA15, ST_LIB::GPIODomain::AlternateFunction::AF1, ExternalTriggerFilter}, \ + {ST_LIB::PB3, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_2}, \ + {ST_LIB::PB10, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_3}, \ + {ST_LIB::PB11, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_4}, + + // 4 capture-compare channels +#define Tim3PinsMacro \ + {ST_LIB::PA6, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_1}, \ + {ST_LIB::PA7, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_2}, \ + {ST_LIB::PB0, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_3}, \ + {ST_LIB::PB1, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_4}, \ + \ + {ST_LIB::PB4, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_1}, \ + {ST_LIB::PB5, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_2}, \ + \ + {ST_LIB::PC6, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_1}, \ + {ST_LIB::PC7, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_2}, \ + {ST_LIB::PC8, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_3}, \ + {ST_LIB::PC9, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_4}, \ + \ + {ST_LIB::PD2, ST_LIB::GPIODomain::AlternateFunction::AF2, ExternalTriggerFilter}, + + // 4 capture-compare channels +#define Tim4PinsMacro \ + {ST_LIB::PB6, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_1}, \ + {ST_LIB::PB7, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_2}, \ + {ST_LIB::PB8, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_3}, \ + {ST_LIB::PB9, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_4}, \ + \ + {ST_LIB::PD12, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_1}, \ + {ST_LIB::PD13, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_2}, \ + {ST_LIB::PD14, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_3}, \ + {ST_LIB::PD15, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_4}, \ + \ + {ST_LIB::PE0, ST_LIB::GPIODomain::AlternateFunction::AF2, ExternalTriggerFilter}, + + // 4 capture-compare channels +#define Tim5PinsMacro \ + {ST_LIB::PA0, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_1}, \ + {ST_LIB::PA1, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_2}, \ + {ST_LIB::PA2, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_3}, \ + {ST_LIB::PA3, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_4}, \ + {ST_LIB::PA4, ST_LIB::GPIODomain::AlternateFunction::AF2, ExternalTriggerFilter}, + + // anything invalid, this doesn't get checked for basic timers because they have no pins +#define Tim6PinsMacro \ + { ST_LIB::PA0, ST_LIB::GPIODomain::AlternateFunction::NO_AF, Channel_1 } + // anything invalid, this doesn't get checked for basic timers because they have no pins +#define Tim7PinsMacro \ + { ST_LIB::PA0, ST_LIB::GPIODomain::AlternateFunction::NO_AF, Channel_1 } + + // 4 capture-compare channels + // complementary output +#define Tim8PinsMacro \ + {ST_LIB::PA0, ST_LIB::GPIODomain::AlternateFunction::AF3, ExternalTriggerFilter}, \ + {ST_LIB::PA5, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_1_Negated}, \ + \ + {ST_LIB::PA6, ST_LIB::GPIODomain::AlternateFunction::AF3, BreakInput_1}, \ + {ST_LIB::PA6, ST_LIB::GPIODomain::AlternateFunction::AF10, BreakInputCompare_1}, \ + \ + {ST_LIB::PA7, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_1_Negated}, \ + \ + {ST_LIB::PA8, ST_LIB::GPIODomain::AlternateFunction::AF3, BreakInput_2}, \ + {ST_LIB::PA8, ST_LIB::GPIODomain::AlternateFunction::AF12, BreakInputCompare_2}, \ + \ + {ST_LIB::PB0, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_2_Negated}, \ + {ST_LIB::PB1, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_3_Negated}, \ + {ST_LIB::PB14, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_2_Negated}, \ + {ST_LIB::PB15, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_3_Negated}, \ + \ + {ST_LIB::PC6, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_1}, \ + {ST_LIB::PC7, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_2}, \ + {ST_LIB::PC8, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_3}, \ + {ST_LIB::PC9, ST_LIB::GPIODomain::AlternateFunction::AF3, Channel_4}, \ + \ + {ST_LIB::PG2, ST_LIB::GPIODomain::AlternateFunction::AF3, BreakInput_1}, \ + {ST_LIB::PG2, ST_LIB::GPIODomain::AlternateFunction::AF11, BreakInputCompare_1}, \ + \ + {ST_LIB::PG3, ST_LIB::GPIODomain::AlternateFunction::AF3, BreakInput_2}, \ + {ST_LIB::PG3, ST_LIB::GPIODomain::AlternateFunction::AF11, BreakInputCompare_2}, \ + \ + {ST_LIB::PG8, ST_LIB::GPIODomain::AlternateFunction::AF3, ExternalTriggerFilter}, + + // 2 capture-compare channels +#define Tim12PinsMacro \ + {ST_LIB::PB14, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_1}, \ + {ST_LIB::PB15, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_2}, + + // 1 capture-compare channel +#define Tim13PinsMacro \ + {ST_LIB::PA6, ST_LIB::GPIODomain::AlternateFunction::AF9, Channel_1}, \ + {ST_LIB::PF8, ST_LIB::GPIODomain::AlternateFunction::AF9, Channel_1}, + + // 1 capture-compare channel +#define Tim14PinsMacro \ + {ST_LIB::PA7, ST_LIB::GPIODomain::AlternateFunction::AF9, Channel_1}, \ + {ST_LIB::PF9, ST_LIB::GPIODomain::AlternateFunction::AF9, Channel_1}, + + // 2 capture-compare channels +#define Tim15PinsMacro \ + {ST_LIB::PA0, ST_LIB::GPIODomain::AlternateFunction::AF4, BreakInput_1}, \ + \ + {ST_LIB::PA1, ST_LIB::GPIODomain::AlternateFunction::AF4, Channel_1_Negated}, \ + {ST_LIB::PA2, ST_LIB::GPIODomain::AlternateFunction::AF4, Channel_1}, \ + {ST_LIB::PA3, ST_LIB::GPIODomain::AlternateFunction::AF4, Channel_2}, \ + \ + {ST_LIB::PC12, ST_LIB::GPIODomain::AlternateFunction::AF2, Channel_1}, \ + {ST_LIB::PD2, ST_LIB::GPIODomain::AlternateFunction::AF4, Channel_2}, \ + \ + {ST_LIB::PE3, ST_LIB::GPIODomain::AlternateFunction::AF4, BreakInput_1}, \ + {ST_LIB::PE4, ST_LIB::GPIODomain::AlternateFunction::AF4, BreakInput_1}, \ + \ + {ST_LIB::PE4, ST_LIB::GPIODomain::AlternateFunction::AF4, Channel_1}, \ + {ST_LIB::PE4, ST_LIB::GPIODomain::AlternateFunction::AF4, Channel_2}, + + // 1 capture-compare channel +#define Tim16PinsMacro \ + {ST_LIB::PF6, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1}, \ + {ST_LIB::PF8, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1_Negated}, \ + {ST_LIB::PF10, ST_LIB::GPIODomain::AlternateFunction::AF1, BreakInput_1}, + + // 1 capture-compare channel +#define Tim17PinsMacro \ + {ST_LIB::PB5, ST_LIB::GPIODomain::AlternateFunction::AF1, BreakInput_1}, \ + {ST_LIB::PB7, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1_Negated}, \ + {ST_LIB::PB9, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1}, \ + {ST_LIB::PF7, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1}, \ + {ST_LIB::PF9, ST_LIB::GPIODomain::AlternateFunction::AF1, Channel_1_Negated}, \ + {ST_LIB::PG6, ST_LIB::GPIODomain::AlternateFunction::AF1, BreakInput_1}, + + // 4 capture-compare channels +#define Tim23PinsMacro \ + {ST_LIB::PB2, ST_LIB::GPIODomain::AlternateFunction::AF13, ExternalTriggerFilter}, \ + {ST_LIB::PF0, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_1}, \ + {ST_LIB::PF1, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_2}, \ + {ST_LIB::PF2, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_3}, \ + {ST_LIB::PF3, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_4}, \ + \ + {ST_LIB::PF6, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_1}, \ + {ST_LIB::PF7, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_2}, \ + {ST_LIB::PF8, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_3}, \ + {ST_LIB::PF9, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_4}, \ + \ + {ST_LIB::PG3, ST_LIB::GPIODomain::AlternateFunction::AF13, ExternalTriggerFilter}, \ + {ST_LIB::PG12, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_1}, \ + {ST_LIB::PG13, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_2}, \ + {ST_LIB::PG14, ST_LIB::GPIODomain::AlternateFunction::AF13, Channel_3}, + + // 4 capture-compare channels +#define Tim24PinsMacro \ + {ST_LIB::PB3, ST_LIB::GPIODomain::AlternateFunction::AF14, ExternalTriggerFilter}, \ + {ST_LIB::PF11, ST_LIB::GPIODomain::AlternateFunction::AF14, Channel_1}, \ + {ST_LIB::PF12, ST_LIB::GPIODomain::AlternateFunction::AF14, Channel_2}, \ + {ST_LIB::PF13, ST_LIB::GPIODomain::AlternateFunction::AF14, Channel_3}, \ + {ST_LIB::PF14, ST_LIB::GPIODomain::AlternateFunction::AF14, Channel_4}, \ + {ST_LIB::PG2, ST_LIB::GPIODomain::AlternateFunction::AF14, ExternalTriggerFilter}, + +#define X(timx, ignore) \ + constexpr TimerPossiblePin glue(tim, glue(timx, pins))[31] = {glue(Tim, glue(timx, PinsMacro)) \ + }; \ + constexpr std::size_t glue(tim, glue(timx, pin_count)) = \ + ARRAY_LENGTH(glue(tim, glue(timx, pins))); + + TimerXList #undef X - struct TimerPossPins { - TimerPossiblePin pins[31]; - std::size_t pin_count; - }; - constexpr TimerPossPins empty_pins = { - .pins = {}, - .pin_count = 0 - }; - constexpr TimerPossPins tim_pins[25] = { - empty_pins, /* 0 */ - {{Tim1PinsMacro}, tim1pin_count}, /* TIM1 */ - {{Tim2PinsMacro}, tim2pin_count}, /* TIM2 */ - {{Tim3PinsMacro}, tim3pin_count}, /* TIM3 */ - {{Tim4PinsMacro}, tim4pin_count}, /* TIM4 */ - {{Tim5PinsMacro}, tim5pin_count}, /* TIM5 */ - {{Tim6PinsMacro}, tim6pin_count}, /* TIM6 - won't get checked since they have no associated pins */ - {{Tim7PinsMacro}, tim7pin_count}, /* TIM7 - won't get checked since they have no associated pins */ - {{Tim8PinsMacro}, tim8pin_count}, /* TIM8 */ - empty_pins, /* 9 */ - empty_pins, /* 10 */ - empty_pins, /* 11 */ - {{Tim12PinsMacro}, tim12pin_count}, /* TIM12 */ - {{Tim13PinsMacro}, tim13pin_count}, /* TIM13 */ - {{Tim14PinsMacro}, tim14pin_count}, /* TIM14 */ - {{Tim15PinsMacro}, tim15pin_count}, /* TIM15 */ - {{Tim16PinsMacro}, tim16pin_count}, /* TIM16 */ - {{Tim17PinsMacro}, tim17pin_count}, /* TIM17 */ - empty_pins, /* 18 */ - empty_pins, /* 19 */ - empty_pins, /* 20 */ - empty_pins, /* 21 */ - empty_pins, /* 22 */ - {{Tim23PinsMacro}, tim23pin_count}, /* TIM23 */ - {{Tim24PinsMacro}, tim24pin_count}, /* TIM24 */ - }; + struct TimerPossPins { + TimerPossiblePin pins[31]; + std::size_t pin_count; + }; + constexpr TimerPossPins empty_pins = {.pins = {}, .pin_count = 0}; + constexpr TimerPossPins tim_pins[25] = { + empty_pins, /* 0 */ + {{Tim1PinsMacro}, tim1pin_count}, /* TIM1 */ + {{Tim2PinsMacro}, tim2pin_count}, /* TIM2 */ + {{Tim3PinsMacro}, tim3pin_count}, /* TIM3 */ + {{Tim4PinsMacro}, tim4pin_count}, /* TIM4 */ + {{Tim5PinsMacro}, tim5pin_count}, /* TIM5 */ + {{Tim6PinsMacro}, tim6pin_count + }, /* TIM6 - won't get checked since they have no associated pins */ + {{Tim7PinsMacro}, tim7pin_count + }, /* TIM7 - won't get checked since they have no associated pins */ + {{Tim8PinsMacro}, tim8pin_count}, /* TIM8 */ + empty_pins, /* 9 */ + empty_pins, /* 10 */ + empty_pins, /* 11 */ + {{Tim12PinsMacro}, tim12pin_count}, /* TIM12 */ + {{Tim13PinsMacro}, tim13pin_count}, /* TIM13 */ + {{Tim14PinsMacro}, tim14pin_count}, /* TIM14 */ + {{Tim15PinsMacro}, tim15pin_count}, /* TIM15 */ + {{Tim16PinsMacro}, tim16pin_count}, /* TIM16 */ + {{Tim17PinsMacro}, tim17pin_count}, /* TIM17 */ + empty_pins, /* 18 */ + empty_pins, /* 19 */ + empty_pins, /* 20 */ + empty_pins, /* 21 */ + empty_pins, /* 22 */ + {{Tim23PinsMacro}, tim23pin_count}, /* TIM23 */ + {{Tim24PinsMacro}, tim24pin_count}, /* TIM24 */ + }; - if(req == TimerRequest::AnyGeneralPurpose || req == TimerRequest::Any32bit) - { - ST_LIB::compile_error("Any* timers can't use pins"); - return GPIODomain::AlternateFunction::NO_AF; - } - if(req == TimerRequest::Basic_6 || req == TimerRequest::Basic_7) - { - ST_LIB::compile_error("Basic timers can't use pins"); - return GPIODomain::AlternateFunction::NO_AF; - } + if (req == TimerRequest::AnyGeneralPurpose || req == TimerRequest::Any32bit) { + ST_LIB::compile_error("Any* timers can't use pins"); + return GPIODomain::AlternateFunction::NO_AF; + } + if (req == TimerRequest::Basic_6 || req == TimerRequest::Basic_7) { + ST_LIB::compile_error("Basic timers can't use pins"); + return GPIODomain::AlternateFunction::NO_AF; + } - if(pin.channel == TimerChannel::CHANNEL_NEGATED_FLAG) { - ST_LIB::compile_error("You're not supporsed to use TimerChannel::CHANNEL_NEGATED_FLAG as a channel nº"); - return GPIODomain::AlternateFunction::NO_AF; - } + if (pin.channel == TimerChannel::CHANNEL_NEGATED_FLAG) { + ST_LIB::compile_error( + "You're not supporsed to use TimerChannel::CHANNEL_NEGATED_FLAG as a channel nº" + ); + return GPIODomain::AlternateFunction::NO_AF; + } - bool found = false; - for(std::size_t j = 0; j < tim_pins[(int)req].pin_count; j++) { - if(pin.af == ST_LIB::TimerAF::None) { - ST_LIB::compile_error("Error: Timers with pins must have associated TimerAF (alternate functions)"); - } else if(((pin.af == ST_LIB::TimerAF::InputCapture || pin.af == ST_LIB::TimerAF::PWM || pin.af == ST_LIB::TimerAF::Encoder) && + bool found = false; + for (std::size_t j = 0; j < tim_pins[(int)req].pin_count; j++) { + if (pin.af == ST_LIB::TimerAF::None) { + ST_LIB::compile_error( + "Error: Timers with pins must have associated TimerAF (alternate functions)" + ); + } else if(((pin.af == ST_LIB::TimerAF::InputCapture || pin.af == ST_LIB::TimerAF::PWM || pin.af == ST_LIB::TimerAF::Encoder) && (static_cast(pin.channel) == static_cast(tim_pins[(int)req].pins[j].use))) || ((pin.af == ST_LIB::TimerAF::BreakInput) && @@ -911,16 +1020,18 @@ TimerDomain::Timer::get_gpio_af(ST_LIB::TimerRequest req, ST_LIB::TimerPin pin) ((pin.af == ST_LIB::TimerAF::BreakInputCompare) && (tim_pins[(int)req].pins[j].use == BreakInputCompare_1 || tim_pins[(int)req].pins[j].use == BreakInputCompare_2))) { - found = true; - return tim_pins[(int)req].pins[j].af; + found = true; + return tim_pins[(int)req].pins[j].af; + } + } + if (!found) { + ST_LIB::compile_error( + "Error: Couldn't find any pins with the requested alternate function" + ); } - } - if(!found) { - ST_LIB::compile_error("Error: Couldn't find any pins with the requested alternate function"); - } - return GPIODomain::AlternateFunction::NO_AF; -} + return GPIODomain::AlternateFunction::NO_AF; + } } // namespace ST_LIB @@ -934,4 +1045,4 @@ TimerDomain::Timer::get_gpio_af(ST_LIB::TimerRequest req, ST_LIB::TimerPin pin) if(HAL_TIMEx_MasterConfigSynchronization(handle, &sMasterConfig) != HAL_OK) { ErrorHandler("Unable to configure master synch on %s", e.name); } -*/ +*/ diff --git a/Inc/HALAL/Models/TimerPeripheral/TimerPeripheral.hpp b/Inc/HALAL/Models/TimerPeripheral/TimerPeripheral.hpp index 43e010212..3cfb047c8 100644 --- a/Inc/HALAL/Models/TimerPeripheral/TimerPeripheral.hpp +++ b/Inc/HALAL/Models/TimerPeripheral/TimerPeripheral.hpp @@ -24,66 +24,66 @@ extern TIM_HandleTypeDef htim23; #include "ErrorHandler/ErrorHandler.hpp" #define PWMmap map, TimerPeripheral::PWMData>> -#define DualPWMmap map, pair, TimerPeripheral::PWMData>> +#define DualPWMmap \ + map, pair, TimerPeripheral::PWMData>> class TimerPeripheral { public: - enum PWM_MODE : uint8_t { - NORMAL = 0, - PHASED = 1, - CENTER_ALIGNED = 2 - }; - - enum TIM_TYPE { - BASE, - ADVANCED - }; - - struct PWMData { - uint32_t channel; - PWM_MODE mode; - - }; - - struct InitData { - private: - InitData() = default; - public: - TIM_TYPE type; - uint32_t prescaler; - uint32_t period; - uint32_t deadtime; - uint32_t polarity; - uint32_t negated_polarity; - vector pwm_channels = {}; - vector> input_capture_channels = {}; - InitData(TIM_TYPE type, uint32_t prescaler = 5, - uint32_t period = 55000, uint32_t deadtime = 0, uint32_t polarity = TIM_OCPOLARITY_HIGH, uint32_t negated_polarity = TIM_OCPOLARITY_HIGH); - }; - - TIM_HandleTypeDef* handle; - InitData init_data; - string name; - - static PWMmap available_pwm; - static DualPWMmap available_dual_pwms; - static vector> timers; - - TimerPeripheral() = default; - TimerPeripheral(TIM_HandleTypeDef* timer, InitData&& init_data, string name); - - static void start(); - - void init(); - bool is_registered(); - uint32_t get_prescaler(); - uint32_t get_period(); - bool is_occupied(); + enum PWM_MODE : uint8_t { NORMAL = 0, PHASED = 1, CENTER_ALIGNED = 2 }; + + enum TIM_TYPE { BASE, ADVANCED }; + + struct PWMData { + uint32_t channel; + PWM_MODE mode; + }; + + struct InitData { + private: + InitData() = default; + + public: + TIM_TYPE type; + uint32_t prescaler; + uint32_t period; + uint32_t deadtime; + uint32_t polarity; + uint32_t negated_polarity; + vector pwm_channels = {}; + vector> input_capture_channels = {}; + InitData( + TIM_TYPE type, + uint32_t prescaler = 5, + uint32_t period = 55000, + uint32_t deadtime = 0, + uint32_t polarity = TIM_OCPOLARITY_HIGH, + uint32_t negated_polarity = TIM_OCPOLARITY_HIGH + ); + }; + + TIM_HandleTypeDef* handle; + InitData init_data; + string name; + + static PWMmap available_pwm; + static DualPWMmap available_dual_pwms; + static vector> timers; + + TimerPeripheral() = default; + TimerPeripheral(TIM_HandleTypeDef* timer, InitData&& init_data, string name); + + static void start(); + + void init(); + bool is_registered(); + uint32_t get_prescaler(); + uint32_t get_period(); + bool is_occupied(); private: - static map handle_to_timer; + static map handle_to_timer; - friend class Encoder; + friend class Encoder; }; #endif diff --git a/Inc/HALAL/Services/ADC/ADC.hpp b/Inc/HALAL/Services/ADC/ADC.hpp index 100dba5a6..c0c5f9766 100644 --- a/Inc/HALAL/Services/ADC/ADC.hpp +++ b/Inc/HALAL/Services/ADC/ADC.hpp @@ -25,134 +25,155 @@ using std::string; #define MAX_12BIT 4095.0 #define MAX_16BIT 65535.0 - /** * @brief A utility class that controls ADC inputs. * - * This class handles the configuration, read, and correct casting of the ADC values in the ADC inputs using DMA to make cyclic reads, and the code pulls from the space given to the DMA memory. - * For the class to work, the pins used as parameters in each inscribe has to be declared on Runes and on Pins as ADCs (change adc_instances and AF of pins, respectively). - * Moreover, if the ADC used is not on the code generated by the .ioc you may need to change HAL_ADC_MspInit (on stm32h7xx_hal_msp.c) to configure the ADC pins and channels and their DMAs. - * Template-project has all the adcs declared on runes and the .ioc correctly configured, so unless more are needed these configurations can be assumed. + * This class handles the configuration, read, and correct casting of the ADC values in the ADC + * inputs using DMA to make cyclic reads, and the code pulls from the space given to the DMA memory. + * For the class to work, the pins used as parameters in each inscribe has to be declared on Runes + * and on Pins as ADCs (change adc_instances and AF of pins, respectively). Moreover, if the ADC + * used is not on the code generated by the .ioc you may need to change HAL_ADC_MspInit (on + * stm32h7xx_hal_msp.c) to configure the ADC pins and channels and their DMAs. Template-project has + * all the adcs declared on runes and the .ioc correctly configured, so unless more are needed these + * configurations can be assumed. */ class ADC { public: - struct InitData { - public: - ADC_TypeDef* adc; - uint32_t resolution; - uint32_t external_trigger; - vector channels; - ST_LIB::DMA_Domain::Instance *dma_instance; - string name; - - InitData() = default; - InitData(ADC_TypeDef* adc, uint32_t resolution, uint32_t external_trigger, vector& channels, ST_LIB::DMA_Domain::Instance *dma_instance, string name); - }; - - class Peripheral { - public: - ADC_HandleTypeDef* handle; - uint16_t* dma_data_buffer; - LowPowerTimer timer; - InitData init_data; - bool is_on = false; - - Peripheral() = default; - Peripheral(ADC_HandleTypeDef* handle, LowPowerTimer& timer, InitData& init_data); - - bool is_registered(); - }; - - class Instance { - public: - Peripheral* peripheral; - uint32_t channel; - uint32_t rank; - - Instance() = default; - Instance(Peripheral* peripheral, uint32_t channel); - }; - - /** - * @brief A method to add a pin as an ADC input on the ST-LIB. - - * This method has to be invoked before the ST-LIB::start(), as it marks the pin to be configured as an ADC, and that configuration is made in the ST-LIB::start(). - * As said on the class description, only correctly configured ADC pins will work correctly when declared as an ADC. - * It is forbidden to declare a pin as an ADC and as anything else on services at the same time, and it will result in undefined behavior. - * - * @param pin the Pin to be added as an ADC - * - * @return the id that represents the ADC inside this utility class, used in all its functions. - */ - static uint8_t inscribe(Pin pin); - - /** - * @brief Method used in ST-LIBstart() to configure pins inscribed as ADCs. - * - * The start methods of the HALAL are made to be invoked in an specific order. - * As such, its recommended to not use them isolated, and instead use the ST-LIB::start(). - * The If for any reason not starting a service from HALAL was desired, removing the definition of its HAL parent module would deactivate it. - * For example, if ADC was needed out of the code, removing define HAL_ADC_MODULE_ENABLED on stm32h7xx_hal_conf.h would result in that behavior. - */ - static void start(); - - /** - * @brief Activates the ADC represented by the id. - * - * After the ADC is configured in the ST-LIBstart::() it needs to be activated with turn_on to start reading. - * This is made this way so the user can control exactly when the measures start to be taken. - * The get_value() method will only work on a Pin that has both been inscribed, started, and turned on. - * - * @param id the id of ADC to be activated. - */ - static void turn_on(uint8_t id); - - /** - * @brief Returns the value of the last DMA read made by the ADC. - * - * The get_value function doesn t issue a read, but rather pulls the memory where the last read made is saved, transforms the value - * with the reference voltage and returns the voltage represented with that value. - * The capture of the value is made automatically by the DMA configured for the respective ADC channel, and the frequency of the reads is - * dependant on the configuration of the DMA itself. - * - * @param id the id of the ADC to be read. - * - * @return the value of the ADC in volts. The ADC_MAX_VOLTAGE needs to be correctly configured in order for this function to work. - */ - static float get_value(uint8_t id); - - /** - * @brief Returns the value of the last DMA read made by the ADC. - * - * The get_int_value function doesn t issue a read, but rather pulls the memory where the last read made is saved and returns that value. - * The capture of the value is made automatically by the DMA configured for the respective ADC channel, and the frequency of the reads is - * dependant on the configuration of the DMA itself. - * - * @param id the id of the ADC to be read. - * - * @return the value of the ADC, in uint16_t format. 0 is minimum possible value and max_uint16_t is the maximum. - */ - static uint16_t get_int_value(uint8_t id); - - /** - * @brief Function that returns the pointer where the DMA of the ADC writes its value, for maximum efficiency on the access - * - * This function has no protection of any kind, other that checking that an adc exists before giving the pointer back. - * If the ADC is running or not should be handled by the user. - * The adcs of the adc3 peripheral are not aligned in the buffer, and are instead aligned in the get functions. - * If the values are accessed from the buffer, is the responsibility of the user to handle the shift problems. - */ - static uint16_t* get_value_pointer(uint8_t id); - - static Peripheral peripherals[3]; + struct InitData { + public: + ADC_TypeDef* adc; + uint32_t resolution; + uint32_t external_trigger; + vector channels; + ST_LIB::DMA_Domain::Instance* dma_instance; + string name; + + InitData() = default; + InitData( + ADC_TypeDef* adc, + uint32_t resolution, + uint32_t external_trigger, + vector& channels, + ST_LIB::DMA_Domain::Instance* dma_instance, + string name + ); + }; + + class Peripheral { + public: + ADC_HandleTypeDef* handle; + uint16_t* dma_data_buffer; + LowPowerTimer timer; + InitData init_data; + bool is_on = false; + + Peripheral() = default; + Peripheral(ADC_HandleTypeDef* handle, LowPowerTimer& timer, InitData& init_data); + + bool is_registered(); + }; + + class Instance { + public: + Peripheral* peripheral; + uint32_t channel; + uint32_t rank; + + Instance() = default; + Instance(Peripheral* peripheral, uint32_t channel); + }; + + /** + * @brief A method to add a pin as an ADC input on the ST-LIB. + + * This method has to be invoked before the ST-LIB::start(), as it marks the pin to be + configured as an ADC, and that configuration is made in the ST-LIB::start(). + * As said on the class description, only correctly configured ADC pins will work correctly when + declared as an ADC. + * It is forbidden to declare a pin as an ADC and as anything else on services at the same time, + and it will result in undefined behavior. + * + * @param pin the Pin to be added as an ADC + * + * @return the id that represents the ADC inside this utility class, used in all its functions. + */ + static uint8_t inscribe(Pin pin); + + /** + * @brief Method used in ST-LIBstart() to configure pins inscribed as ADCs. + * + * The start methods of the HALAL are made to be invoked in an specific order. + * As such, its recommended to not use them isolated, and instead use the ST-LIB::start(). + * The If for any reason not starting a service from HALAL was desired, removing the definition + * of its HAL parent module would deactivate it. For example, if ADC was needed out of the code, + * removing define HAL_ADC_MODULE_ENABLED on stm32h7xx_hal_conf.h would result in that behavior. + */ + static void start(); + + /** + * @brief Activates the ADC represented by the id. + * + * After the ADC is configured in the ST-LIBstart::() it needs to be activated with turn_on to + * start reading. This is made this way so the user can control exactly when the measures start + * to be taken. The get_value() method will only work on a Pin that has both been inscribed, + * started, and turned on. + * + * @param id the id of ADC to be activated. + */ + static void turn_on(uint8_t id); + + /** + * @brief Returns the value of the last DMA read made by the ADC. + * + * The get_value function doesn t issue a read, but rather pulls the memory where the last read + * made is saved, transforms the value with the reference voltage and returns the voltage + * represented with that value. The capture of the value is made automatically by the DMA + * configured for the respective ADC channel, and the frequency of the reads is dependant on the + * configuration of the DMA itself. + * + * @param id the id of the ADC to be read. + * + * @return the value of the ADC in volts. The ADC_MAX_VOLTAGE needs to be correctly configured + * in order for this function to work. + */ + static float get_value(uint8_t id); + + /** + * @brief Returns the value of the last DMA read made by the ADC. + * + * The get_int_value function doesn t issue a read, but rather pulls the memory where the last + * read made is saved and returns that value. The capture of the value is made automatically by + * the DMA configured for the respective ADC channel, and the frequency of the reads is + * dependant on the configuration of the DMA itself. + * + * @param id the id of the ADC to be read. + * + * @return the value of the ADC, in uint16_t format. 0 is minimum possible value and + * max_uint16_t is the maximum. + */ + static uint16_t get_int_value(uint8_t id); + + /** + * @brief Function that returns the pointer where the DMA of the ADC writes its value, for + * maximum efficiency on the access + * + * This function has no protection of any kind, other that checking that an adc exists before + * giving the pointer back. If the ADC is running or not should be handled by the user. The adcs + * of the adc3 peripheral are not aligned in the buffer, and are instead aligned in the get + * functions. If the values are accessed from the buffer, is the responsibility of the user to + * handle the shift problems. + */ + static uint16_t* get_value_pointer(uint8_t id); + + static Peripheral peripherals[3]; private: - static uint32_t ranks[16]; - static map available_instances; - static unordered_map active_instances; - static uint8_t id_counter; + static uint32_t ranks[16]; + static map available_instances; + static unordered_map active_instances; + static uint8_t id_counter; - static void init(Peripheral& peripheral); + static void init(Peripheral& peripheral); }; #endif \ No newline at end of file diff --git a/Inc/HALAL/Services/ADC/NewADC.hpp b/Inc/HALAL/Services/ADC/NewADC.hpp index 58cc8181f..2933637d5 100644 --- a/Inc/HALAL/Services/ADC/NewADC.hpp +++ b/Inc/HALAL/Services/ADC/NewADC.hpp @@ -22,7 +22,7 @@ extern ADC_HandleTypeDef hadc3; #endif namespace ST_LIB { -extern void compile_error(const char *msg); +extern void compile_error(const char* msg); #ifdef HAL_ADC_MODULE_ENABLED using ::hadc1; @@ -30,696 +30,702 @@ using ::hadc2; using ::hadc3; struct ADCDomain { - static constexpr std::size_t max_channels_per_peripheral = 16; - enum class Peripheral : uint8_t { AUTO, ADC_1, ADC_2, ADC_3 }; - - enum class Resolution : uint32_t { - BITS_16 = ADC_RESOLUTION_16B, - BITS_14 = ADC_RESOLUTION_14B, - BITS_12 = ADC_RESOLUTION_12B, - BITS_10 = ADC_RESOLUTION_10B, - BITS_8 = ADC_RESOLUTION_8B, - }; - - enum class SampleTime : uint32_t { - CYCLES_1_5 = ADC_SAMPLETIME_1CYCLE_5, - CYCLES_2_5 = ADC_SAMPLETIME_2CYCLES_5, - CYCLES_8_5 = ADC_SAMPLETIME_8CYCLES_5, - CYCLES_16_5 = ADC_SAMPLETIME_16CYCLES_5, - CYCLES_32_5 = ADC_SAMPLETIME_32CYCLES_5, - CYCLES_64_5 = ADC_SAMPLETIME_64CYCLES_5, - CYCLES_387_5 = ADC_SAMPLETIME_387CYCLES_5, - CYCLES_810_5 = ADC_SAMPLETIME_810CYCLES_5, - }; - - enum class ClockPrescaler : uint32_t { - DIV1 = ADC_CLOCK_ASYNC_DIV1, - DIV2 = ADC_CLOCK_ASYNC_DIV2, - DIV4 = ADC_CLOCK_ASYNC_DIV4, - DIV6 = ADC_CLOCK_ASYNC_DIV6, - DIV8 = ADC_CLOCK_ASYNC_DIV8, - DIV10 = ADC_CLOCK_ASYNC_DIV10, - DIV12 = ADC_CLOCK_ASYNC_DIV12, - DIV16 = ADC_CLOCK_ASYNC_DIV16, - DIV32 = ADC_CLOCK_ASYNC_DIV32, - DIV64 = ADC_CLOCK_ASYNC_DIV64, - DIV128 = ADC_CLOCK_ASYNC_DIV128, - DIV256 = ADC_CLOCK_ASYNC_DIV256, - }; - - enum class Channel : uint32_t { - AUTO = 0xFFFFFFFFu, - CH0 = ADC_CHANNEL_0, - CH1 = ADC_CHANNEL_1, - CH2 = ADC_CHANNEL_2, - CH3 = ADC_CHANNEL_3, - CH4 = ADC_CHANNEL_4, - CH5 = ADC_CHANNEL_5, - CH6 = ADC_CHANNEL_6, - CH7 = ADC_CHANNEL_7, - CH8 = ADC_CHANNEL_8, - CH9 = ADC_CHANNEL_9, - CH10 = ADC_CHANNEL_10, - CH11 = ADC_CHANNEL_11, - CH12 = ADC_CHANNEL_12, - CH13 = ADC_CHANNEL_13, - CH14 = ADC_CHANNEL_14, - CH15 = ADC_CHANNEL_15, - CH16 = ADC_CHANNEL_16, - CH17 = ADC_CHANNEL_17, - CH18 = ADC_CHANNEL_18, - CH19 = ADC_CHANNEL_19, - VREFINT = ADC_CHANNEL_VREFINT, - TEMPSENSOR = ADC_CHANNEL_TEMPSENSOR, - VBAT = ADC_CHANNEL_VBAT, - }; - - struct Entry { - size_t gpio_idx; - GPIODomain::Pin pin; - Peripheral peripheral; - Channel channel; - Resolution resolution; - SampleTime sample_time; - ClockPrescaler prescaler; - uint32_t sample_rate_hz; - float *output; - }; - - struct ADC { - GPIODomain::GPIO gpio; - using domain = ADCDomain; - - Entry e; - - consteval ADC(const GPIODomain::Pin &pin, float &output, - Resolution resolution = Resolution::BITS_12, - SampleTime sample_time = SampleTime::CYCLES_8_5, - ClockPrescaler prescaler = ClockPrescaler::DIV1, - uint32_t sample_rate_hz = 0, - Peripheral peripheral = Peripheral::AUTO, - Channel channel = Channel::AUTO) - : gpio{pin, GPIODomain::OperationMode::ANALOG, GPIODomain::Pull::None, - GPIODomain::Speed::Low}, - e{.gpio_idx = 0, - .pin = pin, - .peripheral = peripheral, - .channel = channel, - .resolution = resolution, - .sample_time = sample_time, - .prescaler = prescaler, - .sample_rate_hz = sample_rate_hz, - .output = &output} { + static constexpr std::size_t max_channels_per_peripheral = 16; + enum class Peripheral : uint8_t { AUTO, ADC_1, ADC_2, ADC_3 }; + + enum class Resolution : uint32_t { + BITS_16 = ADC_RESOLUTION_16B, + BITS_14 = ADC_RESOLUTION_14B, + BITS_12 = ADC_RESOLUTION_12B, + BITS_10 = ADC_RESOLUTION_10B, + BITS_8 = ADC_RESOLUTION_8B, + }; + + enum class SampleTime : uint32_t { + CYCLES_1_5 = ADC_SAMPLETIME_1CYCLE_5, + CYCLES_2_5 = ADC_SAMPLETIME_2CYCLES_5, + CYCLES_8_5 = ADC_SAMPLETIME_8CYCLES_5, + CYCLES_16_5 = ADC_SAMPLETIME_16CYCLES_5, + CYCLES_32_5 = ADC_SAMPLETIME_32CYCLES_5, + CYCLES_64_5 = ADC_SAMPLETIME_64CYCLES_5, + CYCLES_387_5 = ADC_SAMPLETIME_387CYCLES_5, + CYCLES_810_5 = ADC_SAMPLETIME_810CYCLES_5, + }; + + enum class ClockPrescaler : uint32_t { + DIV1 = ADC_CLOCK_ASYNC_DIV1, + DIV2 = ADC_CLOCK_ASYNC_DIV2, + DIV4 = ADC_CLOCK_ASYNC_DIV4, + DIV6 = ADC_CLOCK_ASYNC_DIV6, + DIV8 = ADC_CLOCK_ASYNC_DIV8, + DIV10 = ADC_CLOCK_ASYNC_DIV10, + DIV12 = ADC_CLOCK_ASYNC_DIV12, + DIV16 = ADC_CLOCK_ASYNC_DIV16, + DIV32 = ADC_CLOCK_ASYNC_DIV32, + DIV64 = ADC_CLOCK_ASYNC_DIV64, + DIV128 = ADC_CLOCK_ASYNC_DIV128, + DIV256 = ADC_CLOCK_ASYNC_DIV256, + }; + + enum class Channel : uint32_t { + AUTO = 0xFFFFFFFFu, + CH0 = ADC_CHANNEL_0, + CH1 = ADC_CHANNEL_1, + CH2 = ADC_CHANNEL_2, + CH3 = ADC_CHANNEL_3, + CH4 = ADC_CHANNEL_4, + CH5 = ADC_CHANNEL_5, + CH6 = ADC_CHANNEL_6, + CH7 = ADC_CHANNEL_7, + CH8 = ADC_CHANNEL_8, + CH9 = ADC_CHANNEL_9, + CH10 = ADC_CHANNEL_10, + CH11 = ADC_CHANNEL_11, + CH12 = ADC_CHANNEL_12, + CH13 = ADC_CHANNEL_13, + CH14 = ADC_CHANNEL_14, + CH15 = ADC_CHANNEL_15, + CH16 = ADC_CHANNEL_16, + CH17 = ADC_CHANNEL_17, + CH18 = ADC_CHANNEL_18, + CH19 = ADC_CHANNEL_19, + VREFINT = ADC_CHANNEL_VREFINT, + TEMPSENSOR = ADC_CHANNEL_TEMPSENSOR, + VBAT = ADC_CHANNEL_VBAT, + }; + + struct Entry { + size_t gpio_idx; + GPIODomain::Pin pin; + Peripheral peripheral; + Channel channel; + Resolution resolution; + SampleTime sample_time; + ClockPrescaler prescaler; + uint32_t sample_rate_hz; + float* output; + }; + + struct ADC { + GPIODomain::GPIO gpio; + using domain = ADCDomain; + + Entry e; + + consteval ADC( + const GPIODomain::Pin& pin, + float& output, + Resolution resolution = Resolution::BITS_12, + SampleTime sample_time = SampleTime::CYCLES_8_5, + ClockPrescaler prescaler = ClockPrescaler::DIV1, + uint32_t sample_rate_hz = 0, + Peripheral peripheral = Peripheral::AUTO, + Channel channel = Channel::AUTO + ) + : gpio{pin, GPIODomain::OperationMode::ANALOG, GPIODomain::Pull::None, GPIODomain::Speed::Low}, + e{.gpio_idx = 0, + .pin = pin, + .peripheral = peripheral, + .channel = channel, + .resolution = resolution, + .sample_time = sample_time, + .prescaler = prescaler, + .sample_rate_hz = sample_rate_hz, + .output = &output} {} + + consteval ADC( + const GPIODomain::Pin& pin, + Peripheral peripheral, + Channel channel, + float& output, + Resolution resolution = Resolution::BITS_12, + SampleTime sample_time = SampleTime::CYCLES_8_5, + ClockPrescaler prescaler = ClockPrescaler::DIV1, + uint32_t sample_rate_hz = 0 + ) + : ADC(pin, + output, + resolution, + sample_time, + prescaler, + sample_rate_hz, + peripheral, + channel) {} + + template consteval std::size_t inscribe(Ctx& ctx) const { + const auto gpio_idx = gpio.inscribe(ctx); + Entry entry = e; + entry.gpio_idx = gpio_idx; + const auto resolved = resolve_mapping(entry); + entry.peripheral = resolved.first; + entry.channel = resolved.second; + return ctx.template add(entry, this); + } + }; + + static constexpr std::size_t max_instances{32}; + + struct Config { + size_t gpio_idx; + Peripheral peripheral; + Channel channel; + Resolution resolution; + SampleTime sample_time; + ClockPrescaler prescaler; + uint32_t sample_rate_hz; + float* output; + }; + + static constexpr uint8_t peripheral_index(Peripheral p) { + switch (p) { + case Peripheral::AUTO: + return 0; + case Peripheral::ADC_1: + return 0; + case Peripheral::ADC_2: + return 1; + case Peripheral::ADC_3: + return 2; + } + return 0; } - consteval ADC(const GPIODomain::Pin &pin, Peripheral peripheral, - Channel channel, float &output, - Resolution resolution = Resolution::BITS_12, - SampleTime sample_time = SampleTime::CYCLES_8_5, - ClockPrescaler prescaler = ClockPrescaler::DIV1, - uint32_t sample_rate_hz = 0) - : ADC(pin, output, resolution, sample_time, prescaler, sample_rate_hz, - peripheral, channel) {} - - template consteval std::size_t inscribe(Ctx &ctx) const { - const auto gpio_idx = gpio.inscribe(ctx); - Entry entry = e; - entry.gpio_idx = gpio_idx; - const auto resolved = resolve_mapping(entry); - entry.peripheral = resolved.first; - entry.channel = resolved.second; - return ctx.template add(entry, this); - } - }; - - static constexpr std::size_t max_instances{32}; - - struct Config { - size_t gpio_idx; - Peripheral peripheral; - Channel channel; - Resolution resolution; - SampleTime sample_time; - ClockPrescaler prescaler; - uint32_t sample_rate_hz; - float *output; - }; - - static constexpr uint8_t peripheral_index(Peripheral p) { - switch (p) { - case Peripheral::AUTO: - return 0; - case Peripheral::ADC_1: - return 0; - case Peripheral::ADC_2: - return 1; - case Peripheral::ADC_3: - return 2; + static uint8_t peripheral_index_from_handle(ADC_HandleTypeDef* handle) { + if (handle == &hadc1) { + return peripheral_index(Peripheral::ADC_1); + } + if (handle == &hadc2) { + return peripheral_index(Peripheral::ADC_2); + } + if (handle == &hadc3) { + return peripheral_index(Peripheral::ADC_3); + } + return 0; } - return 0; - } - static uint8_t peripheral_index_from_handle(ADC_HandleTypeDef *handle) { - if (handle == &hadc1) { - return peripheral_index(Peripheral::ADC_1); - } - if (handle == &hadc2) { - return peripheral_index(Peripheral::ADC_2); - } - if (handle == &hadc3) { - return peripheral_index(Peripheral::ADC_3); - } - return 0; - } - - static inline std::array peripheral_running{false, false, false}; - static inline std::array active_channel{ - Channel::AUTO, Channel::AUTO, Channel::AUTO}; - - static consteval bool is_valid_channel(Channel ch) { - switch (ch) { - case Channel::AUTO: - return true; - case Channel::CH0: - case Channel::CH1: - case Channel::CH2: - case Channel::CH3: - case Channel::CH4: - case Channel::CH5: - case Channel::CH6: - case Channel::CH7: - case Channel::CH8: - case Channel::CH9: - case Channel::CH10: - case Channel::CH11: - case Channel::CH12: - case Channel::CH13: - case Channel::CH14: - case Channel::CH15: - case Channel::CH16: - case Channel::CH17: - case Channel::CH18: - case Channel::CH19: - case Channel::VREFINT: - case Channel::TEMPSENSOR: - case Channel::VBAT: - return true; - } - return false; - } - - static consteval uint8_t resolution_bits(Resolution r) { - switch (r) { - case Resolution::BITS_16: - return 16; - case Resolution::BITS_14: - return 14; - case Resolution::BITS_12: - return 12; - case Resolution::BITS_10: - return 10; - case Resolution::BITS_8: - return 8; - } - return 12; - } - - static consteval bool is_internal_channel(Channel ch) { - switch (ch) { - case Channel::VREFINT: - case Channel::TEMPSENSOR: - case Channel::VBAT: - return true; - default: - return false; + static inline std::array peripheral_running{false, false, false}; + static inline std::array active_channel{ + Channel::AUTO, + Channel::AUTO, + Channel::AUTO + }; + + static consteval bool is_valid_channel(Channel ch) { + switch (ch) { + case Channel::AUTO: + return true; + case Channel::CH0: + case Channel::CH1: + case Channel::CH2: + case Channel::CH3: + case Channel::CH4: + case Channel::CH5: + case Channel::CH6: + case Channel::CH7: + case Channel::CH8: + case Channel::CH9: + case Channel::CH10: + case Channel::CH11: + case Channel::CH12: + case Channel::CH13: + case Channel::CH14: + case Channel::CH15: + case Channel::CH16: + case Channel::CH17: + case Channel::CH18: + case Channel::CH19: + case Channel::VREFINT: + case Channel::TEMPSENSOR: + case Channel::VBAT: + return true; + } + return false; } - } - static consteval bool internal_channel_allowed(Peripheral p, Channel ch) { - if (!is_internal_channel(ch)) { - return true; + static consteval uint8_t resolution_bits(Resolution r) { + switch (r) { + case Resolution::BITS_16: + return 16; + case Resolution::BITS_14: + return 14; + case Resolution::BITS_12: + return 12; + case Resolution::BITS_10: + return 10; + case Resolution::BITS_8: + return 8; + } + return 12; } -#if STLIB_HAS_ADC3 - return p == Peripheral::ADC_3; -#else - return p == Peripheral::ADC_2; -#endif - } - static consteval bool resolution_supported(Peripheral p, Resolution r) { - if (p == Peripheral::ADC_3) { - return resolution_bits(r) <= 12; - } - return true; - } - - struct PinMapping { - GPIODomain::Pin pin; - Peripheral peripheral; - Channel channel; - uint8_t max_bits; - }; - - static constexpr uint8_t max_bits_adc12 = 16; - static constexpr uint8_t max_bits_adc3 = 12; - - static constexpr std::array pin_map{{ - {PA0, Peripheral::ADC_1, Channel::CH16, max_bits_adc12}, - {PA1, Peripheral::ADC_1, Channel::CH17, max_bits_adc12}, - {PA2, Peripheral::ADC_1, Channel::CH14, max_bits_adc12}, - {PA3, Peripheral::ADC_1, Channel::CH15, max_bits_adc12}, - {PA4, Peripheral::ADC_1, Channel::CH18, max_bits_adc12}, - {PA5, Peripheral::ADC_1, Channel::CH19, max_bits_adc12}, - {PA6, Peripheral::ADC_1, Channel::CH3, max_bits_adc12}, - {PA7, Peripheral::ADC_1, Channel::CH7, max_bits_adc12}, - {PB0, Peripheral::ADC_1, Channel::CH9, max_bits_adc12}, - {PB1, Peripheral::ADC_1, Channel::CH5, max_bits_adc12}, - {PC0, Peripheral::ADC_1, Channel::CH10, max_bits_adc12}, - {PC1, Peripheral::ADC_1, Channel::CH11, max_bits_adc12}, - {PC4, Peripheral::ADC_1, Channel::CH4, max_bits_adc12}, - {PC5, Peripheral::ADC_1, Channel::CH8, max_bits_adc12}, - {PF11, Peripheral::ADC_1, Channel::CH2, max_bits_adc12}, - {PF12, Peripheral::ADC_1, Channel::CH6, max_bits_adc12}, - - {PA2, Peripheral::ADC_2, Channel::CH14, max_bits_adc12}, - {PA3, Peripheral::ADC_2, Channel::CH15, max_bits_adc12}, - {PA4, Peripheral::ADC_2, Channel::CH18, max_bits_adc12}, - {PA5, Peripheral::ADC_2, Channel::CH19, max_bits_adc12}, - {PA6, Peripheral::ADC_2, Channel::CH3, max_bits_adc12}, - {PA7, Peripheral::ADC_2, Channel::CH7, max_bits_adc12}, - {PB0, Peripheral::ADC_2, Channel::CH9, max_bits_adc12}, - {PB1, Peripheral::ADC_2, Channel::CH5, max_bits_adc12}, - {PC0, Peripheral::ADC_2, Channel::CH10, max_bits_adc12}, - {PC1, Peripheral::ADC_2, Channel::CH11, max_bits_adc12}, - {PC4, Peripheral::ADC_2, Channel::CH4, max_bits_adc12}, - {PC5, Peripheral::ADC_2, Channel::CH8, max_bits_adc12}, - {PF13, Peripheral::ADC_2, Channel::CH2, max_bits_adc12}, - {PF14, Peripheral::ADC_2, Channel::CH6, max_bits_adc12}, - - {PC0, Peripheral::ADC_3, Channel::CH10, max_bits_adc3}, - {PC1, Peripheral::ADC_3, Channel::CH11, max_bits_adc3}, - {PC2, Peripheral::ADC_3, Channel::CH0, max_bits_adc3}, - {PC3, Peripheral::ADC_3, Channel::CH1, max_bits_adc3}, - {PF3, Peripheral::ADC_3, Channel::CH5, max_bits_adc3}, - {PF4, Peripheral::ADC_3, Channel::CH9, max_bits_adc3}, - {PF5, Peripheral::ADC_3, Channel::CH4, max_bits_adc3}, - {PF6, Peripheral::ADC_3, Channel::CH8, max_bits_adc3}, - {PF7, Peripheral::ADC_3, Channel::CH3, max_bits_adc3}, - {PF8, Peripheral::ADC_3, Channel::CH7, max_bits_adc3}, - {PF9, Peripheral::ADC_3, Channel::CH2, max_bits_adc3}, - {PF10, Peripheral::ADC_3, Channel::CH6, max_bits_adc3}, - }}; - - static consteval uint8_t preference_score(Resolution r, Peripheral p) { - const uint8_t bits = resolution_bits(r); - if (bits > 12) { - switch (p) { - case Peripheral::ADC_1: - return 3; - case Peripheral::ADC_2: - return 2; - case Peripheral::ADC_3: - return 0; - case Peripheral::AUTO: - return 0; - } - } else { - switch (p) { - case Peripheral::ADC_3: - return 3; - case Peripheral::ADC_1: - return 2; - case Peripheral::ADC_2: - return 1; - case Peripheral::AUTO: - return 0; - } - } - return 0; - } - - static consteval std::pair - resolve_mapping(const Entry &e) { - if (e.peripheral != Peripheral::AUTO && e.channel != Channel::AUTO) { - if (!is_valid_channel(e.channel)) { - compile_error("ADC: invalid channel"); - } - if (!internal_channel_allowed(e.peripheral, e.channel)) { - compile_error("ADC: internal channel must use dedicated ADC"); - } - if (!resolution_supported(e.peripheral, e.resolution)) { - compile_error("ADC: resolution not supported by selected ADC"); - } - return {e.peripheral, e.channel}; + static consteval bool is_internal_channel(Channel ch) { + switch (ch) { + case Channel::VREFINT: + case Channel::TEMPSENSOR: + case Channel::VBAT: + return true; + default: + return false; + } } - if (e.channel != Channel::AUTO && is_internal_channel(e.channel)) { - const Peripheral p = (e.peripheral == Peripheral::AUTO) + static consteval bool internal_channel_allowed(Peripheral p, Channel ch) { + if (!is_internal_channel(ch)) { + return true; + } #if STLIB_HAS_ADC3 - ? Peripheral::ADC_3 + return p == Peripheral::ADC_3; #else - ? Peripheral::ADC_2 + return p == Peripheral::ADC_2; #endif - : e.peripheral; - - if (!internal_channel_allowed(p, e.channel)) { - compile_error("ADC: internal channel must use dedicated ADC"); - } - if (!resolution_supported(p, e.resolution)) { - compile_error("ADC: resolution not supported by selected ADC"); - } - return {p, e.channel}; } - bool found = false; - Peripheral best_peripheral = Peripheral::AUTO; - Channel best_channel = Channel::AUTO; - uint8_t best_score = 0; - - for (const auto &m : pin_map) { - if (m.pin != e.pin) { - continue; - } - if (e.peripheral != Peripheral::AUTO && m.peripheral != e.peripheral) { - continue; - } - if (e.channel != Channel::AUTO && m.channel != e.channel) { - continue; - } - if (resolution_bits(e.resolution) > m.max_bits) { - continue; - } - - const uint8_t score = preference_score(e.resolution, m.peripheral); - if (!found || score > best_score) { - found = true; - best_score = score; - best_peripheral = m.peripheral; - best_channel = m.channel; - } + static consteval bool resolution_supported(Peripheral p, Resolution r) { + if (p == Peripheral::ADC_3) { + return resolution_bits(r) <= 12; + } + return true; } - if (!found) { - compile_error("ADC: no valid ADC mapping for pin/resolution"); + struct PinMapping { + GPIODomain::Pin pin; + Peripheral peripheral; + Channel channel; + uint8_t max_bits; + }; + + static constexpr uint8_t max_bits_adc12 = 16; + static constexpr uint8_t max_bits_adc3 = 12; + + static constexpr std::array pin_map{{ + {PA0, Peripheral::ADC_1, Channel::CH16, max_bits_adc12}, + {PA1, Peripheral::ADC_1, Channel::CH17, max_bits_adc12}, + {PA2, Peripheral::ADC_1, Channel::CH14, max_bits_adc12}, + {PA3, Peripheral::ADC_1, Channel::CH15, max_bits_adc12}, + {PA4, Peripheral::ADC_1, Channel::CH18, max_bits_adc12}, + {PA5, Peripheral::ADC_1, Channel::CH19, max_bits_adc12}, + {PA6, Peripheral::ADC_1, Channel::CH3, max_bits_adc12}, + {PA7, Peripheral::ADC_1, Channel::CH7, max_bits_adc12}, + {PB0, Peripheral::ADC_1, Channel::CH9, max_bits_adc12}, + {PB1, Peripheral::ADC_1, Channel::CH5, max_bits_adc12}, + {PC0, Peripheral::ADC_1, Channel::CH10, max_bits_adc12}, + {PC1, Peripheral::ADC_1, Channel::CH11, max_bits_adc12}, + {PC4, Peripheral::ADC_1, Channel::CH4, max_bits_adc12}, + {PC5, Peripheral::ADC_1, Channel::CH8, max_bits_adc12}, + {PF11, Peripheral::ADC_1, Channel::CH2, max_bits_adc12}, + {PF12, Peripheral::ADC_1, Channel::CH6, max_bits_adc12}, + + {PA2, Peripheral::ADC_2, Channel::CH14, max_bits_adc12}, + {PA3, Peripheral::ADC_2, Channel::CH15, max_bits_adc12}, + {PA4, Peripheral::ADC_2, Channel::CH18, max_bits_adc12}, + {PA5, Peripheral::ADC_2, Channel::CH19, max_bits_adc12}, + {PA6, Peripheral::ADC_2, Channel::CH3, max_bits_adc12}, + {PA7, Peripheral::ADC_2, Channel::CH7, max_bits_adc12}, + {PB0, Peripheral::ADC_2, Channel::CH9, max_bits_adc12}, + {PB1, Peripheral::ADC_2, Channel::CH5, max_bits_adc12}, + {PC0, Peripheral::ADC_2, Channel::CH10, max_bits_adc12}, + {PC1, Peripheral::ADC_2, Channel::CH11, max_bits_adc12}, + {PC4, Peripheral::ADC_2, Channel::CH4, max_bits_adc12}, + {PC5, Peripheral::ADC_2, Channel::CH8, max_bits_adc12}, + {PF13, Peripheral::ADC_2, Channel::CH2, max_bits_adc12}, + {PF14, Peripheral::ADC_2, Channel::CH6, max_bits_adc12}, + + {PC0, Peripheral::ADC_3, Channel::CH10, max_bits_adc3}, + {PC1, Peripheral::ADC_3, Channel::CH11, max_bits_adc3}, + {PC2, Peripheral::ADC_3, Channel::CH0, max_bits_adc3}, + {PC3, Peripheral::ADC_3, Channel::CH1, max_bits_adc3}, + {PF3, Peripheral::ADC_3, Channel::CH5, max_bits_adc3}, + {PF4, Peripheral::ADC_3, Channel::CH9, max_bits_adc3}, + {PF5, Peripheral::ADC_3, Channel::CH4, max_bits_adc3}, + {PF6, Peripheral::ADC_3, Channel::CH8, max_bits_adc3}, + {PF7, Peripheral::ADC_3, Channel::CH3, max_bits_adc3}, + {PF8, Peripheral::ADC_3, Channel::CH7, max_bits_adc3}, + {PF9, Peripheral::ADC_3, Channel::CH2, max_bits_adc3}, + {PF10, Peripheral::ADC_3, Channel::CH6, max_bits_adc3}, + }}; + + static consteval uint8_t preference_score(Resolution r, Peripheral p) { + const uint8_t bits = resolution_bits(r); + if (bits > 12) { + switch (p) { + case Peripheral::ADC_1: + return 3; + case Peripheral::ADC_2: + return 2; + case Peripheral::ADC_3: + return 0; + case Peripheral::AUTO: + return 0; + } + } else { + switch (p) { + case Peripheral::ADC_3: + return 3; + case Peripheral::ADC_1: + return 2; + case Peripheral::ADC_2: + return 1; + case Peripheral::AUTO: + return 0; + } + } + return 0; } - return {best_peripheral, best_channel}; - } - - template - static consteval array build(span entries) { - static_assert(N <= max_instances, "ADCDomain: too many instances"); - if (entries.size() != N) { - compile_error("ADC: build entries size mismatch"); - } + static consteval std::pair resolve_mapping(const Entry& e) { + if (e.peripheral != Peripheral::AUTO && e.channel != Channel::AUTO) { + if (!is_valid_channel(e.channel)) { + compile_error("ADC: invalid channel"); + } + if (!internal_channel_allowed(e.peripheral, e.channel)) { + compile_error("ADC: internal channel must use dedicated ADC"); + } + if (!resolution_supported(e.peripheral, e.resolution)) { + compile_error("ADC: resolution not supported by selected ADC"); + } + return {e.peripheral, e.channel}; + } - array cfgs{}; - array periph_seen{}; - array periph_resolution{}; - array periph_prescaler{}; - array periph_rate{}; - array periph_counts{}; - - for (std::size_t i = 0; i < N; ++i) { - const auto &e = entries[i]; - const auto [peripheral, channel] = resolve_mapping(e); - - if (!is_valid_channel(channel)) { - compile_error("ADC: invalid channel"); - } - if (!internal_channel_allowed(peripheral, channel)) { - compile_error("ADC: internal channel must use dedicated ADC"); - } - if (!resolution_supported(peripheral, e.resolution)) { - compile_error("ADC: resolution not supported by selected ADC"); - } - if (e.sample_rate_hz != 0) { - compile_error("ADC: sample_rate_hz is not supported in polling mode"); - } - - const auto pidx = peripheral_index(peripheral); - - if (!periph_seen[pidx]) { - periph_seen[pidx] = true; - periph_resolution[pidx] = e.resolution; - periph_prescaler[pidx] = e.prescaler; - periph_rate[pidx] = e.sample_rate_hz; - } else { - if (periph_resolution[pidx] != e.resolution) { - compile_error("ADC: resolution mismatch on same peripheral"); + if (e.channel != Channel::AUTO && is_internal_channel(e.channel)) { + const Peripheral p = (e.peripheral == Peripheral::AUTO) +#if STLIB_HAS_ADC3 + ? Peripheral::ADC_3 +#else + ? Peripheral::ADC_2 +#endif + : e.peripheral; + + if (!internal_channel_allowed(p, e.channel)) { + compile_error("ADC: internal channel must use dedicated ADC"); + } + if (!resolution_supported(p, e.resolution)) { + compile_error("ADC: resolution not supported by selected ADC"); + } + return {p, e.channel}; } - if (periph_prescaler[pidx] != e.prescaler) { - compile_error("ADC: prescaler mismatch on same peripheral"); + + bool found = false; + Peripheral best_peripheral = Peripheral::AUTO; + Channel best_channel = Channel::AUTO; + uint8_t best_score = 0; + + for (const auto& m : pin_map) { + if (m.pin != e.pin) { + continue; + } + if (e.peripheral != Peripheral::AUTO && m.peripheral != e.peripheral) { + continue; + } + if (e.channel != Channel::AUTO && m.channel != e.channel) { + continue; + } + if (resolution_bits(e.resolution) > m.max_bits) { + continue; + } + + const uint8_t score = preference_score(e.resolution, m.peripheral); + if (!found || score > best_score) { + found = true; + best_score = score; + best_peripheral = m.peripheral; + best_channel = m.channel; + } } - if (periph_rate[pidx] != e.sample_rate_hz) { - compile_error("ADC: sample rate mismatch on same peripheral"); + + if (!found) { + compile_error("ADC: no valid ADC mapping for pin/resolution"); } - } - ++periph_counts[pidx]; - if (periph_counts[pidx] > max_channels_per_peripheral) { - compile_error("ADC: too many channels on same peripheral"); - } + return {best_peripheral, best_channel}; + } - for (std::size_t j = 0; j < i; ++j) { - const auto &prev = entries[j]; - if (prev.gpio_idx == e.gpio_idx) { - compile_error("ADC: GPIO already used"); + template static consteval array build(span entries) { + static_assert(N <= max_instances, "ADCDomain: too many instances"); + if (entries.size() != N) { + compile_error("ADC: build entries size mismatch"); } - const auto [prev_peripheral, prev_channel] = resolve_mapping(prev); - if (prev_peripheral == peripheral && prev_channel == channel) { - compile_error("ADC: duplicate channel on same peripheral"); + + array cfgs{}; + array periph_seen{}; + array periph_resolution{}; + array periph_prescaler{}; + array periph_rate{}; + array periph_counts{}; + + for (std::size_t i = 0; i < N; ++i) { + const auto& e = entries[i]; + const auto [peripheral, channel] = resolve_mapping(e); + + if (!is_valid_channel(channel)) { + compile_error("ADC: invalid channel"); + } + if (!internal_channel_allowed(peripheral, channel)) { + compile_error("ADC: internal channel must use dedicated ADC"); + } + if (!resolution_supported(peripheral, e.resolution)) { + compile_error("ADC: resolution not supported by selected ADC"); + } + if (e.sample_rate_hz != 0) { + compile_error("ADC: sample_rate_hz is not supported in polling mode"); + } + + const auto pidx = peripheral_index(peripheral); + + if (!periph_seen[pidx]) { + periph_seen[pidx] = true; + periph_resolution[pidx] = e.resolution; + periph_prescaler[pidx] = e.prescaler; + periph_rate[pidx] = e.sample_rate_hz; + } else { + if (periph_resolution[pidx] != e.resolution) { + compile_error("ADC: resolution mismatch on same peripheral"); + } + if (periph_prescaler[pidx] != e.prescaler) { + compile_error("ADC: prescaler mismatch on same peripheral"); + } + if (periph_rate[pidx] != e.sample_rate_hz) { + compile_error("ADC: sample rate mismatch on same peripheral"); + } + } + + ++periph_counts[pidx]; + if (periph_counts[pidx] > max_channels_per_peripheral) { + compile_error("ADC: too many channels on same peripheral"); + } + + for (std::size_t j = 0; j < i; ++j) { + const auto& prev = entries[j]; + if (prev.gpio_idx == e.gpio_idx) { + compile_error("ADC: GPIO already used"); + } + const auto [prev_peripheral, prev_channel] = resolve_mapping(prev); + if (prev_peripheral == peripheral && prev_channel == channel) { + compile_error("ADC: duplicate channel on same peripheral"); + } + } + + cfgs[i] = { + .gpio_idx = e.gpio_idx, + .peripheral = peripheral, + .channel = channel, + .resolution = e.resolution, + .sample_time = e.sample_time, + .prescaler = e.prescaler, + .sample_rate_hz = e.sample_rate_hz, + .output = e.output, + }; } - } - - cfgs[i] = { - .gpio_idx = e.gpio_idx, - .peripheral = peripheral, - .channel = channel, - .resolution = e.resolution, - .sample_time = e.sample_time, - .prescaler = e.prescaler, - .sample_rate_hz = e.sample_rate_hz, - .output = e.output, - }; - } - return cfgs; - } - - struct Instance { - ADC_HandleTypeDef *handle = nullptr; - Channel channel = Channel::CH0; - SampleTime sample_time = SampleTime::CYCLES_8_5; - Resolution resolution = Resolution::BITS_12; - float *output = nullptr; - - static constexpr uint32_t max_raw_for_resolution(Resolution r) { - switch (r) { - case Resolution::BITS_16: - return 65535U; - case Resolution::BITS_14: - return 16383U; - case Resolution::BITS_12: - return 4095U; - case Resolution::BITS_10: - return 1023U; - case Resolution::BITS_8: - return 255U; - } - return 4095U; + return cfgs; } - private: - uint32_t sample_raw(uint32_t timeout_ms = 2) { - if (handle == nullptr) { - return 0; - } - - const uint8_t pidx = peripheral_index_from_handle(handle); - const bool channel_change = active_channel[pidx] != channel; - - if (channel_change && peripheral_running[pidx]) { - (void)HAL_ADC_Stop(handle); - peripheral_running[pidx] = false; - } - - if (channel_change) { - ADC_ChannelConfTypeDef sConfig{}; - sConfig.Channel = static_cast(channel); - sConfig.Rank = ADC_REGULAR_RANK_1; - sConfig.SamplingTime = static_cast(sample_time); - sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.OffsetNumber = ADC_OFFSET_NONE; - sConfig.Offset = 0; + struct Instance { + ADC_HandleTypeDef* handle = nullptr; + Channel channel = Channel::CH0; + SampleTime sample_time = SampleTime::CYCLES_8_5; + Resolution resolution = Resolution::BITS_12; + float* output = nullptr; + + static constexpr uint32_t max_raw_for_resolution(Resolution r) { + switch (r) { + case Resolution::BITS_16: + return 65535U; + case Resolution::BITS_14: + return 16383U; + case Resolution::BITS_12: + return 4095U; + case Resolution::BITS_10: + return 1023U; + case Resolution::BITS_8: + return 255U; + } + return 4095U; + } + + private: + uint32_t sample_raw(uint32_t timeout_ms = 2) { + if (handle == nullptr) { + return 0; + } + + const uint8_t pidx = peripheral_index_from_handle(handle); + const bool channel_change = active_channel[pidx] != channel; + + if (channel_change && peripheral_running[pidx]) { + (void)HAL_ADC_Stop(handle); + peripheral_running[pidx] = false; + } + + if (channel_change) { + ADC_ChannelConfTypeDef sConfig{}; + sConfig.Channel = static_cast(channel); + sConfig.Rank = ADC_REGULAR_RANK_1; + sConfig.SamplingTime = static_cast(sample_time); + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; #if defined(ADC_VER_V5_V90) - sConfig.OffsetSignedSaturation = DISABLE; + sConfig.OffsetSignedSaturation = DISABLE; #endif - if (HAL_ADC_ConfigChannel(handle, &sConfig) != HAL_OK) { - return 0; + if (HAL_ADC_ConfigChannel(handle, &sConfig) != HAL_OK) { + return 0; + } + active_channel[pidx] = channel; + } + + if (!peripheral_running[pidx]) { + const HAL_StatusTypeDef start_status = HAL_ADC_Start(handle); + if (start_status != HAL_OK && start_status != HAL_BUSY) { + return 0; + } + peripheral_running[pidx] = true; + } + + if (HAL_ADC_PollForConversion(handle, timeout_ms) != HAL_OK) { + // Recover from a stalled peripheral (timeout/error) by re-arming once. + (void)HAL_ADC_Stop(handle); + peripheral_running[pidx] = false; + + const HAL_StatusTypeDef restart_status = HAL_ADC_Start(handle); + if (restart_status != HAL_OK && restart_status != HAL_BUSY) { + return 0; + } + peripheral_running[pidx] = true; + + if (HAL_ADC_PollForConversion(handle, timeout_ms) != HAL_OK) { + return 0; + } + } + return HAL_ADC_GetValue(handle); } - active_channel[pidx] = channel; - } - if (!peripheral_running[pidx]) { - const HAL_StatusTypeDef start_status = HAL_ADC_Start(handle); - if (start_status != HAL_OK && start_status != HAL_BUSY) { - return 0; + public: + float get_raw(uint32_t timeout_ms = 2) { + return static_cast(sample_raw(timeout_ms)); } - peripheral_running[pidx] = true; - } - if (HAL_ADC_PollForConversion(handle, timeout_ms) != HAL_OK) { - // Recover from a stalled peripheral (timeout/error) by re-arming once. - (void)HAL_ADC_Stop(handle); - peripheral_running[pidx] = false; - - const HAL_StatusTypeDef restart_status = HAL_ADC_Start(handle); - if (restart_status != HAL_OK && restart_status != HAL_BUSY) { - return 0; + float get_value_from_raw(float raw, float vref = 3.3f) const { + const float max_val = static_cast(max_raw_for_resolution(resolution)); + if (max_val <= 0.0f) { + return 0.0f; + } + return (raw / max_val) * vref; } - peripheral_running[pidx] = true; - if (HAL_ADC_PollForConversion(handle, timeout_ms) != HAL_OK) { - return 0; + float get_value(uint32_t timeout_ms = 2, float vref = 3.3f) { + const float raw = get_raw(timeout_ms); + return get_value_from_raw(raw, vref); } - } - return HAL_ADC_GetValue(handle); - } - - public: - float get_raw(uint32_t timeout_ms = 2) { - return static_cast(sample_raw(timeout_ms)); - } - - float get_value_from_raw(float raw, float vref = 3.3f) const { - const float max_val = static_cast(max_raw_for_resolution(resolution)); - if (max_val <= 0.0f) { - return 0.0f; - } - return (raw / max_val) * vref; - } - - float get_value(uint32_t timeout_ms = 2, float vref = 3.3f) { - const float raw = get_raw(timeout_ms); - return get_value_from_raw(raw, vref); - } - - void read(float vref = 3.3f, uint32_t timeout_ms = 2) { - if (output == nullptr) { - return; - } - *output = get_value(timeout_ms, vref); - } - }; - - template struct Init { - static inline std::array instances{}; - - static ADC_HandleTypeDef *handle_for(Peripheral p) { - switch (p) { - case Peripheral::ADC_1: - return &hadc1; - case Peripheral::ADC_2: - return &hadc2; - case Peripheral::ADC_3: - return &hadc3; - case Peripheral::AUTO: - break; - } - return &hadc1; - } + void read(float vref = 3.3f, uint32_t timeout_ms = 2) { + if (output == nullptr) { + return; + } + *output = get_value(timeout_ms, vref); + } + }; + + template struct Init { + static inline std::array instances{}; + + static ADC_HandleTypeDef* handle_for(Peripheral p) { + switch (p) { + case Peripheral::ADC_1: + return &hadc1; + case Peripheral::ADC_2: + return &hadc2; + case Peripheral::ADC_3: + return &hadc3; + case Peripheral::AUTO: + break; + } + return &hadc1; + } - static void configure_peripheral(const Config &cfg) { - ADC_HandleTypeDef *hadc = handle_for(cfg.peripheral); - - if (cfg.peripheral == Peripheral::ADC_1 || - cfg.peripheral == Peripheral::ADC_2) { - __HAL_RCC_ADC12_CLK_ENABLE(); - hadc->Instance = (cfg.peripheral == Peripheral::ADC_1) ? ADC1 : ADC2; - } else { - __HAL_RCC_ADC3_CLK_ENABLE(); - hadc->Instance = ADC3; - HAL_SYSCFG_AnalogSwitchConfig(SYSCFG_SWITCH_PC2, - SYSCFG_SWITCH_PC2_OPEN); - HAL_SYSCFG_AnalogSwitchConfig(SYSCFG_SWITCH_PC3, - SYSCFG_SWITCH_PC3_OPEN); - } - - hadc->Init.ClockPrescaler = static_cast(cfg.prescaler); - hadc->Init.Resolution = static_cast(cfg.resolution); - hadc->Init.ScanConvMode = ADC_SCAN_DISABLE; - hadc->Init.EOCSelection = ADC_EOC_SINGLE_CONV; - hadc->Init.LowPowerAutoWait = DISABLE; - hadc->Init.ContinuousConvMode = ENABLE; - hadc->Init.NbrOfConversion = 1; - hadc->Init.DiscontinuousConvMode = DISABLE; - hadc->Init.NbrOfDiscConversion = 0; - hadc->Init.ExternalTrigConv = ADC_SOFTWARE_START; - hadc->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; - hadc->Init.ConversionDataManagement = ADC_CONVERSIONDATA_DR; + static void configure_peripheral(const Config& cfg) { + ADC_HandleTypeDef* hadc = handle_for(cfg.peripheral); + + if (cfg.peripheral == Peripheral::ADC_1 || cfg.peripheral == Peripheral::ADC_2) { + __HAL_RCC_ADC12_CLK_ENABLE(); + hadc->Instance = (cfg.peripheral == Peripheral::ADC_1) ? ADC1 : ADC2; + } else { + __HAL_RCC_ADC3_CLK_ENABLE(); + hadc->Instance = ADC3; + HAL_SYSCFG_AnalogSwitchConfig(SYSCFG_SWITCH_PC2, SYSCFG_SWITCH_PC2_OPEN); + HAL_SYSCFG_AnalogSwitchConfig(SYSCFG_SWITCH_PC3, SYSCFG_SWITCH_PC3_OPEN); + } + + hadc->Init.ClockPrescaler = static_cast(cfg.prescaler); + hadc->Init.Resolution = static_cast(cfg.resolution); + hadc->Init.ScanConvMode = ADC_SCAN_DISABLE; + hadc->Init.EOCSelection = ADC_EOC_SINGLE_CONV; + hadc->Init.LowPowerAutoWait = DISABLE; + hadc->Init.ContinuousConvMode = ENABLE; + hadc->Init.NbrOfConversion = 1; + hadc->Init.DiscontinuousConvMode = DISABLE; + hadc->Init.NbrOfDiscConversion = 0; + hadc->Init.ExternalTrigConv = ADC_SOFTWARE_START; + hadc->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + hadc->Init.ConversionDataManagement = ADC_CONVERSIONDATA_DR; #if defined(ADC_VER_V5_V90) - hadc->Init.SamplingMode = ADC_SAMPLING_MODE_NORMAL; - hadc->Init.DMAContinuousRequests = DISABLE; + hadc->Init.SamplingMode = ADC_SAMPLING_MODE_NORMAL; + hadc->Init.DMAContinuousRequests = DISABLE; #endif - hadc->Init.Overrun = ADC_OVR_DATA_PRESERVED; - hadc->Init.LeftBitShift = ADC_LEFTBITSHIFT_NONE; - hadc->Init.OversamplingMode = DISABLE; - } - - static void init(std::span cfgs, - std::span gpio_instances = - std::span{}) { - bool periph_configured[3] = {false, false, false}; - (void)gpio_instances; - - for (std::size_t i = 0; i < N; ++i) { - const auto &cfg = cfgs[i]; - if (cfg.peripheral == Peripheral::AUTO || - cfg.channel == Channel::AUTO) { - ErrorHandler("ADC config unresolved (AUTO)"); - } - const auto pidx = peripheral_index(cfg.peripheral); - if (!periph_configured[pidx]) { - configure_peripheral(cfg); - ADC_HandleTypeDef *hadc = handle_for(cfg.peripheral); - hadc->Init.NbrOfConversion = 1; - hadc->Init.ScanConvMode = ADC_SCAN_DISABLE; - hadc->Init.ContinuousConvMode = ENABLE; - if (HAL_ADC_Init(hadc) != HAL_OK) { - ErrorHandler("ADC Init failed"); - } - - periph_configured[pidx] = true; + hadc->Init.Overrun = ADC_OVR_DATA_PRESERVED; + hadc->Init.LeftBitShift = ADC_LEFTBITSHIFT_NONE; + hadc->Init.OversamplingMode = DISABLE; } - instances[i].handle = handle_for(cfg.peripheral); - instances[i].channel = cfg.channel; - instances[i].sample_time = cfg.sample_time; - instances[i].resolution = cfg.resolution; - instances[i].output = cfg.output; - } - } - }; + static void init( + std::span cfgs, + std::span gpio_instances = std::span{} + ) { + bool periph_configured[3] = {false, false, false}; + (void)gpio_instances; + + for (std::size_t i = 0; i < N; ++i) { + const auto& cfg = cfgs[i]; + if (cfg.peripheral == Peripheral::AUTO || cfg.channel == Channel::AUTO) { + ErrorHandler("ADC config unresolved (AUTO)"); + } + const auto pidx = peripheral_index(cfg.peripheral); + if (!periph_configured[pidx]) { + configure_peripheral(cfg); + ADC_HandleTypeDef* hadc = handle_for(cfg.peripheral); + hadc->Init.NbrOfConversion = 1; + hadc->Init.ScanConvMode = ADC_SCAN_DISABLE; + hadc->Init.ContinuousConvMode = ENABLE; + if (HAL_ADC_Init(hadc) != HAL_OK) { + ErrorHandler("ADC Init failed"); + } + + periph_configured[pidx] = true; + } + + instances[i].handle = handle_for(cfg.peripheral); + instances[i].channel = cfg.channel; + instances[i].sample_time = cfg.sample_time; + instances[i].resolution = cfg.resolution; + instances[i].output = cfg.output; + } + } + }; }; #else // HAL_ADC_MODULE_ENABLED struct ADCDomain { - static constexpr std::size_t max_instances{0}; - struct Entry {}; - struct Config {}; - template - static consteval array build(span) { - return {}; - } - template struct Init { - static void - init(std::span, - std::span = std::span{}) {} - }; + static constexpr std::size_t max_instances{0}; + struct Entry {}; + struct Config {}; + template static consteval array build(span) { return {}; } + template struct Init { + static void init( + std::span, + std::span = std::span{} + ) {} + }; }; #endif // HAL_ADC_MODULE_ENABLED diff --git a/Inc/HALAL/Services/CORDIC/CORDIC.hpp b/Inc/HALAL/Services/CORDIC/CORDIC.hpp index 511acdf34..19b811f10 100644 --- a/Inc/HALAL/Services/CORDIC/CORDIC.hpp +++ b/Inc/HALAL/Services/CORDIC/CORDIC.hpp @@ -20,126 +20,136 @@ /** * @brief Cordic class. Has some trigonometric static functions with high speed computation time * - * The cordic class calculates with integers cosine, sine, phase and modulus twice as fast as arm_q31 functions do - * The class uses its own units (as arm_q31 methods do) to traduce angles, coordinates, and results to have the maximun precision - * Angles go in the range of [-pi, pi] -> [-max int32, max int32] radians for ints and [0,2*pi] -> [0, max uint32] for uints. They are equivalent - * Coordinates go in the range of [-1,1] -> [-max int32, max int32] for ints (they work on uints but not in a logical way). - * If we wanted to represent any euclidean space we would need to delimit and scale it. - * Precision of these functions are all around 5*10^-10 - * The methods just check if the cordic is correctly configurated, configurate it if thats not the case, then writes inside the CORDIC and reads the results - * They work with vectors so you can ask for multiple values so it skips the small overhead after the first. + * The cordic class calculates with integers cosine, sine, phase and modulus twice as fast as + * arm_q31 functions do The class uses its own units (as arm_q31 methods do) to traduce angles, + * coordinates, and results to have the maximun precision Angles go in the range of [-pi, pi] -> + * [-max int32, max int32] radians for ints and [0,2*pi] -> [0, max uint32] for uints. They are + * equivalent Coordinates go in the range of [-1,1] -> [-max int32, max int32] for ints (they work + * on uints but not in a logical way). If we wanted to represent any euclidean space we would need + * to delimit and scale it. Precision of these functions are all around 5*10^-10 The methods just + * check if the cordic is correctly configurated, configurate it if thats not the case, then writes + * inside the CORDIC and reads the results They work with vectors so you can ask for multiple values + * so it skips the small overhead after the first. * * * The CORDIC configuration work as follows * In the CSR CORDIC register you can directly configurate the CORDIC by writting on it. * The masks for each variable that you can configurate are: - * Functions : 0x0000000F (0.COSINE 1.SINE 2.PHASE 3.MODULUS 4.ARCTANGENT 5.HCOSINE 6.HSINE 7.HARCTANGENT 8.NATLOG 9.SQRT) - * Precision : 0x000000F0 (0.1_cycle 1.2_cycles 2.3_cycles ...) (we use on all of them 6, the maximum for the CORDIC inside the stm (can configurate more but does nothing)) - * Scale : 0x00000700 (0 = 0, 1 = 1 ... means that the in has been shifted to the right and the output needs to be shifted to the left) (we do not use this, 0) - * Interrupt enabled : 0x00010000 (0.disabled 1.enabled) (only useful for DMA, which we do not use) - * DMA read and write : 0x00020000 0x00040000 (respectively) (same as abode) - * In and out arguments: 0x00100000 0x00080000 (respectively) (0.1argument 1.2arguments) (depends on the function and if we want residual results) - * Width input and output: 0x00200000 0x00400000 (respectively) (0.32bytes 1.16bytes) (we use max size for more precision, 0) - * Ready flag : 0x80000000 (this is to check if the CORDIC is ready, but the bus already handles all the transactions on the CORDIC inside the stm so we don t need to waste processing time on this) + * Functions : 0x0000000F + * (0.COSINE 1.SINE 2.PHASE 3.MODULUS 4.ARCTANGENT 5.HCOSINE 6.HSINE 7.HARCTANGENT 8.NATLOG 9.SQRT) + * Precision : 0x000000F0 (0.1_cycle 1.2_cycles 2.3_cycles ...) (we use on all of them 6, the + * maximum for the CORDIC inside the stm (can configurate more but does nothing)) Scale : 0x00000700 + * (0 = 0, 1 = 1 ... means that the in has been shifted to the right and the output needs to be + * shifted to the left) (we do not use this, 0) Interrupt enabled : 0x00010000 + * (0.disabled 1.enabled) (only useful for DMA, which we do not use) DMA read and write : 0x00020000 + * 0x00040000 (respectively) (same as abode) In and out arguments: 0x00100000 0x00080000 + * (respectively) (0.1argument 1.2arguments) (depends on the function and if we want residual + * results) Width input and output: 0x00200000 0x00400000 (respectively) (0.32bytes 1.16bytes) (we + * use max size for more precision, 0) Ready flag : 0x80000000 (this is to check if the CORDIC is + * ready, but the bus already handles all the transactions on the CORDIC inside the stm so we don t + * need to waste processing time on this) */ -enum Operation_Computation{ - COSINE, - SINE, - SINE_COSINE, - PHASE, - MODULUS, - PHASE_MODULUS, - NONE - }; +enum Operation_Computation { COSINE, SINE, SINE_COSINE, PHASE, MODULUS, PHASE_MODULUS, NONE }; - -class RotationComputer{ +class RotationComputer { public: - static void start(); - /** - * @brief Cosine function. Receives size angles and output size results - * @param Angle the pointer to the angle array with size "size". input - * @param Out the pointer to the array where the results are saved, with size "size". output - * @param Size the size of both pointers in indexes. - * - * The cosine queries on single shot each of the values on the angle array until size index - * If the angle or the out parameter has less size than the size parameter, it will throw an exception - * If one or both angle and out parameters have more size than size, it will just calculate until size and leave the rest as it is - */ - static void cos(int32_t *angle,int32_t *out,int32_t size); - - - /** - * @brief sine function. virtually the same as the cosine function - */ - static void sin(int32_t *angle,int32_t *out,int32_t size); - - - /** - * @brief Cosine and sine function. Does both while only needing the time that one takes - * @param Angle the pointer to the angle array with size "size". input - * @param Cos_out the pointer to the array where the cosine results are saved, with size "size". output - * @param Sin:out the pointer to the array where the sine results are saved, with size "size". output - * @param Size the size of both pointers in indexes. - * - * Uses a cosine query but configurating the CORDIC to save the residual results - * The residual results are values that appear when calculating any function, which are not the return of the function itself - * The cosine function of the CORDIC gives as residual result the sine function, so if you read twice you will get first cosine then sine - * If you don t configurate the CORDIC to save its residual result it will not work. The method does it by itself, thats why we don t just use the cosine method. - * The exact time it takes is around 4 cycles more than the cos function, which is around 30-40. So 10% more as it needs to read one more register. - * The reads are protected to give the correct result, thats why it needs 4 cycles for the read (on average) and not just one - */ - static void cos_and_sin(int32_t *angle, int32_t *cos_out, int32_t *sin_out, int32_t size); - - - /** - * @brief Phase function. Calculates the angle between a vector from (0,0) to (x,y) and the vector (1,0) - * @param x. The array of values x of each vector with size "size". input. - * @param y. The array of values y of each vector with size "size". input. - * @param angle_out. The array where the functions puts its output, which is an angle. output. - */ - static void phase(int32_t *x, int32_t *y, int32_t *angle_out, int32_t size); - - - /** - * @brief modulus function. Calculates the modulus of the vector from (0,0) to (x,y). If the modulus result where to be higher than 2147483392 (~0.995) it will give aberrant result instead - * @param x. The array of values x of each vector with size "size". input. - * @param y. The array of values y of each vector with size "size". input. - * @param out. The array where the functions puts its output, cannot be higher than 2147483392. output. - * - * The modulus function is a bit more fragile than the other functions. - * If it were to give a value higher than 2147483392, the value that it would return would be absurd. - * Avoiding to get near the limits of the euclidean space described (no more than 70% on both of the axis) is advised. - */ - static void modulus(int32_t *x, int32_t *y, int32_t *out, int32_t size); - - /** - * @brief modulus and phase function. Does both while only needing the time that one takes - * @param x. The array of values x of each vector with size "size". input. - * @param y. The array of values y of each vector with size "size". input. - * @param angle_out. The array of results of the phase, given as an angle. output. - * @param mod_out. The array of results of the modulus, with same sepcifications. output. - * - * This function uses the same trick as the cosine and sine function, as the residual result of the phase is the modulus. - * The modulus returned by this function shares weaknesses with the modulus function. - * Avoiding getting near the 70% of the maximun (~ 1518500000) on both x and y at the same time is advised, unless modulus is handled. - */ - static void phase_and_modulus(int32_t *x, int32_t *y, int32_t *angle_out, int32_t *mod_out, int32_t size); - /** - * @brief Testing function that translates the angle to a float in radians, to make reading easier. Its slow, not intended to use on continous calculations - */ - static float q31_to_radian_f32(uint32_t in); - /** - * @brief Testing function that translates radians to an angle on int format, to make writting easier. Its slow, not intended to use on continous calculations - */ - static int32_t radian_f32_to_q31(double in); + static void start(); + /** + * @brief Cosine function. Receives size angles and output size results + * @param Angle the pointer to the angle array with size "size". input + * @param Out the pointer to the array where the results are saved, with size "size". output + * @param Size the size of both pointers in indexes. + * + * The cosine queries on single shot each of the values on the angle array until size index + * If the angle or the out parameter has less size than the size parameter, it will throw an + * exception If one or both angle and out parameters have more size than size, it will just + * calculate until size and leave the rest as it is + */ + static void cos(int32_t* angle, int32_t* out, int32_t size); + + /** + * @brief sine function. virtually the same as the cosine function + */ + static void sin(int32_t* angle, int32_t* out, int32_t size); + + /** + * @brief Cosine and sine function. Does both while only needing the time that one takes + * @param Angle the pointer to the angle array with size "size". input + * @param Cos_out the pointer to the array where the cosine results are saved, with size "size". + * output + * @param Sin:out the pointer to the array where the sine results are saved, with size "size". + * output + * @param Size the size of both pointers in indexes. + * + * Uses a cosine query but configurating the CORDIC to save the residual results + * The residual results are values that appear when calculating any function, which are not the + * return of the function itself The cosine function of the CORDIC gives as residual result the + * sine function, so if you read twice you will get first cosine then sine If you don t + * configurate the CORDIC to save its residual result it will not work. The method does it by + * itself, thats why we don t just use the cosine method. The exact time it takes is around 4 + * cycles more than the cos function, which is around 30-40. So 10% more as it needs to read one + * more register. The reads are protected to give the correct result, thats why it needs 4 + * cycles for the read (on average) and not just one + */ + static void cos_and_sin(int32_t* angle, int32_t* cos_out, int32_t* sin_out, int32_t size); + + /** + * @brief Phase function. Calculates the angle between a vector from (0,0) to (x,y) and the + * vector (1,0) + * @param x. The array of values x of each vector with size "size". input. + * @param y. The array of values y of each vector with size "size". input. + * @param angle_out. The array where the functions puts its output, which is an angle. output. + */ + static void phase(int32_t* x, int32_t* y, int32_t* angle_out, int32_t size); + + /** + * @brief modulus function. Calculates the modulus of the vector from (0,0) to (x,y). If the + * modulus result where to be higher than 2147483392 (~0.995) it will give aberrant result + * instead + * @param x. The array of values x of each vector with size "size". input. + * @param y. The array of values y of each vector with size "size". input. + * @param out. The array where the functions puts its output, cannot be higher than 2147483392. + * output. + * + * The modulus function is a bit more fragile than the other functions. + * If it were to give a value higher than 2147483392, the value that it would return would be + * absurd. Avoiding to get near the limits of the euclidean space described (no more than 70% on + * both of the axis) is advised. + */ + static void modulus(int32_t* x, int32_t* y, int32_t* out, int32_t size); + + /** + * @brief modulus and phase function. Does both while only needing the time that one takes + * @param x. The array of values x of each vector with size "size". input. + * @param y. The array of values y of each vector with size "size". input. + * @param angle_out. The array of results of the phase, given as an angle. output. + * @param mod_out. The array of results of the modulus, with same sepcifications. output. + * + * This function uses the same trick as the cosine and sine function, as the residual result of + * the phase is the modulus. The modulus returned by this function shares weaknesses with the + * modulus function. Avoiding getting near the 70% of the maximun (~ 1518500000) on both x and y + * at the same time is advised, unless modulus is handled. + */ + static void + phase_and_modulus(int32_t* x, int32_t* y, int32_t* angle_out, int32_t* mod_out, int32_t size); + /** + * @brief Testing function that translates the angle to a float in radians, to make reading + * easier. Its slow, not intended to use on continous calculations + */ + static float q31_to_radian_f32(uint32_t in); + /** + * @brief Testing function that translates radians to an angle on int format, to make writting + * easier. Its slow, not intended to use on continous calculations + */ + static int32_t radian_f32_to_q31(double in); private: - /** - * @brief The mode that the cordic is configurated at this instant. Used to skip configuration if possible - */ - static Operation_Computation mode; + /** + * @brief The mode that the cordic is configurated at this instant. Used to skip configuration + * if possible + */ + static Operation_Computation mode; }; #endif diff --git a/Inc/HALAL/Services/Communication/Ethernet/LWIP/Ethernet.hpp b/Inc/HALAL/Services/Communication/Ethernet/LWIP/Ethernet.hpp index f53aee01f..f44d2ef3a 100644 --- a/Inc/HALAL/Services/Communication/Ethernet/LWIP/Ethernet.hpp +++ b/Inc/HALAL/Services/Communication/Ethernet/LWIP/Ethernet.hpp @@ -25,19 +25,17 @@ class Ethernet { public: - static bool is_ready; - static bool is_running; - - static void inscribe(); - static void start(string local_mac, string local_ip, string subnet_mask, - string gateway); - static void start(MAC local_mac, IPV4 local_ip, IPV4 subnet_mask, - IPV4 gateway); - - /** - * @brief handles the received messages by ethernet - */ - static void update(); + static bool is_ready; + static bool is_running; + + static void inscribe(); + static void start(string local_mac, string local_ip, string subnet_mask, string gateway); + static void start(MAC local_mac, IPV4 local_ip, IPV4 subnet_mask, IPV4 gateway); + + /** + * @brief handles the received messages by ethernet + */ + static void update(); private: }; diff --git a/Inc/HALAL/Services/Communication/Ethernet/LWIP/EthernetHelper.hpp b/Inc/HALAL/Services/Communication/Ethernet/LWIP/EthernetHelper.hpp index 99b1753d8..f03644333 100644 --- a/Inc/HALAL/Services/Communication/Ethernet/LWIP/EthernetHelper.hpp +++ b/Inc/HALAL/Services/Communication/Ethernet/LWIP/EthernetHelper.hpp @@ -1,10 +1,10 @@ /** * this file is used to be included inside LWIP, so it cannot have any C++ dependencies - * also declared global and extern to indicate external linkage, then lwip.c includes + * also declared global and extern to indicate external linkage, then lwip.c includes * this file and has the symbol. - * * -*/ + * + */ #pragma once #include extern bool ETH_is_cable_connected; diff --git a/Inc/HALAL/Services/Communication/Ethernet/LWIP/EthernetNode.hpp b/Inc/HALAL/Services/Communication/Ethernet/LWIP/EthernetNode.hpp index 1648ba15f..b35bd2b83 100644 --- a/Inc/HALAL/Services/Communication/Ethernet/LWIP/EthernetNode.hpp +++ b/Inc/HALAL/Services/Communication/Ethernet/LWIP/EthernetNode.hpp @@ -4,23 +4,21 @@ #ifdef HAL_ETH_MODULE_ENABLED -struct EthernetNode{ - IPV4 ip; - uint32_t port; +struct EthernetNode { + IPV4 ip; + uint32_t port; - EthernetNode(IPV4 ip, uint32_t port); + EthernetNode(IPV4 ip, uint32_t port); - bool operator==(const EthernetNode& other) const; + bool operator==(const EthernetNode& other) const; }; namespace std { - template <> - struct hash - { +template <> struct hash { std::size_t operator()(const EthernetNode& k) const; - }; +}; -} +} // namespace std #endif diff --git a/Inc/HALAL/Services/Communication/Ethernet/LWIP/TCP/ServerSocket.hpp b/Inc/HALAL/Services/Communication/Ethernet/LWIP/TCP/ServerSocket.hpp index b5d6b0ffc..424c7d0b8 100644 --- a/Inc/HALAL/Services/Communication/Ethernet/LWIP/TCP/ServerSocket.hpp +++ b/Inc/HALAL/Services/Communication/Ethernet/LWIP/TCP/ServerSocket.hpp @@ -40,188 +40,190 @@ */ class ServerSocket : public OrderProtocol { public: - enum ServerState { INACTIVE, LISTENING, ACCEPTED, CLOSING, CLOSED }; - - static unordered_map listening_sockets; - IPV4 local_ip; - uint32_t local_port; - IPV4 remote_ip; - ServerState state; - static uint8_t priority; - - struct KeepaliveConfig { - uint32_t inactivity_time_until_keepalive_ms = - TCP_INACTIVITY_TIME_UNTIL_KEEPALIVE_MS; - uint32_t space_between_tries_ms = TCP_SPACE_BETWEEN_KEEPALIVE_TRIES_MS; - uint32_t tries_until_disconnection = - TCP_KEEPALIVE_TRIES_UNTIL_DISCONNECTION; - } keepalive_config; - - ServerSocket(); - - ServerSocket(ServerSocket &&other); - - /** - * @brief ServerSocket constructor that receives the server ip on the net as a - * binary value. - * - * @param local_ip the server ip on. - * @param local_port the port number that the server listens for connections. - */ - ServerSocket(IPV4 local_ip, uint32_t local_port); - ServerSocket(IPV4 local_ip, uint32_t local_port, - uint32_t inactivity_time_until_keepalive_ms, - uint32_t space_between_tries_ms, - uint32_t tries_until_disconnection); - /** - * @brief ServerSocket constructor that uses the EthernetNode class as a - * parameter - * - * @param local_node the EthernetNode to listen to - * - * @see EthernetNode - */ - ServerSocket(EthernetNode local_node); - ~ServerSocket(); - - void operator=(ServerSocket &&other); - - /** - * @brief ends the connection between the server and the client. - */ - void close(); - - /** - * @brief saves the order data into the tx_packet_buffer so it can be sent - * when a connection is accepted - * - * @param order the order to send, which contains the data and id of the - * message - * @return true if the data could be allocated in the buffer, false otherwise - */ - bool add_order_to_queue(Order &order); - /** - * @brief puts the order data into the tx_packet_buffer and sends all the data - * in the buffer to the client - * - * @param order the order to send, which contains the data and id of the - * message - * @return true if the data was sent successfully, false otherwise - */ - bool send_order(Order &order) override { - if (state != ACCEPTED) { - return false; - } - struct memp *next_memory_pointer_in_packet_buffer_pool = - (*(memp_pools[PBUF_POOL_MEMORY_DESC_POSITION]->tab))->next; - if (next_memory_pointer_in_packet_buffer_pool == nullptr) { - if (client_control_block->unsent != nullptr) { - tcp_output(client_control_block); - } else { - memp_free_pool(memp_pools[PBUF_POOL_MEMORY_DESC_POSITION], - next_memory_pointer_in_packet_buffer_pool); - } - return false; - } - - uint8_t *order_buffer = order.build(); - if (order.get_size() > tcp_sndbuf(client_control_block)) { - return false; + enum ServerState { INACTIVE, LISTENING, ACCEPTED, CLOSING, CLOSED }; + + static unordered_map listening_sockets; + IPV4 local_ip; + uint32_t local_port; + IPV4 remote_ip; + ServerState state; + static uint8_t priority; + + struct KeepaliveConfig { + uint32_t inactivity_time_until_keepalive_ms = TCP_INACTIVITY_TIME_UNTIL_KEEPALIVE_MS; + uint32_t space_between_tries_ms = TCP_SPACE_BETWEEN_KEEPALIVE_TRIES_MS; + uint32_t tries_until_disconnection = TCP_KEEPALIVE_TRIES_UNTIL_DISCONNECTION; + } keepalive_config; + + ServerSocket(); + + ServerSocket(ServerSocket&& other); + + /** + * @brief ServerSocket constructor that receives the server ip on the net as a + * binary value. + * + * @param local_ip the server ip on. + * @param local_port the port number that the server listens for connections. + */ + ServerSocket(IPV4 local_ip, uint32_t local_port); + ServerSocket( + IPV4 local_ip, + uint32_t local_port, + uint32_t inactivity_time_until_keepalive_ms, + uint32_t space_between_tries_ms, + uint32_t tries_until_disconnection + ); + /** + * @brief ServerSocket constructor that uses the EthernetNode class as a + * parameter + * + * @param local_node the EthernetNode to listen to + * + * @see EthernetNode + */ + ServerSocket(EthernetNode local_node); + ~ServerSocket(); + + void operator=(ServerSocket&& other); + + /** + * @brief ends the connection between the server and the client. + */ + void close(); + + /** + * @brief saves the order data into the tx_packet_buffer so it can be sent + * when a connection is accepted + * + * @param order the order to send, which contains the data and id of the + * message + * @return true if the data could be allocated in the buffer, false otherwise + */ + bool add_order_to_queue(Order& order); + /** + * @brief puts the order data into the tx_packet_buffer and sends all the data + * in the buffer to the client + * + * @param order the order to send, which contains the data and id of the + * message + * @return true if the data was sent successfully, false otherwise + */ + bool send_order(Order& order) override { + if (state != ACCEPTED) { + return false; + } + struct memp* next_memory_pointer_in_packet_buffer_pool = + (*(memp_pools[PBUF_POOL_MEMORY_DESC_POSITION]->tab))->next; + if (next_memory_pointer_in_packet_buffer_pool == nullptr) { + if (client_control_block->unsent != nullptr) { + tcp_output(client_control_block); + } else { + memp_free_pool( + memp_pools[PBUF_POOL_MEMORY_DESC_POSITION], + next_memory_pointer_in_packet_buffer_pool + ); + } + return false; + } + + uint8_t* order_buffer = order.build(); + if (order.get_size() > tcp_sndbuf(client_control_block)) { + return false; + } + + struct pbuf* packet = pbuf_alloc(PBUF_TRANSPORT, order.get_size(), PBUF_POOL); + pbuf_take(packet, order_buffer, order.get_size()); + tx_packet_buffer.push(packet); + send(); + return true; } - struct pbuf *packet = - pbuf_alloc(PBUF_TRANSPORT, order.get_size(), PBUF_POOL); - pbuf_take(packet, order_buffer, order.get_size()); - tx_packet_buffer.push(packet); - send(); - return true; - } - - /** - * @brief sends all the binary data saved in the tx_packet_buffer to the - * connected client. - * - * This function is the one that actually handles outgoing communication, - * sending one by one the packets in the tx_packet_buffer The messages in the - * buffer are all immediately sent after calling this function, unless an - * error of any kind happened, in which case ErrorHandler is raised - */ - void send(); - - /** - * @brief function that returns wether or not a client is connected to the - * ServerSocket - * - * This functions returns a comparison to the state of the ServerSocket, - * checking wether or not it is on the ACCEPTED state This function is - * equivalent to doing instance->state == ServerSocket#ACCEPT - * - * @return true if a connection with the client was established, false - * otherwise - */ - bool is_connected(); + /** + * @brief sends all the binary data saved in the tx_packet_buffer to the + * connected client. + * + * This function is the one that actually handles outgoing communication, + * sending one by one the packets in the tx_packet_buffer The messages in the + * buffer are all immediately sent after calling this function, unless an + * error of any kind happened, in which case ErrorHandler is raised + */ + void send(); + + /** + * @brief function that returns wether or not a client is connected to the + * ServerSocket + * + * This functions returns a comparison to the state of the ServerSocket, + * checking wether or not it is on the ACCEPTED state This function is + * equivalent to doing instance->state == ServerSocket#ACCEPT + * + * @return true if a connection with the client was established, false + * otherwise + */ + bool is_connected(); private: - struct tcp_pcb *server_control_block = nullptr; - queue tx_packet_buffer; - queue rx_packet_buffer; - struct tcp_pcb *client_control_block; - - /** - * @brief process the data received by the client orders. It is meant to be - * called only by Lwip on the receive_callback - * - * reads all the data received by the server in the ethernet buffer, packet by - * packet. Then, for each packet, it processes it depending on the order id - * (default behavior is not process) and removes it from the buffer. This - * makes so the receive_callback (and thus the Socket) can only process - * declared orders, and ignores all other packets. - */ - void process_data(); - /** - * @brief the callback for the listener socket receiving a request for - * connection into the ServerSocket. - * - * This function is called on an interrupt when a packet containing a - * connection request to the same port of the listener socket is received. - * accept_callback builds the pcb that acts as the connection socket and saves - * it in the client_control_block pointer It then closes the listener socket - * and makes the server_control_block point to nullptr. - * - * server_control_block shouldn't be accessed in any way while the - * ServerSocket is in the ServerSocket#ACCEPT state, as it will lead into a - * nullptr exception. This in an intended behavior as there shouldn't be more - * than one listener socket on the same port, and a ServerSocket shouldn't be - * able to handle more than one connection by design. - */ - static err_t accept_callback(void *arg, - struct tcp_pcb *incomming_control_block, - err_t error); - - /** - * @brief callback that handles receiving a packet. - * - * On a received packet on an ACCEPTED state receive_callback calls the - * ServerSocket process_data, which calls the Packet process_data() if it is - * inscribed as an Order. - */ - static err_t receive_callback(void *arg, struct tcp_pcb *client_control_block, - struct pbuf *packet_buffer, err_t error); - static void error_callback(void *arg, err_t error); - - /** - * @brief callback called each 500ms to do housekeeping tasks - * - * The housekeeping tasks made now are basically checks to ensure no data - * remains unsent and that the ServerSocket is not left in a middle state - * (such as CLOSING) - */ - static err_t poll_callback(void *arg, struct tcp_pcb *client_control_block); - static err_t send_callback(void *arg, struct tcp_pcb *client_control_block, - u16_t len); - - static void config_keepalive(tcp_pcb *control_block, - ServerSocket *server_socket); + struct tcp_pcb* server_control_block = nullptr; + queue tx_packet_buffer; + queue rx_packet_buffer; + struct tcp_pcb* client_control_block; + + /** + * @brief process the data received by the client orders. It is meant to be + * called only by Lwip on the receive_callback + * + * reads all the data received by the server in the ethernet buffer, packet by + * packet. Then, for each packet, it processes it depending on the order id + * (default behavior is not process) and removes it from the buffer. This + * makes so the receive_callback (and thus the Socket) can only process + * declared orders, and ignores all other packets. + */ + void process_data(); + /** + * @brief the callback for the listener socket receiving a request for + * connection into the ServerSocket. + * + * This function is called on an interrupt when a packet containing a + * connection request to the same port of the listener socket is received. + * accept_callback builds the pcb that acts as the connection socket and saves + * it in the client_control_block pointer It then closes the listener socket + * and makes the server_control_block point to nullptr. + * + * server_control_block shouldn't be accessed in any way while the + * ServerSocket is in the ServerSocket#ACCEPT state, as it will lead into a + * nullptr exception. This in an intended behavior as there shouldn't be more + * than one listener socket on the same port, and a ServerSocket shouldn't be + * able to handle more than one connection by design. + */ + static err_t accept_callback(void* arg, struct tcp_pcb* incomming_control_block, err_t error); + + /** + * @brief callback that handles receiving a packet. + * + * On a received packet on an ACCEPTED state receive_callback calls the + * ServerSocket process_data, which calls the Packet process_data() if it is + * inscribed as an Order. + */ + static err_t receive_callback( + void* arg, + struct tcp_pcb* client_control_block, + struct pbuf* packet_buffer, + err_t error + ); + static void error_callback(void* arg, err_t error); + + /** + * @brief callback called each 500ms to do housekeeping tasks + * + * The housekeeping tasks made now are basically checks to ensure no data + * remains unsent and that the ServerSocket is not left in a middle state + * (such as CLOSING) + */ + static err_t poll_callback(void* arg, struct tcp_pcb* client_control_block); + static err_t send_callback(void* arg, struct tcp_pcb* client_control_block, u16_t len); + + static void config_keepalive(tcp_pcb* control_block, ServerSocket* server_socket); }; #endif // HAL_ETH_MODULE_ENABLED diff --git a/Inc/HALAL/Services/Communication/Ethernet/LWIP/TCP/Socket.hpp b/Inc/HALAL/Services/Communication/Ethernet/LWIP/TCP/Socket.hpp index e25d3f69e..8cb38a1e7 100644 --- a/Inc/HALAL/Services/Communication/Ethernet/LWIP/TCP/Socket.hpp +++ b/Inc/HALAL/Services/Communication/Ethernet/LWIP/TCP/Socket.hpp @@ -17,104 +17,115 @@ class Socket : public OrderProtocol { private: - tcp_pcb *connection_control_block; - tcp_pcb *socket_control_block; - queue tx_packet_buffer; - queue rx_packet_buffer; - void process_data(); - static err_t connect_callback(void *arg, struct tcp_pcb *client_control_block, - err_t error); - static err_t receive_callback(void *arg, struct tcp_pcb *client_control_block, - struct pbuf *packet_buffer, err_t error); - static err_t poll_callback(void *arg, struct tcp_pcb *client_control_block); - static err_t send_callback(void *arg, struct tcp_pcb *client_control_block, - uint16_t length); - static void error_callback(void *arg, err_t error); - - static err_t - connection_poll_callback(void *arg, struct tcp_pcb *connection_control_block); - static void connection_error_callback(void *arg, err_t error); - static void config_keepalive(tcp_pcb *control_block, Socket *socket); + tcp_pcb* connection_control_block; + tcp_pcb* socket_control_block; + queue tx_packet_buffer; + queue rx_packet_buffer; + void process_data(); + static err_t connect_callback(void* arg, struct tcp_pcb* client_control_block, err_t error); + static err_t receive_callback( + void* arg, + struct tcp_pcb* client_control_block, + struct pbuf* packet_buffer, + err_t error + ); + static err_t poll_callback(void* arg, struct tcp_pcb* client_control_block); + static err_t send_callback(void* arg, struct tcp_pcb* client_control_block, uint16_t length); + static void error_callback(void* arg, err_t error); + + static err_t connection_poll_callback(void* arg, struct tcp_pcb* connection_control_block); + static void connection_error_callback(void* arg, err_t error); + static void config_keepalive(tcp_pcb* control_block, Socket* socket); public: - enum SocketState { INACTIVE, CONNECTED, CLOSING }; - - IPV4 local_ip; - uint32_t local_port; - IPV4 remote_ip; - uint32_t remote_port; - - SocketState state; - - static unordered_map connecting_sockets; - bool pending_connection_reset = false; - bool use_keep_alives{true}; - struct KeepaliveConfig { - uint32_t inactivity_time_until_keepalive_ms = - TCP_INACTIVITY_TIME_UNTIL_KEEPALIVE_MS; - uint32_t space_between_tries_ms = TCP_SPACE_BETWEEN_KEEPALIVE_TRIES_MS; - uint32_t tries_until_disconnection = - TCP_KEEPALIVE_TRIES_UNTIL_DISCONNECTION; - } keepalive_config; - - Socket(); - Socket(Socket &&other); - Socket(IPV4 local_ip, uint32_t local_port, IPV4 remote_ip, - uint32_t remote_port, bool use_keep_alives = true); - Socket(IPV4 local_ip, uint32_t local_port, IPV4 remote_ip, - uint32_t remote_port, uint32_t inactivity_time_until_keepalive_ms, - uint32_t space_between_tries_ms, uint32_t tries_until_disconnection); - Socket(EthernetNode local_node, EthernetNode remote_node); - ~Socket(); - - void operator=(Socket &&other); - void close(); - - void reconnect(); - void reset(); - - /* - * @brief puts the order data into the tx_packet_buffer so it can be sent when - * a connection is accepted - * @return true if the data could be allocated in the buffer, false otherwise - */ - bool add_order_to_queue(Order &order); - - /* - * @brief puts the order data into the tx_packet_buffer and sends it - * @return true if the data was sent successfully, false otherwise - */ - - bool send_order(Order &order) override { - if (state != CONNECTED) { - reconnect(); - return false; + enum SocketState { INACTIVE, CONNECTED, CLOSING }; + + IPV4 local_ip; + uint32_t local_port; + IPV4 remote_ip; + uint32_t remote_port; + + SocketState state; + + static unordered_map connecting_sockets; + bool pending_connection_reset = false; + bool use_keep_alives{true}; + struct KeepaliveConfig { + uint32_t inactivity_time_until_keepalive_ms = TCP_INACTIVITY_TIME_UNTIL_KEEPALIVE_MS; + uint32_t space_between_tries_ms = TCP_SPACE_BETWEEN_KEEPALIVE_TRIES_MS; + uint32_t tries_until_disconnection = TCP_KEEPALIVE_TRIES_UNTIL_DISCONNECTION; + } keepalive_config; + + Socket(); + Socket(Socket&& other); + Socket( + IPV4 local_ip, + uint32_t local_port, + IPV4 remote_ip, + uint32_t remote_port, + bool use_keep_alives = true + ); + Socket( + IPV4 local_ip, + uint32_t local_port, + IPV4 remote_ip, + uint32_t remote_port, + uint32_t inactivity_time_until_keepalive_ms, + uint32_t space_between_tries_ms, + uint32_t tries_until_disconnection + ); + Socket(EthernetNode local_node, EthernetNode remote_node); + ~Socket(); + + void operator=(Socket&& other); + void close(); + + void reconnect(); + void reset(); + + /* + * @brief puts the order data into the tx_packet_buffer so it can be sent when + * a connection is accepted + * @return true if the data could be allocated in the buffer, false otherwise + */ + bool add_order_to_queue(Order& order); + + /* + * @brief puts the order data into the tx_packet_buffer and sends it + * @return true if the data was sent successfully, false otherwise + */ + + bool send_order(Order& order) override { + if (state != CONNECTED) { + reconnect(); + return false; + } + struct memp* next_memory_pointer_in_packet_buffer_pool = + (*(memp_pools[PBUF_POOL_MEMORY_DESC_POSITION]->tab))->next; + if (next_memory_pointer_in_packet_buffer_pool == nullptr) { + if (socket_control_block->unsent != nullptr) { + tcp_output(socket_control_block); + } else { + memp_free_pool( + memp_pools[PBUF_POOL_MEMORY_DESC_POSITION], + next_memory_pointer_in_packet_buffer_pool + ); + } + return false; + } + + uint8_t* order_buffer = order.build(); + if (order.get_size() > tcp_sndbuf(socket_control_block)) { + return false; + } + + struct pbuf* packet = pbuf_alloc(PBUF_TRANSPORT, order.get_size(), PBUF_POOL); + pbuf_take(packet, order_buffer, order.get_size()); + tx_packet_buffer.push(packet); + send(); + return true; } - struct memp *next_memory_pointer_in_packet_buffer_pool = - (*(memp_pools[PBUF_POOL_MEMORY_DESC_POSITION]->tab))->next; - if (next_memory_pointer_in_packet_buffer_pool == nullptr) { - if (socket_control_block->unsent != nullptr) { - tcp_output(socket_control_block); - } else { - memp_free_pool(memp_pools[PBUF_POOL_MEMORY_DESC_POSITION], - next_memory_pointer_in_packet_buffer_pool); - } - return false; - } - - uint8_t *order_buffer = order.build(); - if (order.get_size() > tcp_sndbuf(socket_control_block)) { - return false; - } - - struct pbuf *packet = - pbuf_alloc(PBUF_TRANSPORT, order.get_size(), PBUF_POOL); - pbuf_take(packet, order_buffer, order.get_size()); - tx_packet_buffer.push(packet); - send(); - return true; - } - void send(); - bool is_connected(); + void send(); + bool is_connected(); }; #endif diff --git a/Inc/HALAL/Services/Communication/Ethernet/LWIP/UDP/DatagramSocket.hpp b/Inc/HALAL/Services/Communication/Ethernet/LWIP/UDP/DatagramSocket.hpp index af0c62bb2..297284e40 100644 --- a/Inc/HALAL/Services/Communication/Ethernet/LWIP/UDP/DatagramSocket.hpp +++ b/Inc/HALAL/Services/Communication/Ethernet/LWIP/UDP/DatagramSocket.hpp @@ -8,39 +8,42 @@ class DatagramSocket { public: - struct udp_pcb *udp_control_block; - - IPV4 local_ip; - uint32_t local_port; - IPV4 remote_ip; - uint32_t remote_port; - bool is_disconnected = true; - DatagramSocket(); - DatagramSocket(DatagramSocket &&other); - DatagramSocket(IPV4 local_ip, uint32_t local_port, IPV4 remote_ip, - uint32_t remote_port); - DatagramSocket(EthernetNode local_node, EthernetNode remote_node); - ~DatagramSocket(); - - void operator=(DatagramSocket &&); - - void reconnect(); - - static void receive_callback(void *args, struct udp_pcb *udp_control_block, - struct pbuf *packet_buffer, - const ip_addr_t *remote_address, u16_t port); - bool send_packet(Packet &packet) { - uint8_t *packet_buffer = packet.build(); - - struct pbuf *tx_buffer = pbuf_alloc(PBUF_TRANSPORT, packet.size, PBUF_RAM); - pbuf_take(tx_buffer, packet_buffer, packet.size); - udp_send(udp_control_block, tx_buffer); - pbuf_free(tx_buffer); - - return true; - } - - void close(); + struct udp_pcb* udp_control_block; + + IPV4 local_ip; + uint32_t local_port; + IPV4 remote_ip; + uint32_t remote_port; + bool is_disconnected = true; + DatagramSocket(); + DatagramSocket(DatagramSocket&& other); + DatagramSocket(IPV4 local_ip, uint32_t local_port, IPV4 remote_ip, uint32_t remote_port); + DatagramSocket(EthernetNode local_node, EthernetNode remote_node); + ~DatagramSocket(); + + void operator=(DatagramSocket&&); + + void reconnect(); + + static void receive_callback( + void* args, + struct udp_pcb* udp_control_block, + struct pbuf* packet_buffer, + const ip_addr_t* remote_address, + u16_t port + ); + bool send_packet(Packet& packet) { + uint8_t* packet_buffer = packet.build(); + + struct pbuf* tx_buffer = pbuf_alloc(PBUF_TRANSPORT, packet.size, PBUF_RAM); + pbuf_take(tx_buffer, packet_buffer, packet.size); + udp_send(udp_control_block, tx_buffer); + pbuf_free(tx_buffer); + + return true; + } + + void close(); }; #endif diff --git a/Inc/HALAL/Services/Communication/Ethernet/NewEthernet.hpp b/Inc/HALAL/Services/Communication/Ethernet/NewEthernet.hpp index a714a87ae..dbc3b9d6c 100644 --- a/Inc/HALAL/Services/Communication/Ethernet/NewEthernet.hpp +++ b/Inc/HALAL/Services/Communication/Ethernet/NewEthernet.hpp @@ -22,7 +22,7 @@ extern ip4_addr_t ipaddr, netmask, gw; extern uint8_t IP_ADDRESS[4], NETMASK_ADDRESS[4], GATEWAY_ADDRESS[4]; namespace ST_LIB { -extern void compile_error(const char *msg); +extern void compile_error(const char* msg); #ifndef PHY_RESET_LOW_DELAY_MS #define PHY_RESET_LOW_DELAY_MS 10 @@ -33,254 +33,291 @@ extern void compile_error(const char *msg); struct EthernetDomain { - struct EthernetPins { - const GPIODomain::Pin &MDC; - const GPIODomain::Pin &REF_CLK; - const GPIODomain::Pin &MDIO; - const GPIODomain::Pin &CRS_DV; - const GPIODomain::Pin &RXD0; - const GPIODomain::Pin &RXD1; - const GPIODomain::Pin &RXER; - const GPIODomain::Pin &TXD1; - const GPIODomain::Pin &TX_EN; - const GPIODomain::Pin &TXD0; - const GPIODomain::Pin &PHY_RST; - }; - - constexpr static EthernetPins PINSET_H10{.MDC = PC1, - .REF_CLK = PA1, - .MDIO = PA2, - .CRS_DV = PA7, - .RXD0 = PC4, - .RXD1 = PC5, - .RXER = PG2, - .TXD1 = PB13, - .TX_EN = PG11, - .TXD0 = PG13, - .PHY_RST = PG0}; - constexpr static EthernetPins PINSET_H11{.MDC = PC1, - .REF_CLK = PA1, - .MDIO = PA2, - .CRS_DV = PA7, - .RXD0 = PC4, - .RXD1 = PC5, - .RXER = PG2, - .TXD1 = PB13, - .TX_EN = PB11, - .TXD0 = PB12, - .PHY_RST = PF14}; - - struct Entry { - const char *local_mac; - const char *local_ip; - const char *subnet_mask; - const char *gateway; - - size_t phy_reset_id; - }; - - struct Ethernet { - using domain = EthernetDomain; - - EthernetPins pins; - Entry e; - - std::array rmii_gpios; - DigitalOutputDomain::DigitalOutput phy_reset; - - consteval Ethernet(EthernetPins pins, const char *local_mac, - const char *local_ip, - const char *subnet_mask = "255.255.0.0", - const char *gateway = "192.168.1.1") - : pins{pins}, e{local_mac, local_ip, subnet_mask, gateway}, - rmii_gpios{ - GPIODomain::GPIO(pins.MDC, GPIODomain::OperationMode::ALT_PP, - GPIODomain::Pull::None, - GPIODomain::Speed::VeryHigh, - GPIODomain::AlternateFunction::AF11), - GPIODomain::GPIO(pins.REF_CLK, GPIODomain::OperationMode::ALT_PP, - GPIODomain::Pull::None, - GPIODomain::Speed::VeryHigh, - GPIODomain::AlternateFunction::AF11), - GPIODomain::GPIO(pins.MDIO, GPIODomain::OperationMode::ALT_OD, - GPIODomain::Pull::Up, - GPIODomain::Speed::VeryHigh, - GPIODomain::AlternateFunction::AF11), - GPIODomain::GPIO(pins.CRS_DV, GPIODomain::OperationMode::ALT_PP, - GPIODomain::Pull::None, - GPIODomain::Speed::VeryHigh, - GPIODomain::AlternateFunction::AF11), - GPIODomain::GPIO(pins.RXD0, GPIODomain::OperationMode::ALT_PP, - GPIODomain::Pull::None, - GPIODomain::Speed::VeryHigh, - GPIODomain::AlternateFunction::AF11), - GPIODomain::GPIO(pins.RXD1, GPIODomain::OperationMode::ALT_PP, - GPIODomain::Pull::None, - GPIODomain::Speed::VeryHigh, - GPIODomain::AlternateFunction::AF11), - GPIODomain::GPIO(pins.RXER, GPIODomain::OperationMode::ALT_PP, - GPIODomain::Pull::None, - GPIODomain::Speed::VeryHigh, - GPIODomain::AlternateFunction::AF11), - GPIODomain::GPIO(pins.TXD1, GPIODomain::OperationMode::ALT_PP, - GPIODomain::Pull::None, - GPIODomain::Speed::VeryHigh, - GPIODomain::AlternateFunction::AF11), - GPIODomain::GPIO(pins.TX_EN, GPIODomain::OperationMode::ALT_PP, - GPIODomain::Pull::None, - GPIODomain::Speed::VeryHigh, - GPIODomain::AlternateFunction::AF11), - GPIODomain::GPIO(pins.TXD0, GPIODomain::OperationMode::ALT_PP, - GPIODomain::Pull::None, - GPIODomain::Speed::VeryHigh, - GPIODomain::AlternateFunction::AF11)}, - phy_reset{pins.PHY_RST} {} - - template consteval std::size_t inscribe(Ctx &ctx) const { - for (const auto &gpio : rmii_gpios) { - gpio.inscribe(ctx); - } - - const auto phy_reset_id = phy_reset.inscribe(ctx); - Entry entry{ - .local_mac = this->e.local_mac, - .local_ip = this->e.local_ip, - .subnet_mask = this->e.subnet_mask, - .gateway = this->e.gateway, - .phy_reset_id = phy_reset_id, - }; - - return ctx.template add(entry, this); - } - }; - - static constexpr std::size_t max_instances{1}; - - struct Config { - const char *local_mac; - const char *local_ip; - const char *subnet_mask; - const char *gateway; - - size_t phy_reset_id; - }; - - template - static consteval array build(span config) { - array cfgs{}; - static_assert(N <= max_instances, - "EthernetDomain supports at most one instance"); - if constexpr (N == 0) { - return cfgs; - } - const auto &e = config[0]; - cfgs[0].local_mac = e.local_mac; - cfgs[0].local_ip = e.local_ip; - cfgs[0].subnet_mask = e.subnet_mask; - cfgs[0].gateway = e.gateway; - cfgs[0].phy_reset_id = e.phy_reset_id; - - return cfgs; - } - // Runtime object - struct Instance { - constexpr Instance() {} - void update() { - ethernetif_input(&gnetif); - sys_check_timeouts(); - - if (HAL_GetTick() - EthernetLinkTimer >= 100) { - EthernetLinkTimer = HAL_GetTick(); - ethernet_link_check_state(&gnetif); - - if (netif_is_link_up(&gnetif) && !netif_is_up(&gnetif)) { - netif_set_up(&gnetif); + struct EthernetPins { + const GPIODomain::Pin& MDC; + const GPIODomain::Pin& REF_CLK; + const GPIODomain::Pin& MDIO; + const GPIODomain::Pin& CRS_DV; + const GPIODomain::Pin& RXD0; + const GPIODomain::Pin& RXD1; + const GPIODomain::Pin& RXER; + const GPIODomain::Pin& TXD1; + const GPIODomain::Pin& TX_EN; + const GPIODomain::Pin& TXD0; + const GPIODomain::Pin& PHY_RST; + }; + + constexpr static EthernetPins PINSET_H10{ + .MDC = PC1, + .REF_CLK = PA1, + .MDIO = PA2, + .CRS_DV = PA7, + .RXD0 = PC4, + .RXD1 = PC5, + .RXER = PG2, + .TXD1 = PB13, + .TX_EN = PG11, + .TXD0 = PG13, + .PHY_RST = PG0 + }; + constexpr static EthernetPins PINSET_H11{ + .MDC = PC1, + .REF_CLK = PA1, + .MDIO = PA2, + .CRS_DV = PA7, + .RXD0 = PC4, + .RXD1 = PC5, + .RXER = PG2, + .TXD1 = PB13, + .TX_EN = PB11, + .TXD0 = PB12, + .PHY_RST = PF14 + }; + + struct Entry { + const char* local_mac; + const char* local_ip; + const char* subnet_mask; + const char* gateway; + + size_t phy_reset_id; + }; + + struct Ethernet { + using domain = EthernetDomain; + + EthernetPins pins; + Entry e; + + std::array rmii_gpios; + DigitalOutputDomain::DigitalOutput phy_reset; + + consteval Ethernet( + EthernetPins pins, + const char* local_mac, + const char* local_ip, + const char* subnet_mask = "255.255.0.0", + const char* gateway = "192.168.1.1" + ) + : pins{pins}, e{local_mac, local_ip, subnet_mask, gateway}, + rmii_gpios{ + GPIODomain::GPIO( + pins.MDC, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF11 + ), + GPIODomain::GPIO( + pins.REF_CLK, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF11 + ), + GPIODomain::GPIO( + pins.MDIO, + GPIODomain::OperationMode::ALT_OD, + GPIODomain::Pull::Up, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF11 + ), + GPIODomain::GPIO( + pins.CRS_DV, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF11 + ), + GPIODomain::GPIO( + pins.RXD0, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF11 + ), + GPIODomain::GPIO( + pins.RXD1, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF11 + ), + GPIODomain::GPIO( + pins.RXER, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF11 + ), + GPIODomain::GPIO( + pins.TXD1, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF11 + ), + GPIODomain::GPIO( + pins.TX_EN, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF11 + ), + GPIODomain::GPIO( + pins.TXD0, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF11 + ) + }, + phy_reset{pins.PHY_RST} {} + + template consteval std::size_t inscribe(Ctx& ctx) const { + for (const auto& gpio : rmii_gpios) { + gpio.inscribe(ctx); + } + + const auto phy_reset_id = phy_reset.inscribe(ctx); + Entry entry{ + .local_mac = this->e.local_mac, + .local_ip = this->e.local_ip, + .subnet_mask = this->e.subnet_mask, + .gateway = this->e.gateway, + .phy_reset_id = phy_reset_id, + }; + + return ctx.template add(entry, this); + } + }; + + static constexpr std::size_t max_instances{1}; + + struct Config { + const char* local_mac; + const char* local_ip; + const char* subnet_mask; + const char* gateway; + + size_t phy_reset_id; + }; + + template static consteval array build(span config) { + array cfgs{}; + static_assert(N <= max_instances, "EthernetDomain supports at most one instance"); + if constexpr (N == 0) { + return cfgs; } - } + const auto& e = config[0]; + cfgs[0].local_mac = e.local_mac; + cfgs[0].local_ip = e.local_ip; + cfgs[0].subnet_mask = e.subnet_mask; + cfgs[0].gateway = e.gateway; + cfgs[0].phy_reset_id = e.phy_reset_id; + + return cfgs; + } + // Runtime object + struct Instance { + constexpr Instance() {} + void update() { + ethernetif_input(&gnetif); + sys_check_timeouts(); + + if (HAL_GetTick() - EthernetLinkTimer >= 100) { + EthernetLinkTimer = HAL_GetTick(); + ethernet_link_check_state(&gnetif); + + if (netif_is_link_up(&gnetif) && !netif_is_up(&gnetif)) { + netif_set_up(&gnetif); + } + } + }; }; - }; - - template struct Init { - static inline std::array instances{}; - - static void init(std::span cfgs, - std::span do_instances) { - static_assert(N <= max_instances, - "EthernetDomain supports at most one instance"); - if constexpr (N == 0) { - (void)cfgs; - (void)do_instances; - return; - } - const EthernetDomain::Config &e = cfgs[0]; - - /* --- RESET PHY --- */ + + template struct Init { + static inline std::array instances{}; + + static void init( + std::span cfgs, + std::span do_instances + ) { + static_assert(N <= max_instances, "EthernetDomain supports at most one instance"); + if constexpr (N == 0) { + (void)cfgs; + (void)do_instances; + return; + } + const EthernetDomain::Config& e = cfgs[0]; + + /* --- RESET PHY --- */ #ifndef NUCLEO - // RESET_N pin low then high - do_instances[e.phy_reset_id].turn_off(); // RESET_N = 0 - HAL_Delay(PHY_RESET_LOW_DELAY_MS); - do_instances[e.phy_reset_id].turn_on(); // RESET_N = 1 - HAL_Delay(PHY_RESET_HIGH_DELAY_MS); + // RESET_N pin low then high + do_instances[e.phy_reset_id].turn_off(); // RESET_N = 0 + HAL_Delay(PHY_RESET_LOW_DELAY_MS); + do_instances[e.phy_reset_id].turn_on(); // RESET_N = 1 + HAL_Delay(PHY_RESET_HIGH_DELAY_MS); #else - // Nucleo boards typically rely on the PHY's own reset circuitry. - HAL_Delay(PHY_RESET_HIGH_DELAY_MS); + // Nucleo boards typically rely on the PHY's own reset circuitry. + HAL_Delay(PHY_RESET_HIGH_DELAY_MS); #endif - /* --- CLOCKS ETH --- */ - __HAL_RCC_SYSCFG_CLK_ENABLE(); - HAL_SYSCFG_ETHInterfaceSelect(SYSCFG_ETH_RMII); - - __HAL_RCC_ETH1MAC_CLK_ENABLE(); - __HAL_RCC_ETH1TX_CLK_ENABLE(); - __HAL_RCC_ETH1RX_CLK_ENABLE(); - - /* --- NVIC --- */ - HAL_NVIC_SetPriority(ETH_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ETH_IRQn); - - /* --- IP / MAC --- */ - MAC local_mac{e.local_mac}; - IPV4 local_ip{e.local_ip}; - IPV4 subnet_mask{e.subnet_mask}; - IPV4 gateway{e.gateway}; - - ipaddr = local_ip.address; - netmask = subnet_mask.address; - gw = gateway.address; - - IP_ADDRESS[0] = ipaddr.addr & 0xFF; - IP_ADDRESS[1] = (ipaddr.addr >> 8) & 0xFF; - IP_ADDRESS[2] = (ipaddr.addr >> 16) & 0xFF; - IP_ADDRESS[3] = (ipaddr.addr >> 24) & 0xFF; - - NETMASK_ADDRESS[0] = netmask.addr & 0xFF; - NETMASK_ADDRESS[1] = (netmask.addr >> 8) & 0xFF; - NETMASK_ADDRESS[2] = (netmask.addr >> 16) & 0xFF; - NETMASK_ADDRESS[3] = (netmask.addr >> 24) & 0xFF; - - GATEWAY_ADDRESS[0] = gw.addr & 0xFF; - GATEWAY_ADDRESS[1] = (gw.addr >> 8) & 0xFF; - GATEWAY_ADDRESS[2] = (gw.addr >> 16) & 0xFF; - GATEWAY_ADDRESS[3] = (gw.addr >> 24) & 0xFF; - - gnetif.hwaddr[0] = local_mac.address[0]; - gnetif.hwaddr[1] = local_mac.address[1]; - gnetif.hwaddr[2] = local_mac.address[2]; - gnetif.hwaddr[3] = local_mac.address[3]; - gnetif.hwaddr[4] = local_mac.address[4]; - gnetif.hwaddr[5] = local_mac.address[5]; - gnetif.hwaddr_len = 6; - - /* --- LwIP / ETH init --- */ - MX_LWIP_Init(); - netif_set_up(&gnetif); - etharp_gratuitous(&gnetif); - ::Ethernet::is_ready = true; - ::Ethernet::is_running = true; - - instances[0] = Instance{}; - } - }; + /* --- CLOCKS ETH --- */ + __HAL_RCC_SYSCFG_CLK_ENABLE(); + HAL_SYSCFG_ETHInterfaceSelect(SYSCFG_ETH_RMII); + + __HAL_RCC_ETH1MAC_CLK_ENABLE(); + __HAL_RCC_ETH1TX_CLK_ENABLE(); + __HAL_RCC_ETH1RX_CLK_ENABLE(); + + /* --- NVIC --- */ + HAL_NVIC_SetPriority(ETH_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ETH_IRQn); + + /* --- IP / MAC --- */ + MAC local_mac{e.local_mac}; + IPV4 local_ip{e.local_ip}; + IPV4 subnet_mask{e.subnet_mask}; + IPV4 gateway{e.gateway}; + + ipaddr = local_ip.address; + netmask = subnet_mask.address; + gw = gateway.address; + + IP_ADDRESS[0] = ipaddr.addr & 0xFF; + IP_ADDRESS[1] = (ipaddr.addr >> 8) & 0xFF; + IP_ADDRESS[2] = (ipaddr.addr >> 16) & 0xFF; + IP_ADDRESS[3] = (ipaddr.addr >> 24) & 0xFF; + + NETMASK_ADDRESS[0] = netmask.addr & 0xFF; + NETMASK_ADDRESS[1] = (netmask.addr >> 8) & 0xFF; + NETMASK_ADDRESS[2] = (netmask.addr >> 16) & 0xFF; + NETMASK_ADDRESS[3] = (netmask.addr >> 24) & 0xFF; + + GATEWAY_ADDRESS[0] = gw.addr & 0xFF; + GATEWAY_ADDRESS[1] = (gw.addr >> 8) & 0xFF; + GATEWAY_ADDRESS[2] = (gw.addr >> 16) & 0xFF; + GATEWAY_ADDRESS[3] = (gw.addr >> 24) & 0xFF; + + gnetif.hwaddr[0] = local_mac.address[0]; + gnetif.hwaddr[1] = local_mac.address[1]; + gnetif.hwaddr[2] = local_mac.address[2]; + gnetif.hwaddr[3] = local_mac.address[3]; + gnetif.hwaddr[4] = local_mac.address[4]; + gnetif.hwaddr[5] = local_mac.address[5]; + gnetif.hwaddr_len = 6; + + /* --- LwIP / ETH init --- */ + MX_LWIP_Init(); + netif_set_up(&gnetif); + etharp_gratuitous(&gnetif); + ::Ethernet::is_ready = true; + ::Ethernet::is_running = true; + + instances[0] = Instance{}; + } + }; }; } // namespace ST_LIB @@ -288,18 +325,19 @@ struct EthernetDomain { namespace ST_LIB { // Dummy EthernetDomain when STLIB_ETH is not defined struct EthernetDomain { - static constexpr std::size_t max_instances{0}; - struct Entry {}; - struct Config {}; - template - static consteval array build(span config) { - return {}; - } - struct Instance {}; - template struct Init { - static void init(std::span cfgs, - std::span do_instances) {}; - }; + static constexpr std::size_t max_instances{0}; + struct Entry {}; + struct Config {}; + template static consteval array build(span config) { + return {}; + } + struct Instance {}; + template struct Init { + static void init( + std::span cfgs, + std::span do_instances + ){}; + }; }; } // namespace ST_LIB #endif // STLIB_ETH diff --git a/Inc/HALAL/Services/Communication/Ethernet/PHY/phy_driver.h b/Inc/HALAL/Services/Communication/Ethernet/PHY/phy_driver.h index 2d6c83ac6..c2056b119 100644 --- a/Inc/HALAL/Services/Communication/Ethernet/PHY/phy_driver.h +++ b/Inc/HALAL/Services/Communication/Ethernet/PHY/phy_driver.h @@ -14,4 +14,4 @@ typedef struct { phy_link_state_t (*get_link_state)(void); } phy_driver_t; -extern const phy_driver_t *phy_driver; \ No newline at end of file +extern const phy_driver_t* phy_driver; \ No newline at end of file diff --git a/Inc/HALAL/Services/Communication/FDCAN/FDCAN.hpp b/Inc/HALAL/Services/Communication/FDCAN/FDCAN.hpp index 89479fa9c..f6754716a 100644 --- a/Inc/HALAL/Services/Communication/FDCAN/FDCAN.hpp +++ b/Inc/HALAL/Services/Communication/FDCAN/FDCAN.hpp @@ -13,61 +13,49 @@ #ifdef HAL_FDCAN_MODULE_ENABLED +using std::queue; using std::unordered_map; using std::vector; -using std::queue; -enum class CANBitRatesSpeed{ +enum class CANBitRatesSpeed { CAN_125_kbit = 0, CAN_250_kbit = 1, CAN_500_kbit = 2, CAN_1_Mbit = 3 }; -enum class CANFormat{ - CAN_NORMAL_FORMAT = 0, - CAN_FDCAN_FORMAT = 1 -}; -enum class CANIdentifier{ - CAN_11_BIT_IDENTIFIER = 0, - CAN_29_BIT_IDENTIFIER = 1 -}; -enum class CANMode{ - CAN_MODE_NORMAL = 0, - CAN_MODE_LOOPBACK = 4 -}; +enum class CANFormat { CAN_NORMAL_FORMAT = 0, CAN_FDCAN_FORMAT = 1 }; +enum class CANIdentifier { CAN_11_BIT_IDENTIFIER = 0, CAN_29_BIT_IDENTIFIER = 1 }; +enum class CANMode { CAN_MODE_NORMAL = 0, CAN_MODE_LOOPBACK = 4 }; -class FDCAN{ +class FDCAN { public: - enum DLC : uint32_t{ - BYTES_0 = FDCAN_DLC_BYTES_0, - BYTES_1 = FDCAN_DLC_BYTES_1, - BYTES_2 = FDCAN_DLC_BYTES_2, - BYTES_3 = FDCAN_DLC_BYTES_3, - BYTES_4 = FDCAN_DLC_BYTES_4, - BYTES_5 = FDCAN_DLC_BYTES_5, - BYTES_6 = FDCAN_DLC_BYTES_6, - BYTES_7 = FDCAN_DLC_BYTES_7, - BYTES_8 = FDCAN_DLC_BYTES_8, - BYTES_12 = FDCAN_DLC_BYTES_12, - BYTES_16 = FDCAN_DLC_BYTES_16, - BYTES_20 = FDCAN_DLC_BYTES_20, - BYTES_24 = FDCAN_DLC_BYTES_24, - BYTES_32 = FDCAN_DLC_BYTES_32, - BYTES_48 = FDCAN_DLC_BYTES_48, - BYTES_64 = FDCAN_DLC_BYTES_64, - DEFAULT = UINT32_MAX, - }; - enum ID{ - FAULT_ID = 1 + enum DLC : uint32_t { + BYTES_0 = FDCAN_DLC_BYTES_0, + BYTES_1 = FDCAN_DLC_BYTES_1, + BYTES_2 = FDCAN_DLC_BYTES_2, + BYTES_3 = FDCAN_DLC_BYTES_3, + BYTES_4 = FDCAN_DLC_BYTES_4, + BYTES_5 = FDCAN_DLC_BYTES_5, + BYTES_6 = FDCAN_DLC_BYTES_6, + BYTES_7 = FDCAN_DLC_BYTES_7, + BYTES_8 = FDCAN_DLC_BYTES_8, + BYTES_12 = FDCAN_DLC_BYTES_12, + BYTES_16 = FDCAN_DLC_BYTES_16, + BYTES_20 = FDCAN_DLC_BYTES_20, + BYTES_24 = FDCAN_DLC_BYTES_24, + BYTES_32 = FDCAN_DLC_BYTES_32, + BYTES_48 = FDCAN_DLC_BYTES_48, + BYTES_64 = FDCAN_DLC_BYTES_64, + DEFAULT = UINT32_MAX, }; + enum ID { FAULT_ID = 1 }; - struct Packet{ - array rx_data; - uint32_t identifier; - DLC data_length; - - }; + struct Packet { + array rx_data; + uint32_t identifier; + DLC data_length; + }; private: /** @@ -76,7 +64,7 @@ class FDCAN{ * predefined instances should be used. * */ - struct Instance{ + struct Instance { Pin TX; Pin RX; FDCAN_HandleTypeDef* hfdcan; @@ -89,32 +77,32 @@ class FDCAN{ vector tx_data; uint8_t fdcan_number; bool start = false; - }; + public: /** - * @brief Enum which abstracts the use of the Instance struct to facilitate the mocking of the HALAL.Struct - * - */ - enum Peripheral{ - peripheral1 = 0, - peripheral2 = 1, - peripheral3 = 2, - }; - + * @brief Enum which abstracts the use of the Instance struct to facilitate the mocking of the + * HALAL.Struct + * + */ + enum Peripheral { + peripheral1 = 0, + peripheral2 = 1, + peripheral3 = 2, + }; static uint16_t id_counter; static unordered_map registered_fdcan; static unordered_map available_fdcans; static unordered_map handle_to_fdcan; - static unordered_map instance_to_id; - static unordered_map handle_to_id; + static unordered_map instance_to_id; + static unordered_map handle_to_id; static unordered_map dlc_to_len; /** - * @brief FDCAN wrapper enum of the STM32H723. - * - */ + * @brief FDCAN wrapper enum of the STM32H723. + * + */ static FDCAN::Peripheral fdcan1; static FDCAN::Peripheral fdcan2; static FDCAN::Peripheral fdcan3; @@ -126,122 +114,126 @@ class FDCAN{ static FDCAN::Instance instance1; static FDCAN::Instance instance2; static FDCAN::Instance instance3; -template + template static uint8_t inscribe(FDCAN::Peripheral& fdcan); static void start(); - static bool transmit(uint8_t id, uint32_t message_id, const char* data, FDCAN::DLC dlc = FDCAN::DLC::DEFAULT); + static bool transmit( + uint8_t id, + uint32_t message_id, + const char* data, + FDCAN::DLC dlc = FDCAN::DLC::DEFAULT + ); static bool read(uint8_t id, FDCAN::Packet* data); /** - * @brief This method is used to check if the FDCAN have received any new packet. - * - * @param id Id of the FDCAN - * @return bool Return true if the data queue has any packet. - */ - static bool received_test(uint8_t id); + * @brief This method is used to check if the FDCAN have received any new packet. + * + * @param id Id of the FDCAN + * @return bool Return true if the data queue has any packet. + */ + static bool received_test(uint8_t id); static Packet packet; -private: +private: static void init(FDCAN::Instance* fdcan); - - - }; -template -uint8_t FDCAN::inscribe(FDCAN::Peripheral& fdcan){ - if (!FDCAN::available_fdcans.contains(fdcan)) { - ErrorHandler(" The FDCAN peripheral %d is already used or does not exists.", (uint16_t)fdcan); - return 0; - } +template +uint8_t FDCAN::inscribe(FDCAN::Peripheral& fdcan) { + if (!FDCAN::available_fdcans.contains(fdcan)) { + ErrorHandler( + " The FDCAN peripheral %d is already used or does not exists.", + (uint16_t)fdcan + ); + return 0; + } - FDCAN::Instance* fdcan_instance = FDCAN::available_fdcans[fdcan]; - if constexpr(format == CANFormat::CAN_FDCAN_FORMAT){ + FDCAN::Instance* fdcan_instance = FDCAN::available_fdcans[fdcan]; + if constexpr (format == CANFormat::CAN_FDCAN_FORMAT) { fdcan_instance->tx_header.FDFormat = FDCAN_FD_CAN; - }else{ + } else { fdcan_instance->tx_header.FDFormat = FDCAN_CLASSIC_CAN; } - fdcan_instance->tx_header.DataLength = fdcan_instance->dlc; - fdcan_instance->tx_header.TxFrameType = FDCAN_DATA_FRAME; - fdcan_instance->tx_header.ErrorStateIndicator = FDCAN_ESI_ACTIVE; + fdcan_instance->tx_header.DataLength = fdcan_instance->dlc; + fdcan_instance->tx_header.TxFrameType = FDCAN_DATA_FRAME; + fdcan_instance->tx_header.ErrorStateIndicator = FDCAN_ESI_ACTIVE; fdcan_instance->tx_header.BitRateSwitch = FDCAN_BRS_OFF; - if constexpr(message_id == CANIdentifier::CAN_29_BIT_IDENTIFIER){ + if constexpr (message_id == CANIdentifier::CAN_29_BIT_IDENTIFIER) { fdcan_instance->tx_header.IdType = FDCAN_EXTENDED_ID; fdcan_instance->hfdcan->Init.FrameFormat = FDCAN_FRAME_FD_NO_BRS; - }else{ + } else { fdcan_instance->tx_header.IdType = FDCAN_STANDARD_ID; fdcan_instance->hfdcan->Init.FrameFormat = FDCAN_FRAME_CLASSIC; } - fdcan_instance->tx_header.TxEventFifoControl = FDCAN_NO_TX_EVENTS; - fdcan_instance->tx_header.MessageMarker = 0; - fdcan_instance->tx_header.Identifier = 0x0; - - fdcan_instance->hfdcan->Instance = fdcan_instance->instance; -// use NORMAL or EXTERNAL_LOOPBACK mode - fdcan_instance->hfdcan->Init.Mode = static_cast(mode); - fdcan_instance->hfdcan->Init.AutoRetransmission = DISABLE; - fdcan_instance->hfdcan->Init.TransmitPause = DISABLE; - fdcan_instance->hfdcan->Init.ProtocolException = DISABLE; -/////////////////////////////////////////////////////////// - if constexpr(Speed == CANBitRatesSpeed::CAN_125_kbit){ + fdcan_instance->tx_header.TxEventFifoControl = FDCAN_NO_TX_EVENTS; + fdcan_instance->tx_header.MessageMarker = 0; + fdcan_instance->tx_header.Identifier = 0x0; + + fdcan_instance->hfdcan->Instance = fdcan_instance->instance; + // use NORMAL or EXTERNAL_LOOPBACK mode + fdcan_instance->hfdcan->Init.Mode = static_cast(mode); + fdcan_instance->hfdcan->Init.AutoRetransmission = DISABLE; + fdcan_instance->hfdcan->Init.TransmitPause = DISABLE; + fdcan_instance->hfdcan->Init.ProtocolException = DISABLE; + /////////////////////////////////////////////////////////// + if constexpr (Speed == CANBitRatesSpeed::CAN_125_kbit) { fdcan_instance->hfdcan->Init.NominalPrescaler = 20; fdcan_instance->hfdcan->Init.NominalSyncJumpWidth = 2; fdcan_instance->hfdcan->Init.NominalTimeSeg1 = 5; fdcan_instance->hfdcan->Init.NominalTimeSeg2 = 2; - }else if constexpr(Speed == CANBitRatesSpeed::CAN_250_kbit){ + } else if constexpr (Speed == CANBitRatesSpeed::CAN_250_kbit) { fdcan_instance->hfdcan->Init.NominalPrescaler = 10; fdcan_instance->hfdcan->Init.NominalSyncJumpWidth = 2; fdcan_instance->hfdcan->Init.NominalTimeSeg1 = 5; fdcan_instance->hfdcan->Init.NominalTimeSeg2 = 2; - }else if constexpr(Speed == CANBitRatesSpeed::CAN_500_kbit){ + } else if constexpr (Speed == CANBitRatesSpeed::CAN_500_kbit) { fdcan_instance->hfdcan->Init.NominalPrescaler = 5; fdcan_instance->hfdcan->Init.NominalSyncJumpWidth = 2; fdcan_instance->hfdcan->Init.NominalTimeSeg1 = 5; fdcan_instance->hfdcan->Init.NominalTimeSeg2 = 2; - }else if constexpr(Speed == CANBitRatesSpeed::CAN_1_Mbit){ + } else if constexpr (Speed == CANBitRatesSpeed::CAN_1_Mbit) { fdcan_instance->hfdcan->Init.NominalPrescaler = 1; fdcan_instance->hfdcan->Init.NominalSyncJumpWidth = 4; fdcan_instance->hfdcan->Init.NominalTimeSeg1 = 15; fdcan_instance->hfdcan->Init.NominalTimeSeg2 = 4; } -//////////////////////////////////////////////////////////// - fdcan_instance->hfdcan->Init.DataPrescaler = 11; - fdcan_instance->hfdcan->Init.DataSyncJumpWidth = 4; - fdcan_instance->hfdcan->Init.DataTimeSeg1 = 17; - fdcan_instance->hfdcan->Init.DataTimeSeg2 = 8; - fdcan_instance->hfdcan->Init.MessageRAMOffset = 0; - fdcan_instance->hfdcan->Init.StdFiltersNbr = 0; - fdcan_instance->hfdcan->Init.ExtFiltersNbr = 0; - fdcan_instance->hfdcan->Init.RxFifo0ElmtsNbr = 16; - fdcan_instance->hfdcan->Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_64; - fdcan_instance->hfdcan->Init.RxFifo1ElmtsNbr = 0; - fdcan_instance->hfdcan->Init.RxFifo1ElmtSize = FDCAN_DATA_BYTES_64; - fdcan_instance->hfdcan->Init.RxBuffersNbr = 0; - fdcan_instance->hfdcan->Init.RxBufferSize = FDCAN_DATA_BYTES_64; - fdcan_instance->hfdcan->Init.TxEventsNbr = 0; - fdcan_instance->hfdcan->Init.TxBuffersNbr = 0; - fdcan_instance->hfdcan->Init.TxFifoQueueElmtsNbr = 16; - fdcan_instance->hfdcan->Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION; - - if constexpr(format == CANFormat::CAN_NORMAL_FORMAT){ + //////////////////////////////////////////////////////////// + fdcan_instance->hfdcan->Init.DataPrescaler = 11; + fdcan_instance->hfdcan->Init.DataSyncJumpWidth = 4; + fdcan_instance->hfdcan->Init.DataTimeSeg1 = 17; + fdcan_instance->hfdcan->Init.DataTimeSeg2 = 8; + fdcan_instance->hfdcan->Init.MessageRAMOffset = 0; + fdcan_instance->hfdcan->Init.StdFiltersNbr = 0; + fdcan_instance->hfdcan->Init.ExtFiltersNbr = 0; + fdcan_instance->hfdcan->Init.RxFifo0ElmtsNbr = 16; + fdcan_instance->hfdcan->Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_64; + fdcan_instance->hfdcan->Init.RxFifo1ElmtsNbr = 0; + fdcan_instance->hfdcan->Init.RxFifo1ElmtSize = FDCAN_DATA_BYTES_64; + fdcan_instance->hfdcan->Init.RxBuffersNbr = 0; + fdcan_instance->hfdcan->Init.RxBufferSize = FDCAN_DATA_BYTES_64; + fdcan_instance->hfdcan->Init.TxEventsNbr = 0; + fdcan_instance->hfdcan->Init.TxBuffersNbr = 0; + fdcan_instance->hfdcan->Init.TxFifoQueueElmtsNbr = 16; + fdcan_instance->hfdcan->Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION; + + if constexpr (format == CANFormat::CAN_NORMAL_FORMAT) { fdcan_instance->hfdcan->Init.TxElmtSize = FDCAN_DATA_BYTES_8; - } - else{ + } else { fdcan_instance->hfdcan->Init.TxElmtSize = FDCAN_DATA_BYTES_64; } - Pin::inscribe(fdcan_instance->TX, ALTERNATIVE); - Pin::inscribe(fdcan_instance->RX, ALTERNATIVE); + Pin::inscribe(fdcan_instance->TX, ALTERNATIVE); + Pin::inscribe(fdcan_instance->RX, ALTERNATIVE); - uint8_t id = FDCAN::id_counter++; + uint8_t id = FDCAN::id_counter++; - FDCAN::registered_fdcan[id] = fdcan_instance; + FDCAN::registered_fdcan[id] = fdcan_instance; - return id; + return id; } #endif diff --git a/Inc/HALAL/Services/Communication/I2C/I2C.hpp b/Inc/HALAL/Services/Communication/I2C/I2C.hpp index 68a05da68..4817d98d4 100644 --- a/Inc/HALAL/Services/Communication/I2C/I2C.hpp +++ b/Inc/HALAL/Services/Communication/I2C/I2C.hpp @@ -18,137 +18,137 @@ /** * @brief I2C service class. Abstracts the use of the UART service of the HAL library. - * + * */ class I2C { private: - /** - * @brief Struct which defines all data referring to I2C peripherals. It is - * declared private in order to prevent unwanted use. Only - * predefined instances should be used. - * - */ - struct Instance { - Pin &SCL; - Pin &SDA; - I2C_HandleTypeDef *hi2c; - I2C_TypeDef *instance; - DMA::Stream RX_DMA; - DMA::Stream TX_DMA; - uint8_t address = 0; - uint32_t speed_frequency_kHz = 100; - uint32_t data_length_in_bits = 8; - bool is_receive_ready = false; - bool is_initialized = false; - - }; + /** + * @brief Struct which defines all data referring to I2C peripherals. It is + * declared private in order to prevent unwanted use. Only + * predefined instances should be used. + * + */ + struct Instance { + Pin& SCL; + Pin& SDA; + I2C_HandleTypeDef* hi2c; + I2C_TypeDef* instance; + DMA::Stream RX_DMA; + DMA::Stream TX_DMA; + uint8_t address = 0; + uint32_t speed_frequency_kHz = 100; + uint32_t data_length_in_bits = 8; + bool is_receive_ready = false; + bool is_initialized = false; + }; public: - /** - * @brief Enum which abstracts the use of the Instance struct to facilitate the mocking of the HALAL.Struct - * - */ - enum Peripheral { - peripheral2 = 2, - }; + /** + * @brief Enum which abstracts the use of the Instance struct to facilitate the mocking of the + * HALAL.Struct + * + */ + enum Peripheral { + peripheral2 = 2, + }; public: - static uint16_t id_counter; - - static unordered_map active_i2c; - - static unordered_map available_i2cs; - - static unordered_map available_speed_frequencies; - - /** - * @brief I2C 2 wrapper enum of the STM32H723. - * - */ - static I2C::Peripheral i2c2; - - static DMA_HandleTypeDef hdma_i2c2_rx; - static DMA_HandleTypeDef hdma_i2c2_tx; - - /** - * @brief I2C 2 instance of the STM32H723. - * - */ - static I2C::Instance instance2; - - /** - * @brief Registers a new I2C. - * - * @param uart I2C peripheral to register. - * @return uint8_t Id of the service. - */ - static uint8_t inscribe(I2C::Peripheral &i2c); - static uint8_t inscribe(I2C::Peripheral &i2c, uint8_t address); - - /** - * @brief This method initializes all registered I2Cs. The peripherals - * must be enrolled before calling this method. - * - */ - static void start(); - - /**@brief Transmits 1 RawPacket of any size by DMAs. - * Handles the packet size automatically. - * @param id Id of the I2C - * @param packet Packet to be send - * @return bool Returns true if the request to send the packet has been done - * successfully. Returns false if the I2C is busy or a problem - * has occurred. - */ - static bool transmit_next_packet(uint8_t id, I2CPacket &packet); - static bool transmit_next_packet_polling(uint8_t id, I2CPacket &packet); - - /** - * @brief This method request the receive of a new RawPacket of any size by DMA. - * - * @param id Id of the I2C - * @param packet RawPacket in which the data will be stored - * @return bool Return true if the order to receive a new packet has been - * processed correctly. Return false if the I2C is busy or a - * problem has occurred. - */ - static bool receive_next_packet(uint8_t id, I2CPacket &packet); - static bool receive_next_packet_polling(uint8_t id, I2CPacket &packet); - - static bool read_from(uint8_t id, I2CPacket &packet, uint16_t mem_addr, - uint16_t mem_size); - - static bool write_to(uint8_t id, I2CPacket &packet, uint16_t mem_addr, - uint16_t mem_size); - - /** - * @brief This method is used to check if the UART receive operation has finished and data is ready. - * - * @param id Id of the UART - * @return bool Return true if the packet is ready to use and false if not. - */ - static bool has_next_packet(uint8_t id); - - /** - * @brief This method is used to check if the UART transmit operations busy. - * - * @param id Id of the UART - * @return bool Return true if the UART transmit operation is busy and false if not. - */ - static bool is_busy(uint8_t id); - - /** - * @brief This method resets the i2c handler entirely so a board can recover from a communication fail without a full reset - */ - static void reset(uint8_t id); + static uint16_t id_counter; + + static unordered_map active_i2c; + + static unordered_map available_i2cs; + + static unordered_map available_speed_frequencies; + + /** + * @brief I2C 2 wrapper enum of the STM32H723. + * + */ + static I2C::Peripheral i2c2; + + static DMA_HandleTypeDef hdma_i2c2_rx; + static DMA_HandleTypeDef hdma_i2c2_tx; + + /** + * @brief I2C 2 instance of the STM32H723. + * + */ + static I2C::Instance instance2; + + /** + * @brief Registers a new I2C. + * + * @param uart I2C peripheral to register. + * @return uint8_t Id of the service. + */ + static uint8_t inscribe(I2C::Peripheral& i2c); + static uint8_t inscribe(I2C::Peripheral& i2c, uint8_t address); + + /** + * @brief This method initializes all registered I2Cs. The peripherals + * must be enrolled before calling this method. + * + */ + static void start(); + + /**@brief Transmits 1 RawPacket of any size by DMAs. + * Handles the packet size automatically. + * @param id Id of the I2C + * @param packet Packet to be send + * @return bool Returns true if the request to send the packet has been done + * successfully. Returns false if the I2C is busy or a problem + * has occurred. + */ + static bool transmit_next_packet(uint8_t id, I2CPacket& packet); + static bool transmit_next_packet_polling(uint8_t id, I2CPacket& packet); + + /** + * @brief This method request the receive of a new RawPacket of any size by DMA. + * + * @param id Id of the I2C + * @param packet RawPacket in which the data will be stored + * @return bool Return true if the order to receive a new packet has been + * processed correctly. Return false if the I2C is busy or a + * problem has occurred. + */ + static bool receive_next_packet(uint8_t id, I2CPacket& packet); + static bool receive_next_packet_polling(uint8_t id, I2CPacket& packet); + + static bool read_from(uint8_t id, I2CPacket& packet, uint16_t mem_addr, uint16_t mem_size); + + static bool write_to(uint8_t id, I2CPacket& packet, uint16_t mem_addr, uint16_t mem_size); + + /** + * @brief This method is used to check if the UART receive operation has finished and data is + * ready. + * + * @param id Id of the UART + * @return bool Return true if the packet is ready to use and false if not. + */ + static bool has_next_packet(uint8_t id); + + /** + * @brief This method is used to check if the UART transmit operations busy. + * + * @param id Id of the UART + * @return bool Return true if the UART transmit operation is busy and false if not. + */ + static bool is_busy(uint8_t id); + + /** + * @brief This method resets the i2c handler entirely so a board can recover from a + * communication fail without a full reset + */ + static void reset(uint8_t id); private: - /** - * @brief This method initializes the UART peripheral that is passed to it as a parameter. - * - * @param uart Peripheral instance to be initialized. - */ - static void init(I2C::Instance *i2c); + /** + * @brief This method initializes the UART peripheral that is passed to it as a parameter. + * + * @param uart Peripheral instance to be initialized. + */ + static void init(I2C::Instance* i2c); }; #endif \ No newline at end of file diff --git a/Inc/HALAL/Services/Communication/SNTP/SNTP.hpp b/Inc/HALAL/Services/Communication/SNTP/SNTP.hpp index 870b8d8c6..987b09e79 100644 --- a/Inc/HALAL/Services/Communication/SNTP/SNTP.hpp +++ b/Inc/HALAL/Services/Communication/SNTP/SNTP.hpp @@ -14,8 +14,12 @@ class SNTP { public: - static void sntp_update(uint8_t address_head, uint8_t address_second, uint8_t address_third, uint8_t address_last); - static void sntp_update(string ip); - static void sntp_update(); - + static void sntp_update( + uint8_t address_head, + uint8_t address_second, + uint8_t address_third, + uint8_t address_last + ); + static void sntp_update(string ip); + static void sntp_update(); }; diff --git a/Inc/HALAL/Services/Communication/SPI/SPI.hpp b/Inc/HALAL/Services/Communication/SPI/SPI.hpp index 4770e63de..e0c26f0be 100644 --- a/Inc/HALAL/Services/Communication/SPI/SPI.hpp +++ b/Inc/HALAL/Services/Communication/SPI/SPI.hpp @@ -16,90 +16,98 @@ #ifdef HAL_SPI_MODULE_ENABLED -#define MASTER_SPI_CHECK_DELAY 100000 //how often the master should check if the slave is ready, in nanoseconds +#define MASTER_SPI_CHECK_DELAY \ + 100000 // how often the master should check if the slave is ready, in nanoseconds #define MASTER_MAXIMUM_QUEUE_LEN 10 -//TODO: Hay que hacer el Chip select funcione a traves de un GPIO en vez de a traves del periferico. +// TODO: Hay que hacer el Chip select funcione a traves de un GPIO en vez de a traves del +// periferico. /** * @brief SPI service class. Abstracts the use of the SPI service of the HAL library. - * + * */ -class SPI{ +class SPI { public: - /** - * @brief Possible states of the SPI class - */ - enum SPIstate{ - IDLE = 0, - STARTING_ORDER, - WAITING_ORDER, - PROCESSING_ORDER, - ERROR_RECOVERY, + /** + * @brief Possible states of the SPI class + */ + enum SPIstate { + IDLE = 0, + STARTING_ORDER, + WAITING_ORDER, + PROCESSING_ORDER, + ERROR_RECOVERY, }; /** * @brief Struct which defines all data referring to SPI peripherals. DO NOT MODIFY ON RUN TIME. * declared public so HAL callbacks can directly access the instance - * NOT intended to be modified after the inscribe of the spi, configuration should be made in Runes.hpp - * + * NOT intended to be modified after the inscribe of the spi, configuration should be made in + * Runes.hpp + * */ - struct Instance{ - Pin* SCK; /**< Clock pin. */ + struct Instance { + Pin* SCK; /**< Clock pin. */ Pin* MOSI; /**< MOSI pin. */ Pin* MISO; /**< MISO pin. */ - Pin* SS; /**< Slave select pin. */ - Pin* RS; /**< Ready Slave pin (optional)*/ + Pin* SS; /**< Slave select pin. */ + Pin* RS; /**< Ready Slave pin (optional)*/ uint8_t RShandler; - SPI_HandleTypeDef* hspi; /**< HAL spi struct pin. */ - SPI_TypeDef* instance; /**< HAL spi instance. */ - DMA::Stream hdma_tx; /**< HAL DMA handler for writting */ - DMA::Stream hdma_rx; /**< HAL DMA handler for reading */ - - uint32_t baud_rate_prescaler; /**< SPI baud prescaler.*/ - uint32_t mode = SPI_MODE_MASTER; /**< SPI mode. */ - uint32_t data_size = SPI_DATASIZE_8BIT; /**< SPI data size. Default 8 bit */ - uint32_t first_bit = SPI_FIRSTBIT_MSB; /**< SPI first bit,. */ - uint32_t clock_polarity = SPI_POLARITY_LOW; /**< SPI clock polarity. */ - uint32_t clock_phase = SPI_PHASE_1EDGE; /**< SPI clock phase. */ + SPI_HandleTypeDef* hspi; /**< HAL spi struct pin. */ + SPI_TypeDef* instance; /**< HAL spi instance. */ + DMA::Stream hdma_tx; /**< HAL DMA handler for writting */ + DMA::Stream hdma_rx; /**< HAL DMA handler for reading */ + + uint32_t baud_rate_prescaler; /**< SPI baud prescaler.*/ + uint32_t mode = SPI_MODE_MASTER; /**< SPI mode. */ + uint32_t data_size = SPI_DATASIZE_8BIT; /**< SPI data size. Default 8 bit */ + uint32_t first_bit = SPI_FIRSTBIT_MSB; /**< SPI first bit,. */ + uint32_t clock_polarity = SPI_POLARITY_LOW; /**< SPI clock polarity. */ + uint32_t clock_phase = SPI_PHASE_1EDGE; /**< SPI clock phase. */ uint32_t nss_polarity = SPI_NSS_POLARITY_LOW; /**< SPI chip select polarity. */ - bool initialized = false; /**< Peripheral has already been initialized */ bool use_DMA = false; bool using_ready_slave = false; string name; SPIstate state = IDLE; /**< State of the spi on the Order communication*/ - uint8_t *rx_buffer; - uint8_t *tx_buffer; - uint16_t *available_end; /**< variable that checks for what Order id is the other end ready*/ - uint16_t *SPIOrderID; /**< SPIOrder being processed, if any*/ - RingBuffer SPIOrderQueue; /**< Queue of SPIOrders to process after this one*/ + uint8_t* rx_buffer; + uint8_t* tx_buffer; + uint16_t* + available_end; /**< variable that checks for what Order id is the other end ready*/ + uint16_t* SPIOrderID; /**< SPIOrder being processed, if any*/ + RingBuffer + SPIOrderQueue; /**< Queue of SPIOrders to process after this one*/ uint64_t last_end_check = 0; /**< last clock cycle where the available end was checked*/ - uint64_t Order_count = 0; /**< Order completed counter for debugging (success rate)*/ - uint64_t try_count = 0; /**< Tries from the master to communicate a packet with the slave for debugging (affected by how much the slave delays on preparing a packet)*/ - uint64_t error_count = 0; /**< Order error counter for debugging (affected by how much the bits sent are corrupted in any way)*/ + uint64_t Order_count = 0; /**< Order completed counter for debugging (success rate)*/ + uint64_t try_count = + 0; /**< Tries from the master to communicate a packet with the slave for debugging + (affected by how much the slave delays on preparing a packet)*/ + uint64_t error_count = 0; /**< Order error counter for debugging (affected by how much the + bits sent are corrupted in any way)*/ }; /** - * @brief Enum that abstracts the use of the Instance struct to facilitate the mocking of the HALAL. + * @brief Enum that abstracts the use of the Instance struct to facilitate the mocking of the + * HALAL. * */ - enum Peripheral{ - peripheral1 = 0, - peripheral2 = 1, + enum Peripheral { + peripheral1 = 0, + peripheral2 = 1, peripheral3 = 2, - peripheral4 = 3, - peripheral5 = 4, - peripheral6 = 5, - }; + peripheral4 = 3, + peripheral5 = 4, + peripheral6 = 5, + }; static uint16_t id_counter; - - static map registered_spi; + + static map registered_spi; static unordered_map available_spi; @@ -120,13 +128,11 @@ class SPI{ static SPI::Peripheral spi6; static SPI::Instance instance1; - static SPI::Instance instance2; - static SPI::Instance instance3; - static SPI::Instance instance4; - static SPI::Instance instance5; - static SPI::Instance instance6; - - + static SPI::Instance instance2; + static SPI::Instance instance3; + static SPI::Instance instance4; + static SPI::Instance instance5; + static SPI::Instance instance6; /*========================================= * User functions for configuration of the SPI @@ -134,13 +140,12 @@ class SPI{ /** * @brief Registers a new SPI. - * + * * @param spi SPI peripheral to register. * @return uint8_t Id of the service. */ static uint8_t inscribe(SPI::Peripheral& spi); - /** * @brief assigns the RS pin to the SPI. */ @@ -153,14 +158,12 @@ class SPI{ */ static void start(); - - /*========================================= * User functions for dummy mode ==========================================*/ /**@brief Transmits 1 byte by SPI. - * + * * @param id Id of the SPI * @param data Data to be send * @return bool Returns true if the data has been send successfully. @@ -169,29 +172,29 @@ class SPI{ static bool transmit(uint8_t id, uint8_t data); /**@brief Transmits size bytes by SPI. - * - * @param id Id of the SPI - * @param data Data to be send - * @param size Size in bytes to be send - * @return bool Returns true if the data has been send successfully. - * Returns false if a problem has occurred. - */ + * + * @param id Id of the SPI + * @param data Data to be send + * @param size Size in bytes to be send + * @return bool Returns true if the data has been send successfully. + * Returns false if a problem has occurred. + */ static bool transmit(uint8_t id, span data); /**@brief Transmits size bytes by SPI via DMA - * - * @param id Id of the SPI - * @param data Data to be send - * @param size Size in bytes to be send - * @return bool Returns true if the data has been send successfully. - * Returns false if a problem has occurred. - */ + * + * @param id Id of the SPI + * @param data Data to be send + * @param size Size in bytes to be send + * @return bool Returns true if the data has been send successfully. + * Returns false if a problem has occurred. + */ static bool transmit_DMA(uint8_t id, span data); - /** + /** * @brief This method requests an array of data. The data * will be stored in data parameter. You must make sure you have * enough space - * + * * @param id Id of the SPI * @param data Pointer where data will be stored * @param size Size in bytes to receive. @@ -199,11 +202,11 @@ class SPI{ * Returns false if a problem has occurred. */ static bool receive(uint8_t id, span data); - /** + /** * @brief This method requests an array of data. The data * will be stored in data parameter. You must make sure you have * enough space, this function uses DMA - * + * * @param id Id of the SPI * @param data Pointer where data will be stored * @param size Size in bytes to receive. @@ -213,20 +216,19 @@ class SPI{ static bool receive_DMA(uint8_t id, span data); /** - * @brief This method transmits one order of command_size bytes and - * then stores the data received after that order. - * - * @param id Id of the SPI - * @param command_data Command - * @param command_size Command size in bytes to receive - * @param receive_data Pointer where data will be stored - * @param receive_size Number of bytes to read - * @return bool Returns true if the data have been read successfully. - * Returns false if a problem has occurred. - */ - static bool transmit_and_receive(uint8_t id, span command_data, span receive_data); - - + * @brief This method transmits one order of command_size bytes and + * then stores the data received after that order. + * + * @param id Id of the SPI + * @param command_data Command + * @param command_size Command size in bytes to receive + * @param receive_data Pointer where data will be stored + * @param receive_size Number of bytes to read + * @return bool Returns true if the data have been read successfully. + * Returns false if a problem has occurred. + */ + static bool + transmit_and_receive(uint8_t id, span command_data, span receive_data); /*============================================= * User functions for Order mode @@ -235,53 +237,59 @@ class SPI{ /** * @brief master send Order method, which tries to send a single Order */ - static bool master_transmit_Order(uint8_t id, SPIBaseOrder &Order); - static bool master_transmit_Order(uint8_t id, SPIBaseOrder *Order); + static bool master_transmit_Order(uint8_t id, SPIBaseOrder& Order); + static bool master_transmit_Order(uint8_t id, SPIBaseOrder* Order); /** - * @brief slave listen Orders method. When called, the slave will start to listen Orders until the state is set again to IDLE + * @brief slave listen Orders method. When called, the slave will start to listen Orders until + * the state is set again to IDLE */ static void slave_listen_Orders(uint8_t id); /** - * @brief This method transmits one order of command_size bytes and - * then stores the data received after that order using DMA - * - * @param id Id of the SPI - * @param command_data Command - * @param command_size Command size in bytes to receive - * @param receive_data Pointer where data will be stored - * @param receive_size Number of bytes to read - * @return bool Returns true if the data have been read successfully. - * Returns false if a problem has occurred. - */ - static bool transmit_and_receive_DMA(uint8_t id, span command_data, span receive_data); + * @brief This method transmits one order of command_size bytes and + * then stores the data received after that order using DMA + * + * @param id Id of the SPI + * @param command_data Command + * @param command_size Command size in bytes to receive + * @param receive_data Pointer where data will be stored + * @param receive_size Number of bytes to read + * @return bool Returns true if the data have been read successfully. + * Returns false if a problem has occurred. + */ + static bool + transmit_and_receive_DMA(uint8_t id, span command_data, span receive_data); /** - * @brief method that needs to be called periodically by the master code for proper communication. Can be called at any speed, but the faster it is called, the faster the Orders will resolve. + * @brief method that needs to be called periodically by the master code for proper + * communication. Can be called at any speed, but the faster it is called, the faster the Orders + * will resolve. * - * this update has to be called by the code in order for master to check if the slave is ready to send the Order. - * If it is not called periodically, the master_transmit_Order will not work. Not needed for dummy communication (not using Orders) + * this update has to be called by the code in order for master to check if the slave is ready + * to send the Order. If it is not called periodically, the master_transmit_Order will not work. + * Not needed for dummy communication (not using Orders) */ static void Order_update(); - - /*============================================= * SPI Module Functions for HAL (cannot be private) ==============================================*/ /** - * @brief master Order that checks the state of the slave, used on the callback of the SPI module and handled automatically by Order_update method (as long as it is called). + * @brief master Order that checks the state of the slave, used on the callback of the SPI + * module and handled automatically by Order_update method (as long as it is called). */ static void master_check_available_end(SPI::Instance* spi); /** - * @brief slave updates the information on its buffer to check what ID is master asking on next query + * @brief slave updates the information on its buffer to check what ID is master asking on next + * query */ static void slave_check_packet_ID(SPI::Instance* spi); /** - * @brief This method sets chip select to high level, and is used automatically by the SPI module + * @brief This method sets chip select to high level, and is used automatically by the SPI + * module * * The high level signal on SPI on chip select marks a slave to not process anything. * For more information read the chip_select_off method. @@ -291,37 +299,45 @@ class SPI{ static void chip_select_on(uint8_t id); /** - * @brief This method sets chip select to low level, and is used automatically by the SPI module. - * - * The low level signal on SPI on chip select marks the slave connected to it to start processing data, and is marked by the master - * The slave on the ST-LIB doesn t use this configuration yet and is instead active all the time, so for now it can only function as a single point to point configuration - * to have multiple ST-LIB SPI slaves on a single SPI interface you may need to implement an EXTI on the slave that deactivates it while its on. - * This method is still used to communicate to any slave that is not coded with the ST-LIB, as those follow the common rules of the SPI - * - * @param spi Id of the SPI - */ + * @brief This method sets chip select to low level, and is used automatically by the SPI + * module. + * + * The low level signal on SPI on chip select marks the slave connected to it to start + * processing data, and is marked by the master The slave on the ST-LIB doesn t use this + * configuration yet and is instead active all the time, so for now it can only function as a + * single point to point configuration to have multiple ST-LIB SPI slaves on a single SPI + * interface you may need to implement an EXTI on the slave that deactivates it while its on. + * This method is still used to communicate to any slave that is not coded with the ST-LIB, as + * those follow the common rules of the SPI + * + * @param spi Id of the SPI + */ static void chip_select_off(uint8_t id); - static inline void spi_communicate_order_data(SPI::Instance* spi, uint8_t* value_to_send, uint8_t* value_to_receive, uint16_t size_to_send); - + static inline void spi_communicate_order_data( + SPI::Instance* spi, + uint8_t* value_to_send, + uint8_t* value_to_receive, + uint16_t size_to_send + ); static void turn_on_chip_select(SPI::Instance* spi); static void turn_off_chip_select(SPI::Instance* spi); /* - * @brief returns true if its KNOWN that the slave has the packet ready, and false if it is NOT KNOWN + * @brief returns true if its KNOWN that the slave has the packet ready, and false if it is NOT + * KNOWN * - * This function uses the optional RS to know if the slave is ready, and always returns false if the RS is not used. + * This function uses the optional RS to know if the slave is ready, and always returns false if + * the RS is not used. */ static bool known_slave_ready(SPI::Instance* spi); - /** * @brief function used by slave to signal using RS that it has the message ready */ static void mark_slave_ready(SPI::Instance* spi); - /** * @brief function used by slave to signal using RS that it is waiting a new message */ @@ -338,11 +354,10 @@ class SPI{ */ static void spi_check_bus_collision(SPI::Instance* spi); - private: /** * @brief This method initializes the SPI peripheral that is passed to it as a parameter. - * + * * @param spi Peripheral instance to be initialized. */ static void init(SPI::Instance* spi); diff --git a/Inc/HALAL/Services/Communication/UART/UART.hpp b/Inc/HALAL/Services/Communication/UART/UART.hpp index ee39bc422..d3ac241b5 100644 --- a/Inc/HALAL/Services/Communication/UART/UART.hpp +++ b/Inc/HALAL/Services/Communication/UART/UART.hpp @@ -19,51 +19,50 @@ #define endl "\n\r" /** * @brief UART service class. Abstracts the use of the UART service of the HAL library. - * + * */ -class UART{ +class UART { private: /** * @brief Struct which defines all data referring to UART peripherals. It is - * declared private in order to prevent unwanted use. Only + * declared private in order to prevent unwanted use. Only * predefined instances should be used. - * + * */ - struct Instance{ - Pin TX; /**< Clock pin. */ - Pin RX; /**< MOSI pin. */ - UART_HandleTypeDef* huart; /**< HAL UART struct. */ + struct Instance { + Pin TX; /**< Clock pin. */ + Pin RX; /**< MOSI pin. */ + UART_HandleTypeDef* huart; /**< HAL UART struct. */ USART_TypeDef* instance; uint32_t baud_rate; uint32_t word_length; bool receive_ready = false; /**< Receive value is ready to use pin. */ bool initialized = false; - }; static UART_HandleTypeDef* get_handle(uint8_t id); public: - /** - * @brief Enum which abstracts the use of the Instance struct to facilitate the mocking of the HALAL.Struct + * @brief Enum which abstracts the use of the Instance struct to facilitate the mocking of the + * HALAL.Struct * */ - enum Peripheral{ - peripheral1 = 0, - peripheral2 = 1, + enum Peripheral { + peripheral1 = 0, + peripheral2 = 1, peripheral3 = 2, - peripheral4 = 3, - peripheral5 = 4, - peripheral6 = 5, - peripheral7 = 6, - peripheral8 = 7, - peripheral9 = 8, - peripheral10 = 9 + peripheral4 = 3, + peripheral5 = 4, + peripheral6 = 5, + peripheral7 = 6, + peripheral8 = 7, + peripheral9 = 8, + peripheral10 = 9 }; static uint16_t id_counter; - + static unordered_map registered_uart; static unordered_map available_uarts; @@ -71,9 +70,9 @@ class UART{ static bool printf_ready; /** - * @brief UART wrapper enum of the STM32H723. - * - */ + * @brief UART wrapper enum of the STM32H723. + * + */ static UART::Peripheral uart1; static UART::Peripheral uart2; static UART::Peripheral uart3; @@ -85,7 +84,6 @@ class UART{ static UART::Peripheral uart9; static UART::Peripheral uart10; - /** * @brief UART instances of the STM32H723. * @@ -103,7 +101,7 @@ class UART{ /** * @brief Registers a new UART. - * + * * @param uart UART peripheral to register. * @return uint8_t Id of the service. */ @@ -112,7 +110,7 @@ class UART{ /** * @brief This method initializes all registered UARTs. The peripherals * must be enrolled before calling this method. - * + * */ static void start(); @@ -120,7 +118,7 @@ class UART{ * To send various packets in a row you must check if the UART is busy * using is_busy(). All calls to this functions previous * the bus is ready be ignored. - * + * * @param id Id of the UART * @param data data to be send * @return bool Returns true if the request to send the packet has been done @@ -130,47 +128,47 @@ class UART{ static bool transmit(uint8_t id, uint8_t data); /**@brief Transmits size number of bytes by DMA and interrupts. - * To send various packets in a row you must check if the UART is busy - * using is_busy(). All calls to this functions previous + * To send various packets in a row you must check if the UART is busy + * using is_busy(). All calls to this functions previous * the bus is ready be ignored. - * - * @param id Id of the UART - * @param data Data to be sent. - * @return bool Returns true if the request to send the packet has been done - * successfully. Returns false if the UART is busy or a problem - * has occurred. - */ + * + * @param id Id of the UART + * @param data Data to be sent. + * @return bool Returns true if the request to send the packet has been done + * successfully. Returns false if the UART is busy or a problem + * has occurred. + */ static bool transmit(uint8_t id, span data); /**@brief Transmits 1 byte by polling. - * - * @param id Id of the UART - * @param data Data to be sent. - * @return bool Returns true if the request to send the packet has been done - * successfully. Returns false if the UART is busy or a problem - * has occurred. - */ + * + * @param id Id of the UART + * @param data Data to be sent. + * @return bool Returns true if the request to send the packet has been done + * successfully. Returns false if the UART is busy or a problem + * has occurred. + */ static bool transmit_polling(uint8_t id, uint8_t data); /**@brief Transmits size bytes by polling. - * - * @param id Id of the UART - * @param data Data to be sent. - * @return bool Returns true if the packet has been send successfully. - * Returns false if the UART is busy or a problem has occurred. - */ + * + * @param id Id of the UART + * @param data Data to be sent. + * @return bool Returns true if the packet has been send successfully. + * Returns false if the UART is busy or a problem has occurred. + */ static bool transmit_polling(uint8_t id, span data); - /** + /** * @brief This method request the receive of size bytes * by DMA and interrupts. Thus the data should not be used until - * you have checked that the value is already available using the + * you have checked that the value is already available using the * method has_next_paclet(). All calls to this functions previous * the last packet is ready will be ignored. * * @see UART::has_next_packet() - * + * * @param id Id of the UART * @param data Where data will be stored * @return bool Return true if the order to receive a new packet has been @@ -180,20 +178,21 @@ class UART{ static bool receive(uint8_t id, span data); /** - * @brief This method receive size number of bytes by polling. - * - * @see UART::has_next_packet() - * - * @param id Id of the UART - * @param data Where data will be stored - * @return bool Return true if the data has been read successfully. - * Return false if the UART is busy or a problem has occurred. - */ + * @brief This method receive size number of bytes by polling. + * + * @see UART::has_next_packet() + * + * @param id Id of the UART + * @param data Where data will be stored + * @return bool Return true if the data has been read successfully. + * Return false if the UART is busy or a problem has occurred. + */ static bool receive_polling(uint8_t id, span data); /** - * @brief This method is used to check if the UART receive operation has finished and data is ready. - * + * @brief This method is used to check if the UART receive operation has finished and data is + * ready. + * * @param id Id of the UART * @return bool Return true if the packet is ready to use and false if not. */ @@ -201,38 +200,37 @@ class UART{ /** * @brief This method is used to check if the UART transmit operations busy. - * + * * @param id Id of the UART * @return bool Return true if the UART transmit operation is busy and false if not. */ static bool is_busy(uint8_t id); /** - * @brief This method is used to set up the printf. It's inscribe and configure the selected UART to work - * as standard and error output. - * - * @param uart Uart peripheral to be configured. - * @return bool True if everything went well. False if something has gone wrong. - */ + * @brief This method is used to set up the printf. It's inscribe and configure the selected + * UART to work as standard and error output. + * + * @param uart Uart peripheral to be configured. + * @return bool True if everything went well. False if something has gone wrong. + */ static bool set_up_printf(UART::Peripheral& uart); /** - * @brief This method is used to print a message through the uart configured for printf. - * It only works if it has been configured correctly. - * - * @param ptr Pointer to the character string. - * @return bool True if everything went well. False if something has gone wrong. - */ + * @brief This method is used to print a message through the uart configured for printf. + * It only works if it has been configured correctly. + * + * @param ptr Pointer to the character string. + * @return bool True if everything went well. False if something has gone wrong. + */ static void print_by_uart(char* ptr, int len); - private: +private: /** * @brief This method initializes the UART peripheral that is passed to it as a parameter. - * + * * @param uart Peripheral instance to be initialized. */ static void init(UART::Instance* uart); - }; #endif diff --git a/Inc/HALAL/Services/DigitalInputService/DigitalInputService.hpp b/Inc/HALAL/Services/DigitalInputService/DigitalInputService.hpp index 22a9d1a13..2c523d119 100644 --- a/Inc/HALAL/Services/DigitalInputService/DigitalInputService.hpp +++ b/Inc/HALAL/Services/DigitalInputService/DigitalInputService.hpp @@ -9,12 +9,12 @@ #include "ErrorHandler/ErrorHandler.hpp" #ifdef HAL_GPIO_MODULE_ENABLED -class DigitalInput{ +class DigitalInput { public: - static map service_ids; - static uint8_t id_counter; + static map service_ids; + static uint8_t id_counter; - static uint8_t inscribe(Pin& pin); - static PinState read_pin_state(uint8_t id); + static uint8_t inscribe(Pin& pin); + static PinState read_pin_state(uint8_t id); }; #endif diff --git a/Inc/HALAL/Services/DigitalOutputService/DigitalOutputService.hpp b/Inc/HALAL/Services/DigitalOutputService/DigitalOutputService.hpp index ebb6c7e84..d558c2f91 100644 --- a/Inc/HALAL/Services/DigitalOutputService/DigitalOutputService.hpp +++ b/Inc/HALAL/Services/DigitalOutputService/DigitalOutputService.hpp @@ -8,7 +8,7 @@ #include "HALAL/Models/PinModel/Pin.hpp" class DigitalOutputService { - public: +public: static map service_ids; static uint8_t id_counter; diff --git a/Inc/HALAL/Services/EXTI/EXTI.hpp b/Inc/HALAL/Services/EXTI/EXTI.hpp index 7cabcf2ea..217788d9b 100644 --- a/Inc/HALAL/Services/EXTI/EXTI.hpp +++ b/Inc/HALAL/Services/EXTI/EXTI.hpp @@ -2,7 +2,7 @@ * EXTI.hpp * * Created on: Nov 5, 2022 - * Author: alejandro + * Author: alejandro */ #pragma once @@ -12,26 +12,25 @@ class ExternalInterrupt { public: - - class Instance { - public: - IRQn_Type interrupt_request_number; - - function action = nullptr; - bool is_on = true; - - Instance() = default; - Instance(IRQn_Type interrupt_request_number); - }; - - static map service_ids; - static map instances; - static uint8_t id_counter; - - static uint8_t inscribe(Pin& pin, function&& action, TRIGGER trigger=RISING_EDGE); - static void start(); - static void turn_on(uint8_t id); - static void turn_off(uint8_t id); - static bool get_pin_value(uint8_t id); + class Instance { + public: + IRQn_Type interrupt_request_number; + + function action = nullptr; + bool is_on = true; + + Instance() = default; + Instance(IRQn_Type interrupt_request_number); + }; + + static map service_ids; + static map instances; + static uint8_t id_counter; + + static uint8_t inscribe(Pin& pin, function&& action, TRIGGER trigger = RISING_EDGE); + static void start(); + static void turn_on(uint8_t id); + static void turn_off(uint8_t id); + static bool get_pin_value(uint8_t id); }; #endif diff --git a/Inc/HALAL/Services/Encoder/Encoder.hpp b/Inc/HALAL/Services/Encoder/Encoder.hpp index 09b4d7abf..52671ee39 100644 --- a/Inc/HALAL/Services/Encoder/Encoder.hpp +++ b/Inc/HALAL/Services/Encoder/Encoder.hpp @@ -7,14 +7,13 @@ #pragma once - #include "HALAL/Models/PinModel/Pin.hpp" #include "HALAL/Models/TimerPeripheral/TimerPeripheral.hpp" #include "C++Utilities/CppUtils.hpp" #include "ErrorHandler/ErrorHandler.hpp" #define NANO_SECOND 1000000000.0 -#define CLOCK_MAX_VALUE 4294967295 //here goes the tim23 counter period +#define CLOCK_MAX_VALUE 4294967295 // here goes the tim23 counter period #ifdef HAL_TIM_MODULE_ENABLED /** @@ -23,63 +22,63 @@ */ class Encoder { public: - static uint8_t id_counter; - static map, TimerPeripheral*> pin_timer_map; - static map> registered_encoder; + static uint8_t id_counter; + static map, TimerPeripheral*> pin_timer_map; + static map> registered_encoder; - /** - * @brief This method registers a new encoder - * - * @param pin1 First pin of the encoder - * @param pin2 Second pin of the encoder - * - * @retval uint8_t Id of the service - */ - static uint8_t inscribe(Pin& pin1, Pin& pin2); + /** + * @brief This method registers a new encoder + * + * @param pin1 First pin of the encoder + * @param pin2 Second pin of the encoder + * + * @retval uint8_t Id of the service + */ + static uint8_t inscribe(Pin& pin1, Pin& pin2); - static void start(); + static void start(); - /** - * @brief Starts the timer of the encoder - * - * @param id Id of the encoder - */ - static void turn_on(uint8_t id); + /** + * @brief Starts the timer of the encoder + * + * @param id Id of the encoder + */ + static void turn_on(uint8_t id); - /** - * @brief Stop the timer of the encoder - * - * @param id Id of the encoder - */ - static void turn_off(uint8_t id); + /** + * @brief Stop the timer of the encoder + * + * @param id Id of the encoder + */ + static void turn_off(uint8_t id); - /** - * @brief Resets the encoder by setting the CNT register to 0 - * - * @param id Id of the encoder - */ - static void reset(uint8_t id); + /** + * @brief Resets the encoder by setting the CNT register to 0 + * + * @param id Id of the encoder + */ + static void reset(uint8_t id); - /** - * @brief Get the CNT value of the encoder - * - * @param id Id of the encoder - * @return uint32_t CNT value if the id is valid - */ - static uint32_t get_counter(uint8_t id); + /** + * @brief Get the CNT value of the encoder + * + * @param id Id of the encoder + * @return uint32_t CNT value if the id is valid + */ + static uint32_t get_counter(uint8_t id); - /** - * @brief Get the encoder direction - * - * @param id Id - * @return bool Encoder direction if id is valid - */ - static bool get_direction(uint8_t id); + /** + * @brief Get the encoder direction + * + * @param id Id + * @return bool Encoder direction if id is valid + */ + static bool get_direction(uint8_t id); - static void init(TimerPeripheral* encoder); + static void init(TimerPeripheral* encoder); - static uint32_t get_initial_counter_value(uint8_t id); + static uint32_t get_initial_counter_value(uint8_t id); - static int64_t get_delta_clock(uint64_t clock_time, uint64_t last_clock_time); + static int64_t get_delta_clock(uint64_t clock_time, uint64_t last_clock_time); }; #endif diff --git a/Inc/HALAL/Services/Encoder/NewEncoder.hpp b/Inc/HALAL/Services/Encoder/NewEncoder.hpp index d4d2a3a46..db37126ce 100644 --- a/Inc/HALAL/Services/Encoder/NewEncoder.hpp +++ b/Inc/HALAL/Services/Encoder/NewEncoder.hpp @@ -5,37 +5,40 @@ #ifdef HAL_TIM_MODULE_ENABLED #define NANO_SECOND 1000000000.0 -#define CLOCK_MAX_VALUE 4294967295 //here goes the tim23 counter period +#define CLOCK_MAX_VALUE 4294967295 // here goes the tim23 counter period namespace ST_LIB { -template -struct TimerWrapper; +template struct TimerWrapper; -template -class Encoder { - static_assert(dev.e.pin_count == 2, "Encoder must have exactly 2 encoder pins, as it uses the whole timer"); +template class Encoder { + static_assert( + dev.e.pin_count == 2, + "Encoder must have exactly 2 encoder pins, as it uses the whole timer" + ); static_assert(dev.e.pins[0].af == TimerAF::Encoder, "Pin 0 must be declared as encoder"); static_assert(dev.e.pins[1].af == TimerAF::Encoder, "Pin 1 must be declared as encoder"); - static_assert(dev.e.pins[0].channel != dev.e.pins[1].channel, "Pins must be of different channels"); - + static_assert( + dev.e.pins[0].channel != dev.e.pins[1].channel, + "Pins must be of different channels" + ); - inline static TimerWrapper *timer; + inline static TimerWrapper* timer; inline static bool is_on = false; public: - Encoder(TimerWrapper *tim) { - if (timer == nullptr) { - init(tim); - } + Encoder(TimerWrapper* tim) { + if (timer == nullptr) { + init(tim); + } } - static void init(TimerWrapper *tim){ + static void init(TimerWrapper* tim) { timer = tim; TIM_Encoder_InitTypeDef sConfig = {0}; TIM_MasterConfigTypeDef sMasterConfig = {0}; - tim->instance->hal_tim->Init.Prescaler = 5; + tim->instance->hal_tim->Init.Prescaler = 5; tim->instance->hal_tim->Init.CounterMode = TIM_COUNTERMODE_UP; tim->instance->hal_tim->Init.Period = 55000; tim->instance->hal_tim->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; @@ -57,15 +60,17 @@ class Encoder { sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(tim->instance->hal_tim, &sMasterConfig) != HAL_OK) { + if (HAL_TIMEx_MasterConfigSynchronization(tim->instance->hal_tim, &sMasterConfig) != + HAL_OK) { ErrorHandler("Unable to config master synchronization in encoder"); } } static void turn_on() { - if (is_on) return; + if (is_on) + return; - if(HAL_TIM_Encoder_GetState(timer->instance->hal_tim) == HAL_TIM_STATE_RESET) { + if (HAL_TIM_Encoder_GetState(timer->instance->hal_tim) == HAL_TIM_STATE_RESET) { ErrorHandler("Unable to get state from encoder"); return; } @@ -78,40 +83,32 @@ class Encoder { } static void turn_off() { - if (!is_on) return; + if (!is_on) + return; if (HAL_TIM_Encoder_Stop(timer->instance->hal_tim, TIM_CHANNEL_ALL) != HAL_OK) { ErrorHandler("Unable to stop encoder"); } is_on = false; } - static inline void reset() { - timer->instance->tim->CNT = UINT32_MAX / 2; - } + static inline void reset() { timer->instance->tim->CNT = UINT32_MAX / 2; } - static inline uint32_t get_counter() { - return timer->instance->tim->CNT; - } + static inline uint32_t get_counter() { return timer->instance->tim->CNT; } - static inline bool get_direction() { - return ((timer->instance->tim->CR1 & 0b10000) >> 4); - } + static inline bool get_direction() { return ((timer->instance->tim->CR1 & 0b10000) >> 4); } - static inline uint32_t get_initial_counter_value() { - return timer->instance->tim->ARR / 2; - } + static inline uint32_t get_initial_counter_value() { return timer->instance->tim->ARR / 2; } static int64_t get_delta_clock(uint64_t clock_time, uint64_t last_clock_time) { int64_t delta_clock = clock_time - last_clock_time; - if (clock_time < last_clock_time) { // overflow handle - delta_clock = - clock_time + - CLOCK_MAX_VALUE * NANO_SECOND / timer->get_clock_frequency() - - last_clock_time; + if (clock_time < last_clock_time) { // overflow handle + delta_clock = clock_time + + CLOCK_MAX_VALUE * NANO_SECOND / timer->get_clock_frequency() - + last_clock_time; } return delta_clock; } }; -} +} // namespace ST_LIB #endif \ No newline at end of file diff --git a/Inc/HALAL/Services/FMAC/FMAC.hpp b/Inc/HALAL/Services/FMAC/FMAC.hpp index 484898a16..e2c7cea0b 100644 --- a/Inc/HALAL/Services/FMAC/FMAC.hpp +++ b/Inc/HALAL/Services/FMAC/FMAC.hpp @@ -14,122 +14,152 @@ #define FMAC_ERROR_CHECK 0 #endif - #ifdef HAL_FMAC_MODULE_ENABLED -class MultiplierAccelerator{ +class MultiplierAccelerator { public: - enum FMACmodes{ - None, - FIR, - IIR, - }; - - enum FMACstates{ - NOT_RUNNING, - WAITING_DATA, - RUNNING, - }; - - - struct FMACMemoryLayout{ - int16_t *FeedbackFilterCoeffs; - uint16_t FeedbackFilterSize; - int16_t *FFilterCoeffs; - uint16_t FFiltersSize; - int16_t *FeedbackInValues; - uint16_t FeedbackInSize; - int16_t *FInValues; - uint16_t FInSize; - int16_t *OutValues; - int16_t *AuxiliarOutValues; - uint16_t OutSize; - }; - - struct FMACProcessInstance{ - FMACstates state = NOT_RUNNING; - int16_t* input_data; - uint16_t input_data_size; - int16_t* output_data; - int16_t *running_output_data; - uint16_t output_data_size; - }; - - struct FMACInstance{ - FMACmodes mode; - FMAC_HandleTypeDef *hfmac; - DMA::Stream dma_preload; - DMA::Stream dma_read; - DMA::Stream dma_write; - - }; - - static FMACInstance Instance; - static FMACMemoryLayout MemoryLayout; - static FMACProcessInstance Process; - - /** - * @brief Inscribe to make the FMAC run in IIR mode with software inputs and software outputs. - */ - static void IIR_software_in_software_out_inscribe(uint16_t input_coefficient_array_size, int16_t* input_coefficient_array, uint16_t feedback_coefficient_array_size, int16_t* feedback_coefficient_array); - - /** - * @brief used in the HALAL::start() to end the configuration of the FMAC. - */ - static void start(); - - /** - * @brief preloads data to be run into the FMAC. Recommended to preload at least as much zeroes as Feedback Coefficients in the feedback data if no data is planned to be preloaded. - */ - static void software_preload(int16_t* preload_data, uint8_t amount_to_preload, int16_t* preload_feedback_data, uint8_t amount_to_feedback_preload); - - /** - * @brief orders the FMAC to start processing, and will run calculations with the preloaded data. - */ - static void software_run(int16_t* calculated_data, uint16_t* output_size); - - /** - * @brief loads input and only starts a run if the output was loaded with a software function - */ - static void software_load_input(int16_t* input_data, uint16_t* input_size); - - /** - * @brief loads input and starts a run using the same output pointer as the last run - */ - static void software_load_repeat(int16_t* input_data, uint16_t* input_size); - - /** - * @brief loads both input and output pointers and starts a run - */ - static void software_load_full(int16_t* input_data, uint16_t* input_size, int16_t* output_data, uint16_t* output_size); - - /** - * @brief makes the FMAC stop processing. Needed to change the Coefficients. - */ - static void software_stop(); - - /** - * @brief wether or not the FMAC is ready to receive more data. Should be called before using any load function to check if the peripheral is ready. - */ - static bool is_ready(); -private: + enum FMACmodes { + None, + FIR, + IIR, + }; + + enum FMACstates { + NOT_RUNNING, + WAITING_DATA, + RUNNING, + }; + + struct FMACMemoryLayout { + int16_t* FeedbackFilterCoeffs; + uint16_t FeedbackFilterSize; + int16_t* FFilterCoeffs; + uint16_t FFiltersSize; + int16_t* FeedbackInValues; + uint16_t FeedbackInSize; + int16_t* FInValues; + uint16_t FInSize; + int16_t* OutValues; + int16_t* AuxiliarOutValues; + uint16_t OutSize; + }; + + struct FMACProcessInstance { + FMACstates state = NOT_RUNNING; + int16_t* input_data; + uint16_t input_data_size; + int16_t* output_data; + int16_t* running_output_data; + uint16_t output_data_size; + }; + + struct FMACInstance { + FMACmodes mode; + FMAC_HandleTypeDef* hfmac; + DMA::Stream dma_preload; + DMA::Stream dma_read; + DMA::Stream dma_write; + }; + + static FMACInstance Instance; + static FMACMemoryLayout MemoryLayout; + static FMACProcessInstance Process; + + /** + * @brief Inscribe to make the FMAC run in IIR mode with software inputs and software outputs. + */ + static void IIR_software_in_software_out_inscribe( + uint16_t input_coefficient_array_size, + int16_t* input_coefficient_array, + uint16_t feedback_coefficient_array_size, + int16_t* feedback_coefficient_array + ); + + /** + * @brief used in the HALAL::start() to end the configuration of the FMAC. + */ + static void start(); + + /** + * @brief preloads data to be run into the FMAC. Recommended to preload at least as much zeroes + * as Feedback Coefficients in the feedback data if no data is planned to be preloaded. + */ + static void software_preload( + int16_t* preload_data, + uint8_t amount_to_preload, + int16_t* preload_feedback_data, + uint8_t amount_to_feedback_preload + ); + + /** + * @brief orders the FMAC to start processing, and will run calculations with the preloaded + * data. + */ + static void software_run(int16_t* calculated_data, uint16_t* output_size); + + /** + * @brief loads input and only starts a run if the output was loaded with a software function + */ + static void software_load_input(int16_t* input_data, uint16_t* input_size); + + /** + * @brief loads input and starts a run using the same output pointer as the last run + */ + static void software_load_repeat(int16_t* input_data, uint16_t* input_size); + + /** + * @brief loads both input and output pointers and starts a run + */ + static void software_load_full( + int16_t* input_data, + uint16_t* input_size, + int16_t* output_data, + uint16_t* output_size + ); + + /** + * @brief makes the FMAC stop processing. Needed to change the Coefficients. + */ + static void software_stop(); + + /** + * @brief wether or not the FMAC is ready to receive more data. Should be called before using + * any load function to check if the peripheral is ready. + */ + static bool is_ready(); - /** - * @brief generic process used on all inscribe type functions - */ - static void inscribe(); - - /** - * @brief method that configurates the FMAC, used in all init methods - * - * @param FeedbackFiltersCoeffs pointer to the Coefficients used in the Feedback mode. Declare them as nullptr on FIR as they will not be used. Known as A coefficients in FMAC HAL peripheral - * @param FeedbackFilterSize number of FeedbackCoefficients, must be one less than FFiltersSize in normal Feedback use, and 0 on any FIR use - * @param FFiltersCoeffs pointer to the Coefficients used in all modes as the external input. - * @param FFiltersSize number of FIRCoefficients, equal to the inputs computated in one calculation - * @param - */ - static void configure(int16_t *FeedbackFiltersCoeffs, uint8_t FeedbackFilterSize, int16_t *FFiltersCoeffs, uint8_t FFiltersSize, - int16_t *FeedbackInValues, uint8_t FeedbackInSize, int16_t *FInValues, uint8_t FInSize, int16_t *OutValues, uint8_t OutSize, FMACmodes mode); +private: + /** + * @brief generic process used on all inscribe type functions + */ + static void inscribe(); + + /** + * @brief method that configurates the FMAC, used in all init methods + * + * @param FeedbackFiltersCoeffs pointer to the Coefficients used in the Feedback mode. Declare + * them as nullptr on FIR as they will not be used. Known as A coefficients in FMAC HAL + * peripheral + * @param FeedbackFilterSize number of FeedbackCoefficients, must be one less than FFiltersSize + * in normal Feedback use, and 0 on any FIR use + * @param FFiltersCoeffs pointer to the Coefficients used in all modes as the external input. + * @param FFiltersSize number of FIRCoefficients, equal to the inputs computated in one + * calculation + * @param + */ + static void configure( + int16_t* FeedbackFiltersCoeffs, + uint8_t FeedbackFilterSize, + int16_t* FFiltersCoeffs, + uint8_t FFiltersSize, + int16_t* FeedbackInValues, + uint8_t FeedbackInSize, + int16_t* FInValues, + uint8_t FInSize, + int16_t* OutValues, + uint8_t OutSize, + FMACmodes mode + ); }; #endif \ No newline at end of file diff --git a/Inc/HALAL/Services/Flash/Flash.hpp b/Inc/HALAL/Services/Flash/Flash.hpp index f7617d256..54b40dcbe 100644 --- a/Inc/HALAL/Services/Flash/Flash.hpp +++ b/Inc/HALAL/Services/Flash/Flash.hpp @@ -13,7 +13,7 @@ #include "C++Utilities/CppUtils.hpp" #include "ErrorHandler/ErrorHandler.hpp" -#define FLASH_32BITS_WORLD ((uint8_t) 4U) +#define FLASH_32BITS_WORLD ((uint8_t)4U) #define FLASHWORD 8 #define FLASH_START_ADDRESS 0x08000000 #define FLASH_SECTOR0_START_ADDRESS 0x08000000 @@ -27,40 +27,41 @@ #define FLASH_END_ADDRESS 0x080FFFFF #define SECTOR_SIZE_IN_WORDS 32768 -#define SECTOR_SIZE_IN_BYTES ((uint32_t)131072U) +#define SECTOR_SIZE_IN_BYTES ((uint32_t)131072U) /* - **IMPORTANT** - We need to reserve memory in the linker script to avoid overwriting: - MEMORY { - ...... - DATA (xrw) : ORIGIN = 0x8080000, LENGTH = 512K //Allocated half of the flash - ...... - } + **IMPORTANT** + We need to reserve memory in the linker script to avoid overwriting: + MEMORY { + ...... + DATA (xrw) : ORIGIN = 0x8080000, LENGTH = 512K //Allocated half of the flash + ...... + } - To create variables in the flash we need to first create a section: - .user_data : - { . = ALIGN(4); - *(.user_data) - . = ALIGN(4); - } >DATA + To create variables in the flash we need to first create a section: + .user_data : + { . = ALIGN(4); + *(.user_data) + . = ALIGN(4); + } >DATA - Then on the c script we declare: - attribute((section(".user_data"))) const uint8_t userConfig[64] - - Then we can write on the variable using HAL: - HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, (uint32_t)&userConfig[index], someData); + Then on the c script we declare: + attribute((section(".user_data"))) const uint8_t userConfig[64] - More info: https://github.com/HyperloopUPV-H8/ST-LIB/wiki/All-about-Flash-module + Then we can write on the variable using HAL: + HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, (uint32_t)&userConfig[index], someData); + + More info: https://github.com/HyperloopUPV-H8/ST-LIB/wiki/All-about-Flash-module */ -class Flash{ +class Flash { public: - static void read(uint32_t source_addr, uint32_t* result, uint32_t number_of_words); - static bool write(uint32_t* source, uint32_t dest_addr, uint32_t size_in_words); - static bool erase(uint32_t start_sector, uint32_t end_sector); + static void read(uint32_t source_addr, uint32_t* result, uint32_t number_of_words); + static bool write(uint32_t* source, uint32_t dest_addr, uint32_t size_in_words); + static bool erase(uint32_t start_sector, uint32_t end_sector); + private: - static uint32_t get_sector(uint32_t address); - static uint32_t get_sector_starting_address(uint32_t sector); + static uint32_t get_sector(uint32_t address); + static uint32_t get_sector_starting_address(uint32_t sector); }; #endif diff --git a/Inc/HALAL/Services/Flash/FlashTests/Flash_Test.hpp b/Inc/HALAL/Services/Flash/FlashTests/Flash_Test.hpp index beddca530..a2c78e4f9 100644 --- a/Inc/HALAL/Services/Flash/FlashTests/Flash_Test.hpp +++ b/Inc/HALAL/Services/Flash/FlashTests/Flash_Test.hpp @@ -6,26 +6,22 @@ */ #pragma once #include "HALAL/Services/Flash/Flash.hpp" -namespace FlashTest{ +namespace FlashTest { - bool test1_writing_1_float(); +bool test1_writing_1_float(); - bool test2_reading_1_float(); +bool test2_reading_1_float(); - bool test3_writing_multiple_float(); +bool test3_writing_multiple_float(); - bool test4_wr_in_sector_border(); +bool test4_wr_in_sector_border(); - bool test5_write_time_1_word(); - - bool test6_write_time_1000_word(); - - bool test7_read_time_1000_word(uint32_t* result); - - bool test8_erase_time_1_sector(); - -} +bool test5_write_time_1_word(); +bool test6_write_time_1000_word(); +bool test7_read_time_1000_word(uint32_t* result); +bool test8_erase_time_1_sector(); +} // namespace FlashTest diff --git a/Inc/HALAL/Services/InfoWarning/InfoWarning.hpp b/Inc/HALAL/Services/InfoWarning/InfoWarning.hpp index 9935125c2..3a7a57af3 100644 --- a/Inc/HALAL/Services/InfoWarning/InfoWarning.hpp +++ b/Inc/HALAL/Services/InfoWarning/InfoWarning.hpp @@ -12,43 +12,48 @@ class InfoWarning { private: - static string description; - static string line; - static string func; - static string file; + static string description; + static string line; + static string func; + static string file; public: - static bool warning_triggered; - - /** - * @brief Triggers WarningHandler and format the warning message. The format works - * exactly like printf format. - * - * @param format String which will be formated. - * @param args Arguments specifying data to print - */ - static void InfoWarningTrigger(string format, ...); - - /** - * @brief Get all metadata needed for the warning message, including the line function and file. - * The default parameters are not necessary but are there in case the compiler macros stop - * working because a change of the compiler. - * - * @param line Line where the warning occurred - * @param func Function where the warning occurred - * @param file File where the warning occurred - */ - static void SetMetaData( int line = __builtin_LINE(), const char * func = __builtin_FUNCTION(), const char * file = __builtin_FILE()); - - /** - * @brief Transmit the warning message. - */ - static void InfoWarningUpdate(); - - friend class BoundaryInterface; - + static bool warning_triggered; + + /** + * @brief Triggers WarningHandler and format the warning message. The format works + * exactly like printf format. + * + * @param format String which will be formated. + * @param args Arguments specifying data to print + */ + static void InfoWarningTrigger(string format, ...); + + /** + * @brief Get all metadata needed for the warning message, including the line function and file. + * The default parameters are not necessary but are there in case the compiler macros + * stop working because a change of the compiler. + * + * @param line Line where the warning occurred + * @param func Function where the warning occurred + * @param file File where the warning occurred + */ + static void SetMetaData( + int line = __builtin_LINE(), + const char* func = __builtin_FUNCTION(), + const char* file = __builtin_FILE() + ); + + /** + * @brief Transmit the warning message. + */ + static void InfoWarningUpdate(); + + friend class BoundaryInterface; }; -#define WARNING(x, ...) do { InfoWarning::SetMetaData(__LINE__, __FUNCTION__, __FILE__); \ - InfoWarning::InfoWarningTrigger(x, ##__VA_ARGS__);}while(0) - +#define WARNING(x, ...) \ + do { \ + InfoWarning::SetMetaData(__LINE__, __FUNCTION__, __FILE__); \ + InfoWarning::InfoWarningTrigger(x, ##__VA_ARGS__); \ + } while (0) diff --git a/Inc/HALAL/Services/InputCapture/InputCapture.hpp b/Inc/HALAL/Services/InputCapture/InputCapture.hpp index 77801bc10..4de4cd25f 100644 --- a/Inc/HALAL/Services/InputCapture/InputCapture.hpp +++ b/Inc/HALAL/Services/InputCapture/InputCapture.hpp @@ -2,7 +2,7 @@ * InputCapture.hpp * * Created on: 30 oct. 2022 - * Author: alejandro + * Author: alejandro */ #pragma once @@ -13,30 +13,35 @@ class InputCapture { public: - class Instance { - public: - uint8_t id; - Pin pin; - TimerPeripheral* peripheral; - uint32_t channel_rising; - uint32_t channel_falling; - uint32_t frequency; - uint8_t duty_cycle; + class Instance { + public: + uint8_t id; + Pin pin; + TimerPeripheral* peripheral; + uint32_t channel_rising; + uint32_t channel_falling; + uint32_t frequency; + uint8_t duty_cycle; - Instance() = default; - Instance(Pin& pin, TimerPeripheral* peripheral, uint32_t channel_rising, uint32_t channel_falling); - }; + Instance() = default; + Instance( + Pin& pin, + TimerPeripheral* peripheral, + uint32_t channel_rising, + uint32_t channel_falling + ); + }; - static map active_instances; - static map available_instances; - static uint8_t id_counter; + static map active_instances; + static map available_instances; + static uint8_t id_counter; - static uint8_t inscribe(Pin& pin); - static void turn_on(uint8_t id); - static void turn_off(uint8_t id); - static uint32_t read_frequency(uint8_t id); - static uint8_t read_duty_cycle(uint8_t id); - static Instance find_instance_by_channel(uint32_t channel); + static uint8_t inscribe(Pin& pin); + static void turn_on(uint8_t id); + static void turn_off(uint8_t id); + static uint32_t read_frequency(uint8_t id); + static uint8_t read_duty_cycle(uint8_t id); + static Instance find_instance_by_channel(uint32_t channel); }; #endif diff --git a/Inc/HALAL/Services/PWM/DualCenterPWM/DualCenterPWM.hpp b/Inc/HALAL/Services/PWM/DualCenterPWM/DualCenterPWM.hpp index 67befc3a2..9967af046 100644 --- a/Inc/HALAL/Services/PWM/DualCenterPWM/DualCenterPWM.hpp +++ b/Inc/HALAL/Services/PWM/DualCenterPWM/DualCenterPWM.hpp @@ -3,7 +3,7 @@ #include "HALAL/Services/PWM/DualPWM/DualPWM.hpp" class DualCenterPWM : public DualPWM { - public: +public: DualCenterPWM(Pin& pin, Pin& pin_negated); void set_frequency(uint32_t freq_in_hz); diff --git a/Inc/HALAL/Services/PWM/DualPWM.hpp b/Inc/HALAL/Services/PWM/DualPWM.hpp index 5a5519de3..26c36d073 100644 --- a/Inc/HALAL/Services/PWM/DualPWM.hpp +++ b/Inc/HALAL/Services/PWM/DualPWM.hpp @@ -12,59 +12,83 @@ namespace ST_LIB { -template -struct TimerWrapper; +template struct TimerWrapper; -template +template < + const TimerDomain::Timer& dev, + const ST_LIB::TimerPin pin, + const ST_LIB::TimerPin negated_pin> class DualPWM { static consteval uint8_t get_channel_state_idx(const ST_LIB::TimerChannel ch) { - switch(ch) { - case TimerChannel::CHANNEL_1: case TimerChannel::CHANNEL_1_NEGATED: - case TimerChannel::CHANNEL_2: case TimerChannel::CHANNEL_2_NEGATED: - case TimerChannel::CHANNEL_3: case TimerChannel::CHANNEL_3_NEGATED: - case TimerChannel::CHANNEL_4: - case TimerChannel::CHANNEL_5: - case TimerChannel::CHANNEL_6: - return (static_cast(ch) & - ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - 1; - - default: ST_LIB::compile_error("unreachable"); - return 0; + switch (ch) { + case TimerChannel::CHANNEL_1: + case TimerChannel::CHANNEL_1_NEGATED: + case TimerChannel::CHANNEL_2: + case TimerChannel::CHANNEL_2_NEGATED: + case TimerChannel::CHANNEL_3: + case TimerChannel::CHANNEL_3_NEGATED: + case TimerChannel::CHANNEL_4: + case TimerChannel::CHANNEL_5: + case TimerChannel::CHANNEL_6: + return (static_cast(ch) & + ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - + 1; + + default: + ST_LIB::compile_error("unreachable"); + return 0; } } static consteval uint8_t get_channel_mul4(const ST_LIB::TimerChannel ch) { - switch(ch) { - case TimerChannel::CHANNEL_1: case TimerChannel::CHANNEL_1_NEGATED: return 0x00; - case TimerChannel::CHANNEL_2: case TimerChannel::CHANNEL_2_NEGATED: return 0x04; - case TimerChannel::CHANNEL_3: case TimerChannel::CHANNEL_3_NEGATED: return 0x08; - case TimerChannel::CHANNEL_4: return 0x0C; - case TimerChannel::CHANNEL_5: return 0x10; - case TimerChannel::CHANNEL_6: return 0x14; - - default: ST_LIB::compile_error("unreachable"); - return 0; + switch (ch) { + case TimerChannel::CHANNEL_1: + case TimerChannel::CHANNEL_1_NEGATED: + return 0x00; + case TimerChannel::CHANNEL_2: + case TimerChannel::CHANNEL_2_NEGATED: + return 0x04; + case TimerChannel::CHANNEL_3: + case TimerChannel::CHANNEL_3_NEGATED: + return 0x08; + case TimerChannel::CHANNEL_4: + return 0x0C; + case TimerChannel::CHANNEL_5: + return 0x10; + case TimerChannel::CHANNEL_6: + return 0x14; + + default: + ST_LIB::compile_error("unreachable"); + return 0; } } - TimerWrapper *timer; - uint32_t *frequency; - float *duty_cycle = nullptr; + TimerWrapper* timer; + uint32_t* frequency; + float* duty_cycle = nullptr; bool is_on_positive = false; bool is_on_negative = false; public: - DualPWM(TimerWrapper *tim, uint32_t polarity, uint32_t negated_polarity, float *duty_ptr, uint32_t *frequency_ptr) : timer(tim) { + DualPWM( + TimerWrapper* tim, + uint32_t polarity, + uint32_t negated_polarity, + float* duty_ptr, + uint32_t* frequency_ptr + ) + : timer(tim) { duty_cycle = duty_ptr; frequency = frequency_ptr; - TIM_OC_InitTypeDef sConfigOC = { + TIM_OC_InitTypeDef sConfigOC = { .OCMode = TIM_OCMODE_PWM1, .Pulse = 0, .OCPolarity = polarity, .OCNPolarity = negated_polarity, - + .OCFastMode = TIM_OCFAST_DISABLE, .OCIdleState = TIM_OCIDLESTATE_RESET, .OCNIdleState = TIM_OCNIDLESTATE_RESET, @@ -85,27 +109,32 @@ class DualPWM { } void turn_on_positive() { - if(this->is_on_positive) return; - - //if(HAL_TIM_PWM_Start(timer->instance->hal_tim, channel) != HAL_OK) { ErrorHandler("", 0); } - volatile HAL_TIM_ChannelStateTypeDef *state = &timer->instance->hal_tim->ChannelState[get_channel_state_idx(pin.channel)]; - if(*state != HAL_TIM_CHANNEL_STATE_READY) { + if (this->is_on_positive) + return; + + // if(HAL_TIM_PWM_Start(timer->instance->hal_tim, channel) != HAL_OK) { ErrorHandler("", 0); + // } + volatile HAL_TIM_ChannelStateTypeDef* state = + &timer->instance->hal_tim->ChannelState[get_channel_state_idx(pin.channel)]; + if (*state != HAL_TIM_CHANNEL_STATE_READY) { ErrorHandler("Channel not ready"); } *state = HAL_TIM_CHANNEL_STATE_BUSY; // enable CCx - uint32_t enableCCx = TIM_CCER_CC1E << (get_channel_mul4(pin.channel) & 0x1FU); /* 0x1FU = 31 bits max shift */ + uint32_t enableCCx = TIM_CCER_CC1E + << (get_channel_mul4(pin.channel) & 0x1FU + ); /* 0x1FU = 31 bits max shift */ SET_BIT(timer->instance->tim->CCER, enableCCx); - if constexpr(timer->is_break_instance) { + if constexpr (timer->is_break_instance) { // Main Output Enable SET_BIT(timer->instance->tim->BDTR, TIM_BDTR_MOE); } - if constexpr(timer->is_slave_instance) { + if constexpr (timer->is_slave_instance) { uint32_t tmpsmcr = timer->instance->tim->SMCR & TIM_SMCR_SMS; - if(!IS_TIM_SLAVEMODE_TRIGGER_ENABLED(tmpsmcr)) { + if (!IS_TIM_SLAVEMODE_TRIGGER_ENABLED(tmpsmcr)) { timer->counter_enable(); } } else { @@ -116,27 +145,32 @@ class DualPWM { } void turn_on_negative() { - if(this->is_on_negative) return; - - //if(HAL_TIM_PWM_Start(timer->instance->hal_tim, channel) != HAL_OK) { ErrorHandler("", 0); } - volatile HAL_TIM_ChannelStateTypeDef *state = &timer->instance->hal_tim->ChannelNState[get_channel_state_idx(negated_pin.channel)]; - if(*state != HAL_TIM_CHANNEL_STATE_READY) { + if (this->is_on_negative) + return; + + // if(HAL_TIM_PWM_Start(timer->instance->hal_tim, channel) != HAL_OK) { ErrorHandler("", 0); + // } + volatile HAL_TIM_ChannelStateTypeDef* state = + &timer->instance->hal_tim->ChannelNState[get_channel_state_idx(negated_pin.channel)]; + if (*state != HAL_TIM_CHANNEL_STATE_READY) { ErrorHandler("Channel not ready"); } *state = HAL_TIM_CHANNEL_STATE_BUSY; // enable CCNx - uint32_t enableCCNx = TIM_CCER_CC1NE << (get_channel_mul4(negated_pin.channel) & 0x1FU); /* 0x1FU = 31 bits max shift */ + uint32_t enableCCNx = TIM_CCER_CC1NE + << (get_channel_mul4(negated_pin.channel) & 0x1FU + ); /* 0x1FU = 31 bits max shift */ SET_BIT(timer->instance->tim->CCER, enableCCNx); - if constexpr(timer->is_break_instance) { + if constexpr (timer->is_break_instance) { // Main Output Enable SET_BIT(timer->instance->tim->BDTR, TIM_BDTR_MOE); } - if constexpr(timer->is_slave_instance) { + if constexpr (timer->is_slave_instance) { uint32_t tmpsmcr = timer->instance->tim->SMCR & TIM_SMCR_SMS; - if(!IS_TIM_SLAVEMODE_TRIGGER_ENABLED(tmpsmcr)) { + if (!IS_TIM_SLAVEMODE_TRIGGER_ENABLED(tmpsmcr)) { timer->counter_enable(); } } else { @@ -147,15 +181,20 @@ class DualPWM { } void turn_off_positive() { - if(!this->is_on_positive) return; + if (!this->is_on_positive) + return; - CLEAR_BIT(timer->tim->CCER, (uint32_t)(TIM_CCER_CC1E << (get_channel_mul4(pin.channel) & 0x1FU))); + CLEAR_BIT( + timer->tim->CCER, + (uint32_t)(TIM_CCER_CC1E << (get_channel_mul4(pin.channel) & 0x1FU)) + ); - HAL_TIM_ChannelStateTypeDef *state = &timer->instance->hal_tim.ChannelState[get_channel_state_idx(pin.channel)]; + HAL_TIM_ChannelStateTypeDef* state = + &timer->instance->hal_tim.ChannelState[get_channel_state_idx(pin.channel)]; *state = HAL_TIM_CHANNEL_STATE_READY; - if(timer->are_all_channels_free()) { - if constexpr(timer->is_break_instance) { + if (timer->are_all_channels_free()) { + if constexpr (timer->is_break_instance) { // Disable Main Output Enable (MOE) CLEAR_BIT(timer->tim->BDTR, TIM_BDTR_MOE); } @@ -166,15 +205,20 @@ class DualPWM { } void turn_off_negative() { - if(!this->is_on_negative) return; + if (!this->is_on_negative) + return; - CLEAR_BIT(timer->tim->CCER, (uint32_t)(TIM_CCER_CC1NE << (get_channel_mul4(negated_pin.channel) & 0x1FU))); + CLEAR_BIT( + timer->tim->CCER, + (uint32_t)(TIM_CCER_CC1NE << (get_channel_mul4(negated_pin.channel) & 0x1FU)) + ); - HAL_TIM_ChannelStateTypeDef *state = &timer->instance->hal_tim.ChannelState[get_channel_state_idx(negated_pin.channel)]; + HAL_TIM_ChannelStateTypeDef* state = + &timer->instance->hal_tim.ChannelState[get_channel_state_idx(negated_pin.channel)]; *state = HAL_TIM_CHANNEL_STATE_READY; - if(timer->are_all_channels_free()) { - if constexpr(timer->is_break_instance) { + if (timer->are_all_channels_free()) { + if constexpr (timer->is_break_instance) { // Disable Main Output Enable (MOE) CLEAR_BIT(timer->tim->BDTR, TIM_BDTR_MOE); } @@ -185,24 +229,28 @@ class DualPWM { } inline void set_duty_cycle(float duty_cycle) { - if(duty_cycle <= 0.0f) [[unlikely]] { + if (duty_cycle <= 0.0f) [[unlikely]] { timer->template set_capture_compare(0); *(this->duty_cycle) = 0.0f; return; } - - if(duty_cycle > 100.0f) [[unlikely]] { duty_cycle = 100.0f; } - if constexpr(timer->is_32bit_instance) { - uint32_t raw_duty = (uint32_t)((float)(timer->instance->tim->ARR + 1) * (duty_cycle / 100.0f)); - timer->template set_capture_compare(raw_duty); - } else { - uint16_t raw_duty = (uint16_t)((float)(timer->instance->tim->ARR + 1) * (duty_cycle / 100.0f)); - timer->template set_capture_compare(raw_duty); - } + + if (duty_cycle > 100.0f) [[unlikely]] { + duty_cycle = 100.0f; + } + if constexpr (timer->is_32bit_instance) { + uint32_t raw_duty = + (uint32_t)((float)(timer->instance->tim->ARR + 1) * (duty_cycle / 100.0f)); + timer->template set_capture_compare(raw_duty); + } else { + uint16_t raw_duty = + (uint16_t)((float)(timer->instance->tim->ARR + 1) * (duty_cycle / 100.0f)); + timer->template set_capture_compare(raw_duty); + } *(this->duty_cycle) = duty_cycle; } - template + template inline void set_timer_frequency(uint32_t frequency) { timer->template set_pwm_frequency(frequency); } @@ -213,47 +261,47 @@ class DualPWM { this->set_dead_time(dead_time_ns); } - inline uint32_t get_frequency() const { - return *(this->frequency); - } - inline float get_duty_cycle() const { - return *(this->duty_cycle); - } + inline uint32_t get_frequency() const { return *(this->frequency); } + inline float get_duty_cycle() const { return *(this->duty_cycle); } void set_dead_time(int64_t dead_time_ns) { - TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; + TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; int64_t time = dead_time_ns; float clock_period_ns = 1000'000'000.0f / (float)timer->get_clock_frequency(); - if(time <= 127 * clock_period_ns) { + if (time <= 127 * clock_period_ns) { /* range 1: DTG[7] = 0; DTG[6:0] = [0..127]) */ sBreakDeadTimeConfig.DeadTime = time / clock_period_ns; - } else if(time <= (2 * clock_period_ns * 127)) { + } else if (time <= (2 * clock_period_ns * 127)) { /* range 2: DTG[7:6] = 0b10; DTG[5:0] = 2*(64 + [0..63]) */ - sBreakDeadTimeConfig.DeadTime = + sBreakDeadTimeConfig.DeadTime = 0b1000'0000 | (uint32_t)((float)time / (2 * clock_period_ns) - 64); - } else if(time <= (8 * clock_period_ns * 63)) { + } else if (time <= (8 * clock_period_ns * 63)) { /* range 3: DTG[7:5] = 0b110; DTG[4:0] = 8*(32 + [0..31]) */ int64_t min = (8 * clock_period_ns * 32); - if(time < min) { time = min; } - sBreakDeadTimeConfig.DeadTime = + if (time < min) { + time = min; + } + sBreakDeadTimeConfig.DeadTime = 0b1100'0000 | (uint32_t)((float)time / (8 * clock_period_ns) - 32); - } else if(time <= (16 * clock_period_ns * 63)) { + } else if (time <= (16 * clock_period_ns * 63)) { /* range 4: DTG[7:5] = 0b111; DTG[4:0] = 16*(32 + [0..31]) */ int64_t min = (16 * clock_period_ns * 32); - if(time < min) { time = min; } - sBreakDeadTimeConfig.DeadTime = + if (time < min) { + time = min; + } + sBreakDeadTimeConfig.DeadTime = 0b1110'0000 | (uint32_t)((float)time / (16 * clock_period_ns) - 32); } else { ErrorHandler("Invalid dead time configuration"); } - //sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; - //sBreakDeadTimeConfig.BreakState = TIM_BREAK_ENABLE; + // sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; + // sBreakDeadTimeConfig.BreakState = TIM_BREAK_ENABLE; HAL_TIMEx_ConfigBreakDeadTime(timer->instance->hal_tim, &sBreakDeadTimeConfig); - - if constexpr(timer->is_break_instance) { + + if constexpr (timer->is_break_instance) { // Main Output Enable SET_BIT(timer->instance->tim->BDTR, TIM_BDTR_MOE); } @@ -262,4 +310,3 @@ class DualPWM { } // namespace ST_LIB #endif // HAL_TIM_MODULE_ENABLED - diff --git a/Inc/HALAL/Services/PWM/DualPWM/DualPWM.hpp b/Inc/HALAL/Services/PWM/DualPWM/DualPWM.hpp index 129b5822e..8ba5243c4 100644 --- a/Inc/HALAL/Services/PWM/DualPWM/DualPWM.hpp +++ b/Inc/HALAL/Services/PWM/DualPWM/DualPWM.hpp @@ -9,30 +9,30 @@ #include "HALAL/Services/PWM/PWM/PWM.hpp" -class DualPWM{ +class DualPWM { protected: - DualPWM() = default; - TimerPeripheral* peripheral; - uint32_t channel; - float duty_cycle; - uint32_t frequency; - bool is_on = false; - static constexpr float CLOCK_FREQ_MHZ_WITHOUT_PRESCALER = 275; - static constexpr float clock_period_ns = (1/CLOCK_FREQ_MHZ_WITHOUT_PRESCALER)*1'000; - bool is_initialized = false; -public: - DualPWM(Pin& pin, Pin& pin_negated); + DualPWM() = default; + TimerPeripheral* peripheral; + uint32_t channel; + float duty_cycle; + uint32_t frequency; + bool is_on = false; + static constexpr float CLOCK_FREQ_MHZ_WITHOUT_PRESCALER = 275; + static constexpr float clock_period_ns = (1 / CLOCK_FREQ_MHZ_WITHOUT_PRESCALER) * 1'000; + bool is_initialized = false; - void turn_on(); - void turn_on_positive(); - void turn_on_negated(); - void turn_off(); - void turn_off_positive(); - void turn_off_negated(); - void set_duty_cycle(float duty_cycle); - void set_frequency(uint32_t freq_in_hz); - uint32_t get_frequency()const; - float get_duty_cycle()const; - void set_dead_time(std::chrono::nanoseconds dead_time_ns); +public: + DualPWM(Pin& pin, Pin& pin_negated); + void turn_on(); + void turn_on_positive(); + void turn_on_negated(); + void turn_off(); + void turn_off_positive(); + void turn_off_negated(); + void set_duty_cycle(float duty_cycle); + void set_frequency(uint32_t freq_in_hz); + uint32_t get_frequency() const; + float get_duty_cycle() const; + void set_dead_time(std::chrono::nanoseconds dead_time_ns); }; diff --git a/Inc/HALAL/Services/PWM/DualPhasedPWM/DualPhasedPWM.hpp b/Inc/HALAL/Services/PWM/DualPhasedPWM/DualPhasedPWM.hpp index f29147367..0962ef100 100644 --- a/Inc/HALAL/Services/PWM/DualPhasedPWM/DualPhasedPWM.hpp +++ b/Inc/HALAL/Services/PWM/DualPhasedPWM/DualPhasedPWM.hpp @@ -10,15 +10,16 @@ #include "HALAL/Services/PWM/PhasedPWM/PhasedPWM.hpp" #include "HALAL/Services/PWM/DualPWM/DualPWM.hpp" -class DualPhasedPWM : public DualPWM{ +class DualPhasedPWM : public DualPWM { private: - float raw_phase{}; + float raw_phase{}; + public: - DualPhasedPWM() = default; - DualPhasedPWM(Pin& pin, Pin& pin_negated); - void set_duty_cycle(float duty_cycle); - void set_frequency(uint32_t frequency); - void set_raw_phase(float raw_phase); - void set_phase(float phase_in_deg); - float get_phase()const; + DualPhasedPWM() = default; + DualPhasedPWM(Pin& pin, Pin& pin_negated); + void set_duty_cycle(float duty_cycle); + void set_frequency(uint32_t frequency); + void set_raw_phase(float raw_phase); + void set_phase(float phase_in_deg); + float get_phase() const; }; diff --git a/Inc/HALAL/Services/PWM/PWM.hpp b/Inc/HALAL/Services/PWM/PWM.hpp index a5180b8c0..b1d4cdb3a 100644 --- a/Inc/HALAL/Services/PWM/PWM.hpp +++ b/Inc/HALAL/Services/PWM/PWM.hpp @@ -12,58 +12,76 @@ namespace ST_LIB { -template -struct TimerWrapper; +template struct TimerWrapper; -template -class PWM { +template class PWM { static consteval uint8_t get_channel_state_idx(const ST_LIB::TimerChannel ch) { - switch(ch) { - case TimerChannel::CHANNEL_1: case TimerChannel::CHANNEL_1_NEGATED: - case TimerChannel::CHANNEL_2: case TimerChannel::CHANNEL_2_NEGATED: - case TimerChannel::CHANNEL_3: case TimerChannel::CHANNEL_3_NEGATED: - case TimerChannel::CHANNEL_4: - case TimerChannel::CHANNEL_5: - case TimerChannel::CHANNEL_6: - return (static_cast(ch) & - ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - 1; - - default: ST_LIB::compile_error("unreachable"); - return 0; + switch (ch) { + case TimerChannel::CHANNEL_1: + case TimerChannel::CHANNEL_1_NEGATED: + case TimerChannel::CHANNEL_2: + case TimerChannel::CHANNEL_2_NEGATED: + case TimerChannel::CHANNEL_3: + case TimerChannel::CHANNEL_3_NEGATED: + case TimerChannel::CHANNEL_4: + case TimerChannel::CHANNEL_5: + case TimerChannel::CHANNEL_6: + return (static_cast(ch) & + ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - + 1; + + default: + ST_LIB::compile_error("unreachable"); + return 0; } } static consteval uint8_t get_channel_mul4(const ST_LIB::TimerChannel ch) { - switch(ch) { - case TimerChannel::CHANNEL_1: case TimerChannel::CHANNEL_1_NEGATED: return 0x00; - case TimerChannel::CHANNEL_2: case TimerChannel::CHANNEL_2_NEGATED: return 0x04; - case TimerChannel::CHANNEL_3: case TimerChannel::CHANNEL_3_NEGATED: return 0x08; - case TimerChannel::CHANNEL_4: return 0x0C; - case TimerChannel::CHANNEL_5: return 0x10; - case TimerChannel::CHANNEL_6: return 0x14; - - default: ST_LIB::compile_error("unreachable"); - return 0; + switch (ch) { + case TimerChannel::CHANNEL_1: + case TimerChannel::CHANNEL_1_NEGATED: + return 0x00; + case TimerChannel::CHANNEL_2: + case TimerChannel::CHANNEL_2_NEGATED: + return 0x04; + case TimerChannel::CHANNEL_3: + case TimerChannel::CHANNEL_3_NEGATED: + return 0x08; + case TimerChannel::CHANNEL_4: + return 0x0C; + case TimerChannel::CHANNEL_5: + return 0x10; + case TimerChannel::CHANNEL_6: + return 0x14; + + default: + ST_LIB::compile_error("unreachable"); + return 0; } } - TimerWrapper *timer; - uint32_t *frequency; - float *duty_cycle = nullptr; + TimerWrapper* timer; + uint32_t* frequency; + float* duty_cycle = nullptr; bool is_on = false; public: - PWM(TimerWrapper *tim, uint32_t polarity, uint32_t negated_polarity, float *duty_ptr, uint32_t *frequency_ptr) : timer(tim) { + PWM(TimerWrapper* tim, + uint32_t polarity, + uint32_t negated_polarity, + float* duty_ptr, + uint32_t* frequency_ptr) + : timer(tim) { duty_cycle = duty_ptr; frequency = frequency_ptr; - TIM_OC_InitTypeDef sConfigOC = { + TIM_OC_InitTypeDef sConfigOC = { .OCMode = TIM_OCMODE_PWM1, .Pulse = 0, .OCPolarity = polarity, .OCNPolarity = negated_polarity, - + .OCFastMode = TIM_OCFAST_DISABLE, .OCIdleState = TIM_OCIDLESTATE_RESET, .OCNIdleState = TIM_OCNIDLESTATE_RESET, @@ -73,27 +91,32 @@ class PWM { } void turn_on() { - if(this->is_on) return; - - //if(HAL_TIM_PWM_Start(timer->instance->hal_tim, channel) != HAL_OK) { ErrorHandler("", 0); } - volatile HAL_TIM_ChannelStateTypeDef *state = &timer->instance->hal_tim->ChannelState[get_channel_state_idx(pin.channel)]; - if(*state != HAL_TIM_CHANNEL_STATE_READY) { + if (this->is_on) + return; + + // if(HAL_TIM_PWM_Start(timer->instance->hal_tim, channel) != HAL_OK) { ErrorHandler("", 0); + // } + volatile HAL_TIM_ChannelStateTypeDef* state = + &timer->instance->hal_tim->ChannelState[get_channel_state_idx(pin.channel)]; + if (*state != HAL_TIM_CHANNEL_STATE_READY) { ErrorHandler("Channel not ready"); } *state = HAL_TIM_CHANNEL_STATE_BUSY; // enable CCx - uint32_t enableCCx = TIM_CCER_CC1E << (get_channel_mul4(pin.channel) & 0x1FU); /* 0x1FU = 31 bits max shift */ + uint32_t enableCCx = TIM_CCER_CC1E + << (get_channel_mul4(pin.channel) & 0x1FU + ); /* 0x1FU = 31 bits max shift */ SET_BIT(timer->instance->tim->CCER, enableCCx); - if constexpr(timer->is_break_instance) { + if constexpr (timer->is_break_instance) { // Main Output Enable SET_BIT(timer->instance->tim->BDTR, TIM_BDTR_MOE); } - if constexpr(timer->is_slave_instance) { + if constexpr (timer->is_slave_instance) { uint32_t tmpsmcr = timer->instance->tim->SMCR & TIM_SMCR_SMS; - if(!IS_TIM_SLAVEMODE_TRIGGER_ENABLED(tmpsmcr)) { + if (!IS_TIM_SLAVEMODE_TRIGGER_ENABLED(tmpsmcr)) { timer->counter_enable(); } } else { @@ -104,15 +127,20 @@ class PWM { } void turn_off() { - if(!this->is_on) return; + if (!this->is_on) + return; - CLEAR_BIT(timer->instance->tim->CCER, (uint32_t)(TIM_CCER_CC1E << (get_channel_mul4(pin.channel) & 0x1FU))); + CLEAR_BIT( + timer->instance->tim->CCER, + (uint32_t)(TIM_CCER_CC1E << (get_channel_mul4(pin.channel) & 0x1FU)) + ); - volatile HAL_TIM_ChannelStateTypeDef *state = &timer->instance->hal_tim->ChannelState[get_channel_state_idx(pin.channel)]; + volatile HAL_TIM_ChannelStateTypeDef* state = + &timer->instance->hal_tim->ChannelState[get_channel_state_idx(pin.channel)]; *state = HAL_TIM_CHANNEL_STATE_READY; - if(timer->are_all_channels_free()) { - if constexpr(timer->is_break_instance) { + if (timer->are_all_channels_free()) { + if constexpr (timer->is_break_instance) { // Disable Main Output Enable (MOE) CLEAR_BIT(timer->instance->tim->BDTR, TIM_BDTR_MOE); } @@ -123,24 +151,28 @@ class PWM { } inline void set_duty_cycle(float duty_cycle) { - if(duty_cycle <= 0.0f) [[unlikely]] { + if (duty_cycle <= 0.0f) [[unlikely]] { timer->template set_capture_compare(0); *(this->duty_cycle) = 0.0f; return; } - - if(duty_cycle > 100.0f) [[unlikely]] { duty_cycle = 100.0f; } - if constexpr(timer->is_32bit_instance) { - uint32_t raw_duty = (uint32_t)((float)(timer->instance->tim->ARR + 1) * (duty_cycle / 100.0f)); + + if (duty_cycle > 100.0f) [[unlikely]] { + duty_cycle = 100.0f; + } + if constexpr (timer->is_32bit_instance) { + uint32_t raw_duty = + (uint32_t)((float)(timer->instance->tim->ARR + 1) * (duty_cycle / 100.0f)); timer->template set_capture_compare(raw_duty); } else { - uint16_t raw_duty = (uint16_t)((float)(timer->instance->tim->ARR + 1) * (duty_cycle / 100.0f)); + uint16_t raw_duty = + (uint16_t)((float)(timer->instance->tim->ARR + 1) * (duty_cycle / 100.0f)); timer->template set_capture_compare(raw_duty); } *(this->duty_cycle) = duty_cycle; } - template + template inline void set_timer_frequency(uint32_t frequency) { timer->template set_pwm_frequency(frequency); } @@ -150,14 +182,9 @@ class PWM { this->template set_timer_frequency(frequency); } - inline uint32_t get_frequency() const { - return *(this->frequency); - } - inline float get_duty_cycle() const { - return *(this->duty_cycle); - } + inline uint32_t get_frequency() const { return *(this->frequency); } + inline float get_duty_cycle() const { return *(this->duty_cycle); } }; } // namespace ST_LIB #endif // HAL_TIM_MODULE_ENABLED - diff --git a/Inc/HALAL/Services/PWM/PWM/PWM.hpp b/Inc/HALAL/Services/PWM/PWM/PWM.hpp index 3374c7b47..1ac3dc2af 100644 --- a/Inc/HALAL/Services/PWM/PWM/PWM.hpp +++ b/Inc/HALAL/Services/PWM/PWM/PWM.hpp @@ -15,34 +15,37 @@ class PWM { protected: - TimerPeripheral* peripheral; - uint32_t channel; - float duty_cycle; - uint32_t frequency; - bool is_on = false; - bool is_center_aligned = false; - static constexpr float CLOCK_FREQ_MHZ_WITHOUT_PRESCALER = 275; - static constexpr float clock_period_ns = (1/CLOCK_FREQ_MHZ_WITHOUT_PRESCALER)*1'000; - bool is_initialized = false; + TimerPeripheral* peripheral; + uint32_t channel; + float duty_cycle; + uint32_t frequency; + bool is_on = false; + bool is_center_aligned = false; + static constexpr float CLOCK_FREQ_MHZ_WITHOUT_PRESCALER = 275; + static constexpr float clock_period_ns = (1 / CLOCK_FREQ_MHZ_WITHOUT_PRESCALER) * 1'000; + bool is_initialized = false; + public: - PWM() = default; - PWM(Pin& pin); - - void turn_on(); - void turn_off(); - void set_duty_cycle(float duty_cycle); - void set_frequency(uint32_t frequency); - uint32_t get_frequency(); - float get_duty_cycle(); - - /** - * @brief function that sets a deadtime, in which the PWM wouldn t be on HIGH no matter the duty cycle - * - * This function has to be called while the PWM is turned off. - * This function actually substracts from the HIGH state of the PWM the amount of ns, pulling it down; - * thus effectively reducing the duty cycle by an amount dependant on the frequency and the dead time. - */ - void set_dead_time(std::chrono::nanoseconds dead_time_ns); + PWM() = default; + PWM(Pin& pin); + + void turn_on(); + void turn_off(); + void set_duty_cycle(float duty_cycle); + void set_frequency(uint32_t frequency); + uint32_t get_frequency(); + float get_duty_cycle(); + + /** + * @brief function that sets a deadtime, in which the PWM wouldn t be on HIGH no matter the duty + * cycle + * + * This function has to be called while the PWM is turned off. + * This function actually substracts from the HIGH state of the PWM the amount of ns, pulling + * it down; thus effectively reducing the duty cycle by an amount dependant on the frequency and + * the dead time. + */ + void set_dead_time(std::chrono::nanoseconds dead_time_ns); }; #endif diff --git a/Inc/HALAL/Services/PWM/PhasedPWM/PhasedPWM.hpp b/Inc/HALAL/Services/PWM/PhasedPWM/PhasedPWM.hpp index 20cccad80..46286081a 100644 --- a/Inc/HALAL/Services/PWM/PhasedPWM/PhasedPWM.hpp +++ b/Inc/HALAL/Services/PWM/PhasedPWM/PhasedPWM.hpp @@ -10,40 +10,38 @@ #include "HALAL/Services/PWM/PWM/PWM.hpp" #define STLIB_TIMER_CCMR_PWM_MODE_1 0x0 -#define STLIB_TIMER_CCMR_PWM_MODE_2 (16+4096) +#define STLIB_TIMER_CCMR_PWM_MODE_2 (16 + 4096) #define STLIB_TIMER_CCMR_REGISTER_MODE_MASK (0xFFFFFFFF - STLIB_TIMER_CCMR_PWM_MODE_2) -#define __STLIB_TIM_SET_MODE(__HANDLE__, __CHANNEL__, __CCMR_PWM_MODE_COMPARE__) \ - switch(__CHANNEL__){\ - case TIM_CHANNEL_1: \ - case TIM_CHANNEL_2: \ - (__HANDLE__)->Instance->CCMR1 &= STLIB_TIMER_CCMR_REGISTER_MODE_MASK;\ - (__HANDLE__)->Instance->CCMR1 |= __CCMR_PWM_MODE_COMPARE__;\ - break;\ - case TIM_CHANNEL_3: \ - case TIM_CHANNEL_4: \ - (__HANDLE__)->Instance->CCMR2 &= STLIB_TIMER_CCMR_REGISTER_MODE_MASK;\ - (__HANDLE__)->Instance->CCMR2 |= __CCMR_PWM_MODE_COMPARE__;\ - break;\ - case TIM_CHANNEL_5: \ - default: \ - (__HANDLE__)->Instance->CCMR3 &= STLIB_TIMER_CCMR_REGISTER_MODE_MASK;\ - (__HANDLE__)->Instance->CCMR3 |= __CCMR_PWM_MODE_COMPARE__;\ - break;\ - } - - -class PhasedPWM : public PWM { +#define __STLIB_TIM_SET_MODE(__HANDLE__, __CHANNEL__, __CCMR_PWM_MODE_COMPARE__) \ + switch (__CHANNEL__) { \ + case TIM_CHANNEL_1: \ + case TIM_CHANNEL_2: \ + (__HANDLE__)->Instance->CCMR1 &= STLIB_TIMER_CCMR_REGISTER_MODE_MASK; \ + (__HANDLE__)->Instance->CCMR1 |= __CCMR_PWM_MODE_COMPARE__; \ + break; \ + case TIM_CHANNEL_3: \ + case TIM_CHANNEL_4: \ + (__HANDLE__)->Instance->CCMR2 &= STLIB_TIMER_CCMR_REGISTER_MODE_MASK; \ + (__HANDLE__)->Instance->CCMR2 |= __CCMR_PWM_MODE_COMPARE__; \ + break; \ + case TIM_CHANNEL_5: \ + default: \ + (__HANDLE__)->Instance->CCMR3 &= STLIB_TIMER_CCMR_REGISTER_MODE_MASK; \ + (__HANDLE__)->Instance->CCMR3 |= __CCMR_PWM_MODE_COMPARE__; \ + break; \ + } + +class PhasedPWM : public PWM { protected: - float raw_phase{}; - PhasedPWM() = default; + float raw_phase{}; + PhasedPWM() = default; public: - void set_duty_cycle(float duty_cycle); - void set_frequency(uint32_t frequency); - void set_raw_phase(float phase); - void set_phase(float phase_in_deg); - float get_phase()const; - PhasedPWM(Pin& pin); - + void set_duty_cycle(float duty_cycle); + void set_frequency(uint32_t frequency); + void set_raw_phase(float phase); + void set_phase(float phase_in_deg); + float get_phase() const; + PhasedPWM(Pin& pin); }; diff --git a/Inc/HALAL/Services/Time/RTC.hpp b/Inc/HALAL/Services/Time/RTC.hpp index ee572f6bb..8ed994061 100644 --- a/Inc/HALAL/Services/Time/RTC.hpp +++ b/Inc/HALAL/Services/Time/RTC.hpp @@ -3,31 +3,35 @@ #include "stm32h7xx_hal.h" #include "ErrorHandler/ErrorHandler.hpp" - #define RTC_MAX_COUNTER 32767 #ifdef HAL_RTC_MODULE_ENABLED - struct RTCData{ - uint16_t counter; - uint8_t second; - uint8_t minute; - uint8_t hour; - uint8_t day; - uint8_t month; - uint16_t year; - }; - - class Global_RTC{ - public: - - static RTCData global_RTC; - static void start_rtc(); - static void update_rtc_data(); - static RTCData get_rtc_timestamp(); - static void set_rtc_data(uint16_t counter, uint8_t second, uint8_t minute, uint8_t hour, uint8_t day, uint8_t month, uint16_t year); - }; - - +struct RTCData { + uint16_t counter; + uint8_t second; + uint8_t minute; + uint8_t hour; + uint8_t day; + uint8_t month; + uint16_t year; +}; + +class Global_RTC { +public: + static RTCData global_RTC; + static void start_rtc(); + static void update_rtc_data(); + static RTCData get_rtc_timestamp(); + static void set_rtc_data( + uint16_t counter, + uint8_t second, + uint8_t minute, + uint8_t hour, + uint8_t day, + uint8_t month, + uint16_t year + ); +}; #endif \ No newline at end of file diff --git a/Inc/HALAL/Services/Time/Scheduler.hpp b/Inc/HALAL/Services/Time/Scheduler.hpp index 74c5759a2..f4bb7b448 100644 --- a/Inc/HALAL/Services/Time/Scheduler.hpp +++ b/Inc/HALAL/Services/Time/Scheduler.hpp @@ -9,7 +9,7 @@ /* Uso del scheduler, descrito en la wiki: * https://wiki.hyperloopupv.com/es/firmware/Timing/Scheduler */ -#include "stm32h7xx_ll_tim_wrapper.h" +#include "stm32h7xx_ll_tim_wrapper.h" #include #include @@ -34,73 +34,74 @@ extern TIM_HandleTypeDef SCHEDULER_HAL_TIM; #endif struct Scheduler { - using callback_t = void (*)(); - static constexpr uint32_t INVALID_ID = 0xFFu; + using callback_t = void (*)(); + static constexpr uint32_t INVALID_ID = 0xFFu; - static void start(); - static void update(); - static inline uint64_t get_global_tick(); + static void start(); + static void update(); + static inline uint64_t get_global_tick(); - static uint16_t register_task(uint32_t period_us, callback_t func); - static bool unregister_task(uint16_t id); + static uint16_t register_task(uint32_t period_us, callback_t func); + static bool unregister_task(uint16_t id); - static uint16_t set_timeout(uint32_t microseconds, callback_t func); - static bool cancel_timeout(uint16_t id); + static uint16_t set_timeout(uint32_t microseconds, callback_t func); + static bool cancel_timeout(uint16_t id); - // static void global_timer_callback(); + // static void global_timer_callback(); - // Have to be public because SCHEDULER_GLOBAL_TIMER_CALLBACK won't work - // otherwise - // static const uint32_t global_timer_base = SCHEDULER_TIMER_BASE; - static void on_timer_update(); + // Have to be public because SCHEDULER_GLOBAL_TIMER_CALLBACK won't work + // otherwise + // static const uint32_t global_timer_base = SCHEDULER_TIMER_BASE; + static void on_timer_update(); #ifndef SIM_ON private: #endif - struct Task { - uint32_t next_fire_us{0}; - callback_t callback{}; - uint32_t period_us{0}; - uint16_t id; - bool repeating{false}; - }; - - static constexpr std::size_t kMaxTasks = 16; - static_assert((kMaxTasks & (kMaxTasks - 1)) == 0, - "kMaxTasks must be a power of two"); - static constexpr uint32_t FREQUENCY = 1'000'000u; // 1 MHz -> 1us precision - - static std::array tasks_; - static_assert( - kMaxTasks == 16, - "kMaxTasks must be 16, if more is needed, sorted_task_ids_ must change"); - /* sorted_task_ids_ is a sorted queue with 4bits for each id in the - * scheduler's current ids */ - static uint64_t sorted_task_ids_; - - static uint32_t active_task_count_; - static_assert( - kMaxTasks <= 32, - "kMaxTasks must be <= 32, if more is needed, the bitmaps must change"); - static uint32_t ready_bitmap_; - static uint32_t free_bitmap_; - static uint64_t global_tick_us_; - static uint32_t current_interval_us_; - static uint16_t timeout_idx_; - - static inline uint8_t allocate_slot(); - static inline void release_slot(uint8_t id); - static void insert_sorted(uint8_t id); - static void remove_sorted(uint8_t id); - static void schedule_next_interval(); - static inline void configure_timer_for_interval(uint32_t microseconds); - - // helpers - static inline uint8_t get_at(uint8_t idx); - static inline void set_at(uint8_t idx, uint8_t id); - static inline void pop_front(); - static inline uint8_t front_id(); - - static inline void global_timer_disable(); - static inline void global_timer_enable(); + struct Task { + uint32_t next_fire_us{0}; + callback_t callback{}; + uint32_t period_us{0}; + uint16_t id; + bool repeating{false}; + }; + + static constexpr std::size_t kMaxTasks = 16; + static_assert((kMaxTasks & (kMaxTasks - 1)) == 0, "kMaxTasks must be a power of two"); + static constexpr uint32_t FREQUENCY = 1'000'000u; // 1 MHz -> 1us precision + + static std::array tasks_; + static_assert( + kMaxTasks == 16, + "kMaxTasks must be 16, if more is needed, sorted_task_ids_ must change" + ); + /* sorted_task_ids_ is a sorted queue with 4bits for each id in the + * scheduler's current ids */ + static uint64_t sorted_task_ids_; + + static uint32_t active_task_count_; + static_assert( + kMaxTasks <= 32, + "kMaxTasks must be <= 32, if more is needed, the bitmaps must change" + ); + static uint32_t ready_bitmap_; + static uint32_t free_bitmap_; + static uint64_t global_tick_us_; + static uint32_t current_interval_us_; + static uint16_t timeout_idx_; + + static inline uint8_t allocate_slot(); + static inline void release_slot(uint8_t id); + static void insert_sorted(uint8_t id); + static void remove_sorted(uint8_t id); + static void schedule_next_interval(); + static inline void configure_timer_for_interval(uint32_t microseconds); + + // helpers + static inline uint8_t get_at(uint8_t idx); + static inline void set_at(uint8_t idx, uint8_t id); + static inline void pop_front(); + static inline uint8_t front_id(); + + static inline void global_timer_disable(); + static inline void global_timer_enable(); }; diff --git a/Inc/HALAL/Services/Time/Time.hpp b/Inc/HALAL/Services/Time/Time.hpp index 9134947a5..d613e3fe8 100644 --- a/Inc/HALAL/Services/Time/Time.hpp +++ b/Inc/HALAL/Services/Time/Time.hpp @@ -16,133 +16,137 @@ #include "C++Utilities/CppUtils.hpp" #include "HALAL/Services/Time/RTC.hpp" // HIGH RESOLUTION TIMERS -extern TIM_HandleTypeDef htim2; // Used for the global timer (3,36nS step) -extern TIM_HandleTypeDef htim5; // Used for the high precision alarms (1uS) +extern TIM_HandleTypeDef htim2; // Used for the global timer (3,36nS step) +extern TIM_HandleTypeDef htim5; // Used for the high precision alarms (1uS) // MID RESOLUTION TIMERS -extern TIM_HandleTypeDef htim24; // Used for the high precision alarms (50uS) +extern TIM_HandleTypeDef htim24; // Used for the high precision alarms (50uS) // LOW RESOLUTION TIMERS -extern TIM_HandleTypeDef htim7; // Used for the low precision alarms (1mS) +extern TIM_HandleTypeDef htim7; // Used for the low precision alarms (1mS) class Time { -private : - static uint8_t high_precision_ids; - static uint8_t low_precision_ids; - static uint8_t mid_precision_ids; - - struct Alarm { - uint32_t period; - TIM_HandleTypeDef* tim; - function alarm; - uint64_t offset; - bool is_on = false; - }; - - - - static constexpr uint32_t HIGH_PRECISION_MAX_ARR = 4294967295; - static constexpr uint32_t MID_PRECISION_MAX_ARR = 4294967295; - static constexpr uint32_t mid_precision_step_in_us = 50; - static uint64_t global_tick; - static uint64_t low_precision_tick; - static uint64_t mid_precision_tick; - static bool mid_precision_registered; - - static unordered_map timer32_by_timer32handler_map; - static unordered_map timer32interrupt_by_timer32handler_map; - - static unordered_map high_precision_alarms_by_id; - static unordered_map high_precision_alarms_by_timer; - static unordered_map low_precision_alarms_by_id; - static unordered_map mid_precision_alarms_by_id; - - static stack low_precision_erasable_ids; - static stack mid_precision_erasable_ids; - - static void stop_timer(TIM_HandleTypeDef* htim); - static void start_timer(TIM_HandleTypeDef* htim,uint32_t prescaler, uint32_t period); - static void init_timer(TIM_TypeDef* tim, TIM_HandleTypeDef* htim,uint32_t prescaler, uint32_t period, IRQn_Type interrupt_channel); - static void ConfigTimer(TIM_HandleTypeDef* tim, uint32_t period_in_us); - static bool is_valid_timer(TIM_HandleTypeDef* tim); - static void hal_enable_timer(TIM_HandleTypeDef* tim); - -public : - static TIM_HandleTypeDef* global_timer; - - static set high_precision_timers; - static stack available_high_precision_timers; - - static TIM_HandleTypeDef* low_precision_timer; - static TIM_HandleTypeDef* mid_precision_timer; - - static void high_precision_timer_callback(TIM_HandleTypeDef* tim); - static void global_timer_callback(); - static void low_precision_timer_callback(); - static void mid_precision_timer_callback(); - - /** - * @brief Initializes instances registered with init_timer. - * - * @return void - */ - static void start(); - - static uint64_t get_global_tick(); - - /** - * @brief Tries to register a high_precision_alarm that will execute a function cyclically - * until unregistered. - * - * @param period_in_us period in microseconds until timeout. - * @param func function to be executed on timeout. - * @return optional Returns id of the alarm if succesful, nullopt if it is able - * to register the timer correctly (probably because there aren't any timer available). - */ - static uint8_t register_high_precision_alarm(uint32_t period_in_us, function func); - - static bool unregister_high_precision_alarm(uint8_t id); - - /** - * @brief Registers a low_precision_alarm that will execute a function cyclically until unregistered. - * - * @param period_in_ms period in milliseconds until timeout. - * @param func function to be executed on timeout. - * @return uint8_t Returns id of the alarm. - */ - static uint8_t register_low_precision_alarm(uint32_t period_in_ms, function func); - static uint8_t register_low_precision_alarm(uint32_t period_in_ms, void(*func)()); - static bool unregister_low_precision_alarm(uint8_t id); - - /** - * @brief Registers a low_precision_alarm, executes an action ONLY ONE TIME - * and unregisters the alarm. - * - * @param milliseconds time until the action is executed. - * @param func function to be executed on timeout. - * @return void - */ - - static uint8_t register_mid_precision_alarm(uint32_t period_in_us, function func); - static bool unregister_mid_precision_alarm(uint8_t id); - - - /** - * @brief Creates a timeout that will execute a function after a specified time - * - * @param milliseconds the time to wait before executing - * @param callback the function to be executed - * @return uint8_t the id of the order, if it didnot succeed it will return 255 - */ - static uint8_t set_timeout(int milliseconds, function callback); - - /** - * @brief Cancels a timeout by derigstering the alarm bound to it - * - * @param id The id of the timeout to cancel - */ - static void cancel_timeout(uint8_t id); +private: + static uint8_t high_precision_ids; + static uint8_t low_precision_ids; + static uint8_t mid_precision_ids; + + struct Alarm { + uint32_t period; + TIM_HandleTypeDef* tim; + function alarm; + uint64_t offset; + bool is_on = false; + }; + + static constexpr uint32_t HIGH_PRECISION_MAX_ARR = 4294967295; + static constexpr uint32_t MID_PRECISION_MAX_ARR = 4294967295; + static constexpr uint32_t mid_precision_step_in_us = 50; + static uint64_t global_tick; + static uint64_t low_precision_tick; + static uint64_t mid_precision_tick; + static bool mid_precision_registered; + + static unordered_map timer32_by_timer32handler_map; + static unordered_map timer32interrupt_by_timer32handler_map; + + static unordered_map high_precision_alarms_by_id; + static unordered_map high_precision_alarms_by_timer; + static unordered_map low_precision_alarms_by_id; + static unordered_map mid_precision_alarms_by_id; + + static stack low_precision_erasable_ids; + static stack mid_precision_erasable_ids; + + static void stop_timer(TIM_HandleTypeDef* htim); + static void start_timer(TIM_HandleTypeDef* htim, uint32_t prescaler, uint32_t period); + static void init_timer( + TIM_TypeDef* tim, + TIM_HandleTypeDef* htim, + uint32_t prescaler, + uint32_t period, + IRQn_Type interrupt_channel + ); + static void ConfigTimer(TIM_HandleTypeDef* tim, uint32_t period_in_us); + static bool is_valid_timer(TIM_HandleTypeDef* tim); + static void hal_enable_timer(TIM_HandleTypeDef* tim); + +public: + static TIM_HandleTypeDef* global_timer; + + static set high_precision_timers; + static stack available_high_precision_timers; + + static TIM_HandleTypeDef* low_precision_timer; + static TIM_HandleTypeDef* mid_precision_timer; + + static void high_precision_timer_callback(TIM_HandleTypeDef* tim); + static void global_timer_callback(); + static void low_precision_timer_callback(); + static void mid_precision_timer_callback(); + + /** + * @brief Initializes instances registered with init_timer. + * + * @return void + */ + static void start(); + + static uint64_t get_global_tick(); + + /** + * @brief Tries to register a high_precision_alarm that will execute a function cyclically + * until unregistered. + * + * @param period_in_us period in microseconds until timeout. + * @param func function to be executed on timeout. + * @return optional Returns id of the alarm if succesful, nullopt if it is able + * to register the timer correctly (probably because there aren't any timer available). + */ + static uint8_t register_high_precision_alarm(uint32_t period_in_us, function func); + + static bool unregister_high_precision_alarm(uint8_t id); + + /** + * @brief Registers a low_precision_alarm that will execute a function cyclically until + * unregistered. + * + * @param period_in_ms period in milliseconds until timeout. + * @param func function to be executed on timeout. + * @return uint8_t Returns id of the alarm. + */ + static uint8_t register_low_precision_alarm(uint32_t period_in_ms, function func); + static uint8_t register_low_precision_alarm(uint32_t period_in_ms, void (*func)()); + static bool unregister_low_precision_alarm(uint8_t id); + + /** + * @brief Registers a low_precision_alarm, executes an action ONLY ONE TIME + * and unregisters the alarm. + * + * @param milliseconds time until the action is executed. + * @param func function to be executed on timeout. + * @return void + */ + + static uint8_t register_mid_precision_alarm(uint32_t period_in_us, function func); + static bool unregister_mid_precision_alarm(uint8_t id); + + /** + * @brief Creates a timeout that will execute a function after a specified time + * + * @param milliseconds the time to wait before executing + * @param callback the function to be executed + * @return uint8_t the id of the order, if it didnot succeed it will return 255 + */ + static uint8_t set_timeout(int milliseconds, function callback); + + /** + * @brief Cancels a timeout by derigstering the alarm bound to it + * + * @param id The id of the timeout to cancel + */ + static void cancel_timeout(uint8_t id); }; #endif diff --git a/Inc/HALAL/Services/Time/TimerWrapper.hpp b/Inc/HALAL/Services/Time/TimerWrapper.hpp index 3eeaae34d..70a13f5f5 100644 --- a/Inc/HALAL/Services/Time/TimerWrapper.hpp +++ b/Inc/HALAL/Services/Time/TimerWrapper.hpp @@ -7,7 +7,7 @@ #pragma once -#include "hal_wrapper.h" +#include "hal_wrapper.h" #ifdef HAL_TIM_MODULE_ENABLED @@ -19,16 +19,15 @@ #include "ErrorHandler/ErrorHandler.hpp" -#define get_timer_instance(board, timer_type) \ +#define get_timer_instance(board, timer_type) \ ST_LIB::TimerWrapper(&board::instance_of()) namespace ST_LIB { -template -struct TimerWrapper { - TimerDomain::Instance *instance = nullptr; +template struct TimerWrapper { + TimerDomain::Instance* instance = nullptr; TimerWrapper() = default; - TimerWrapper(TimerDomain::Instance *inst) : instance(inst) {} + TimerWrapper(TimerDomain::Instance* inst) : instance(inst) {} static constexpr int MAX_pwm_channel_duties = 4; float pwm_channel_duties[MAX_pwm_channel_duties] = {0.0f, 0.0f, 0.0f, 0.0f}; @@ -38,274 +37,299 @@ struct TimerWrapper { UP = 0, DOWN = 1, /* center-aligned = counter counts up and down alternatively */ - /* Output compare interrupt flags of channels configured in output (CCxS=00 in TIMx_CCMRx register) are - set only when the counter is counting down */ + /* Output compare interrupt flags of channels configured in output (CCxS=00 in TIMx_CCMRx + register) are set only when the counter is counting down */ CENTER_ALIGNED_INTERRUPT_DOWN = 2, - /* Output compare interrupt flags of channels configured in output (CCxS=00 in TIMx_CCMRx register) are - set only when the counter is counting up */ + /* Output compare interrupt flags of channels configured in output (CCxS=00 in TIMx_CCMRx + register) are set only when the counter is counting up */ CENTER_ALIGNED_INTERRUPT_UP = 3, /* both up and down */ CENTER_ALIGNED_INTERRUPT_BOTH = 4, }; - static constexpr bool is_32bit_instance = ( - dev.e.request == TimerRequest::GeneralPurpose32bit_2 || - dev.e.request == TimerRequest::GeneralPurpose32bit_5 || - dev.e.request == TimerRequest::GeneralPurpose32bit_23 || - dev.e.request == TimerRequest::GeneralPurpose32bit_24 || - dev.e.request == TimerRequest::Any32bit - ); + static constexpr bool is_32bit_instance = + (dev.e.request == TimerRequest::GeneralPurpose32bit_2 || + dev.e.request == TimerRequest::GeneralPurpose32bit_5 || + dev.e.request == TimerRequest::GeneralPurpose32bit_23 || + dev.e.request == TimerRequest::GeneralPurpose32bit_24 || + dev.e.request == TimerRequest::Any32bit); /* supports break input */ - static constexpr bool is_break_instance = ( - dev.e.request == TimerRequest::Advanced_1 || - dev.e.request == TimerRequest::Advanced_8 || - dev.e.request == TimerRequest::GeneralPurpose_15 || - dev.e.request == TimerRequest::GeneralPurpose_16 || - dev.e.request == TimerRequest::GeneralPurpose_17 - ); - static constexpr bool is_slave_instance = ( - dev.e.request == TimerRequest::Advanced_1 || - dev.e.request == TimerRequest::GeneralPurpose32bit_2 || - dev.e.request == TimerRequest::GeneralPurpose_3 || - dev.e.request == TimerRequest::GeneralPurpose_4 || - dev.e.request == TimerRequest::GeneralPurpose32bit_5 || - dev.e.request == TimerRequest::Advanced_8 || - dev.e.request == TimerRequest::SlaveTimer_12 || - dev.e.request == TimerRequest::GeneralPurpose_15 || - dev.e.request == TimerRequest::GeneralPurpose32bit_23 || - dev.e.request == TimerRequest::GeneralPurpose32bit_24 - ); - static constexpr bool only_supports_upcounting = ( - dev.e.request == TimerRequest::Basic_6 || - dev.e.request == TimerRequest::Basic_7 || - dev.e.request == TimerRequest::SlaveTimer_12 || - dev.e.request == TimerRequest::SlaveTimer_13 || - dev.e.request == TimerRequest::SlaveTimer_14 || - dev.e.request == TimerRequest::GeneralPurpose_15 || - dev.e.request == TimerRequest::GeneralPurpose_16 || - dev.e.request == TimerRequest::GeneralPurpose_17 - ); - static constexpr bool is_on_APB1 = ( - dev.e.request == TimerRequest::GeneralPurpose32bit_2 || - dev.e.request == TimerRequest::GeneralPurpose_3 || - dev.e.request == TimerRequest::GeneralPurpose_4 || - dev.e.request == TimerRequest::GeneralPurpose32bit_5 || - dev.e.request == TimerRequest::Basic_6 || - dev.e.request == TimerRequest::Basic_7 || - dev.e.request == TimerRequest::SlaveTimer_12 || - dev.e.request == TimerRequest::SlaveTimer_13 || - dev.e.request == TimerRequest::SlaveTimer_14 - ); + static constexpr bool is_break_instance = + (dev.e.request == TimerRequest::Advanced_1 || dev.e.request == TimerRequest::Advanced_8 || + dev.e.request == TimerRequest::GeneralPurpose_15 || + dev.e.request == TimerRequest::GeneralPurpose_16 || + dev.e.request == TimerRequest::GeneralPurpose_17); + static constexpr bool is_slave_instance = + (dev.e.request == TimerRequest::Advanced_1 || + dev.e.request == TimerRequest::GeneralPurpose32bit_2 || + dev.e.request == TimerRequest::GeneralPurpose_3 || + dev.e.request == TimerRequest::GeneralPurpose_4 || + dev.e.request == TimerRequest::GeneralPurpose32bit_5 || + dev.e.request == TimerRequest::Advanced_8 || + dev.e.request == TimerRequest::SlaveTimer_12 || + dev.e.request == TimerRequest::GeneralPurpose_15 || + dev.e.request == TimerRequest::GeneralPurpose32bit_23 || + dev.e.request == TimerRequest::GeneralPurpose32bit_24); + static constexpr bool only_supports_upcounting = + (dev.e.request == TimerRequest::Basic_6 || dev.e.request == TimerRequest::Basic_7 || + dev.e.request == TimerRequest::SlaveTimer_12 || + dev.e.request == TimerRequest::SlaveTimer_13 || + dev.e.request == TimerRequest::SlaveTimer_14 || + dev.e.request == TimerRequest::GeneralPurpose_15 || + dev.e.request == TimerRequest::GeneralPurpose_16 || + dev.e.request == TimerRequest::GeneralPurpose_17); + static constexpr bool is_on_APB1 = + (dev.e.request == TimerRequest::GeneralPurpose32bit_2 || + dev.e.request == TimerRequest::GeneralPurpose_3 || + dev.e.request == TimerRequest::GeneralPurpose_4 || + dev.e.request == TimerRequest::GeneralPurpose32bit_5 || + dev.e.request == TimerRequest::Basic_6 || dev.e.request == TimerRequest::Basic_7 || + dev.e.request == TimerRequest::SlaveTimer_12 || + dev.e.request == TimerRequest::SlaveTimer_13 || + dev.e.request == TimerRequest::SlaveTimer_14); /* returns if the channel can be negated or not {See TimerDomain get_gpio_af()} */ static consteval bool is_ccxn_instance(ST_LIB::TimerChannel ch) { - switch(dev.e.request) { - case TimerRequest::Advanced_1: - case TimerRequest::Advanced_8: - return (ch == TimerChannel::CHANNEL_1) || (ch == TimerChannel::CHANNEL_1_NEGATED) || - (ch == TimerChannel::CHANNEL_2) || (ch == TimerChannel::CHANNEL_2_NEGATED) || - (ch == TimerChannel::CHANNEL_3) || (ch == TimerChannel::CHANNEL_3_NEGATED); - - case TimerRequest::GeneralPurpose_15: - case TimerRequest::GeneralPurpose_16: - case TimerRequest::GeneralPurpose_17: - return (ch == TimerChannel::CHANNEL_1) || (ch == TimerChannel::CHANNEL_1_NEGATED); - - default: - return false; + switch (dev.e.request) { + case TimerRequest::Advanced_1: + case TimerRequest::Advanced_8: + return (ch == TimerChannel::CHANNEL_1) || (ch == TimerChannel::CHANNEL_1_NEGATED) || + (ch == TimerChannel::CHANNEL_2) || (ch == TimerChannel::CHANNEL_2_NEGATED) || + (ch == TimerChannel::CHANNEL_3) || (ch == TimerChannel::CHANNEL_3_NEGATED); + + case TimerRequest::GeneralPurpose_15: + case TimerRequest::GeneralPurpose_16: + case TimerRequest::GeneralPurpose_17: + return (ch == TimerChannel::CHANNEL_1) || (ch == TimerChannel::CHANNEL_1_NEGATED); + + default: + return false; } } inline uint32_t get_clock_frequency() { uint32_t result; - if constexpr(this->is_on_APB1) { + if constexpr (this->is_on_APB1) { result = HAL_RCC_GetPCLK1Freq(); - if((RCC->D2CFGR & RCC_D2CFGR_D2PPRE1) != RCC_HCLK_DIV1) { + if ((RCC->D2CFGR & RCC_D2CFGR_D2PPRE1) != RCC_HCLK_DIV1) { result *= 2; } } else { result = HAL_RCC_GetPCLK2Freq(); - if((RCC->D2CFGR & RCC_D2CFGR_D2PPRE2) != RCC_HCLK_DIV1) { + if ((RCC->D2CFGR & RCC_D2CFGR_D2PPRE2) != RCC_HCLK_DIV1) { result *= 2; } } return result; } - template - inline PWM get_pwm(uint32_t polarity = TIM_OCPOLARITY_HIGH, uint32_t negated_polarity = TIM_OCNPOLARITY_HIGH) { + template + inline PWM get_pwm( + uint32_t polarity = TIM_OCPOLARITY_HIGH, + uint32_t negated_polarity = TIM_OCNPOLARITY_HIGH + ) { static_assert(dev.e.pin_count > 0, "Need at least one pin to get a pwm"); - - if constexpr(dev.e.pins[0].pin == pin.pin && dev.e.pins[0].channel == pin.channel) { - if constexpr(dev.e.pins[0].af != TimerAF::PWM) { + + if constexpr (dev.e.pins[0].pin == pin.pin && dev.e.pins[0].channel == pin.channel) { + if constexpr (dev.e.pins[0].af != TimerAF::PWM) { ST_LIB::compile_error("Pin must be configured in TimerDomain as a PWM"); } - return PWM(this, polarity, negated_polarity, - &pwm_channel_duties[(static_cast(pin.channel) & - ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - 1], &pwm_frequency); - } else if constexpr(dev.e.pin_count > 1 && - dev.e.pins[1].pin == pin.pin && dev.e.pins[1].channel == pin.channel) - { - if constexpr(dev.e.pins[1].af != TimerAF::PWM) { + return PWM( + this, + polarity, + negated_polarity, + &pwm_channel_duties + [(static_cast(pin.channel) & + ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - + 1], + &pwm_frequency + ); + } else if constexpr (dev.e.pin_count > 1 && dev.e.pins[1].pin == pin.pin && dev.e.pins[1].channel == pin.channel) { + if constexpr (dev.e.pins[1].af != TimerAF::PWM) { ST_LIB::compile_error("Pin must be configured in TimerDomain as a PWM"); } - return PWM(this, polarity, negated_polarity, - &pwm_channel_duties[(static_cast(pin.channel) & - ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - 1], &pwm_frequency); - } else if constexpr(dev.e.pin_count > 2 && - dev.e.pins[2].pin == pin.pin && dev.e.pins[2].channel == pin.channel) - { - if constexpr(dev.e.pins[2].af != TimerAF::PWM) { + return PWM( + this, + polarity, + negated_polarity, + &pwm_channel_duties + [(static_cast(pin.channel) & + ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - + 1], + &pwm_frequency + ); + } else if constexpr (dev.e.pin_count > 2 && dev.e.pins[2].pin == pin.pin && dev.e.pins[2].channel == pin.channel) { + if constexpr (dev.e.pins[2].af != TimerAF::PWM) { ST_LIB::compile_error("Pin must be configured in TimerDomain as a PWM"); } - return PWM(this, polarity, negated_polarity, - &pwm_channel_duties[(static_cast(pin.channel) & - ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - 1], &pwm_frequency); - } else if constexpr(dev.e.pin_count > 3 && - dev.e.pins[3].pin == pin.pin && dev.e.pins[3].channel == pin.channel) - { - if constexpr(dev.e.pins[3].af != TimerAF::PWM) { + return PWM( + this, + polarity, + negated_polarity, + &pwm_channel_duties + [(static_cast(pin.channel) & + ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - + 1], + &pwm_frequency + ); + } else if constexpr (dev.e.pin_count > 3 && dev.e.pins[3].pin == pin.pin && dev.e.pins[3].channel == pin.channel) { + if constexpr (dev.e.pins[3].af != TimerAF::PWM) { ST_LIB::compile_error("Pin must be configured in TimerDomain as a PWM"); } - return PWM(this, polarity, negated_polarity, - &pwm_channel_duties[(static_cast(pin.channel) & - ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - 1], &pwm_frequency); - } else if constexpr(dev.e.pin_count > 4 && - dev.e.pins[4].pin == pin.pin && dev.e.pins[4].channel == pin.channel) - { - if constexpr(dev.e.pins[4].af != TimerAF::PWM) { + return PWM( + this, + polarity, + negated_polarity, + &pwm_channel_duties + [(static_cast(pin.channel) & + ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - + 1], + &pwm_frequency + ); + } else if constexpr (dev.e.pin_count > 4 && dev.e.pins[4].pin == pin.pin && dev.e.pins[4].channel == pin.channel) { + if constexpr (dev.e.pins[4].af != TimerAF::PWM) { ST_LIB::compile_error("Pin must be configured in TimerDomain as a PWM"); } - return PWM(this, polarity, negated_polarity, - &pwm_channel_duties[(static_cast(pin.channel) & - ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - 1], &pwm_frequency); - } else if constexpr(dev.e.pin_count > 5 && - dev.e.pins[5].pin == pin.pin && dev.e.pins[5].channel == pin.channel) - { - if constexpr(dev.e.pins[5].af != TimerAF::PWM) { + return PWM( + this, + polarity, + negated_polarity, + &pwm_channel_duties + [(static_cast(pin.channel) & + ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - + 1], + &pwm_frequency + ); + } else if constexpr (dev.e.pin_count > 5 && dev.e.pins[5].pin == pin.pin && dev.e.pins[5].channel == pin.channel) { + if constexpr (dev.e.pins[5].af != TimerAF::PWM) { ST_LIB::compile_error("Pin must be configured in TimerDomain as a PWM"); } - return PWM(this, polarity, negated_polarity, - &pwm_channel_duties[(static_cast(pin.channel) & - ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - 1], &pwm_frequency); - } else if constexpr(dev.e.pin_count > 6 && - dev.e.pins[6].pin == pin.pin && dev.e.pins[6].channel == pin.channel) - { - if constexpr(dev.e.pins[6].af != TimerAF::PWM) { + return PWM( + this, + polarity, + negated_polarity, + &pwm_channel_duties + [(static_cast(pin.channel) & + ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - + 1], + &pwm_frequency + ); + } else if constexpr (dev.e.pin_count > 6 && dev.e.pins[6].pin == pin.pin && dev.e.pins[6].channel == pin.channel) { + if constexpr (dev.e.pins[6].af != TimerAF::PWM) { ST_LIB::compile_error("Pin must be configured in TimerDomain as a PWM"); } - return PWM(this, polarity, negated_polarity, - &pwm_channel_duties[(static_cast(pin.channel) & - ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - 1], &pwm_frequency); - } else if constexpr(dev.e.pin_count == 7 && - dev.e.pins[7].pin == pin.pin && dev.e.pins[7].channel == pin.channel) - { - if constexpr(dev.e.pins[7].af != TimerAF::PWM) { + return PWM( + this, + polarity, + negated_polarity, + &pwm_channel_duties + [(static_cast(pin.channel) & + ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - + 1], + &pwm_frequency + ); + } else if constexpr (dev.e.pin_count == 7 && dev.e.pins[7].pin == pin.pin && dev.e.pins[7].channel == pin.channel) { + if constexpr (dev.e.pins[7].af != TimerAF::PWM) { ST_LIB::compile_error("Pin must be configured in TimerDomain as a PWM"); } - return PWM(this, polarity, negated_polarity, - &pwm_channel_duties[(static_cast(pin.channel) & - ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - 1], &pwm_frequency); + return PWM( + this, + polarity, + negated_polarity, + &pwm_channel_duties + [(static_cast(pin.channel) & + ~static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) - + 1], + &pwm_frequency + ); } else { - ST_LIB::compile_error("No pins passed to TimerWrapper are the same as the pins passed to get_pwm() [this method]"); + ST_LIB::compile_error("No pins passed to TimerWrapper are the same as the pins passed " + "to get_pwm() [this method]"); } } - template - inline DualPWM get_dual_pwm(uint32_t polarity = TIM_OCPOLARITY_HIGH, uint32_t negated_polarity = TIM_OCNPOLARITY_HIGH) - { + template + inline DualPWM get_dual_pwm( + uint32_t polarity = TIM_OCPOLARITY_HIGH, + uint32_t negated_polarity = TIM_OCNPOLARITY_HIGH + ) { static_assert(dev.e.pin_count > 1, "Need at least two pins on one timer to get a dual pwm"); - // NOTE: No voy a comprobar todo esto, no hará falta comprobarlo con la siguiente versión del pwm, dualpwm - static_assert((static_cast(pin.channel) | static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) == static_cast(negated_pin.channel)); + // NOTE: No voy a comprobar todo esto, no hará falta comprobarlo con la siguiente versión + // del pwm, dualpwm + static_assert( + (static_cast(pin.channel) | + static_cast(TimerChannel::CHANNEL_NEGATED_FLAG)) == + static_cast(negated_pin.channel) + ); static_assert(pin.af == TimerAF::PWM && negated_pin.af == TimerAF::PWM); - return DualPWM(this, polarity, negated_polarity, - &pwm_channel_duties[static_cast(pin.channel) - 1], &pwm_frequency); + return DualPWM( + this, + polarity, + negated_polarity, + &pwm_channel_duties[static_cast(pin.channel) - 1], + &pwm_frequency + ); } - inline Encoder get_encoder() - { - return Encoder(this); - } + inline Encoder get_encoder() { return Encoder(this); } - inline void counter_enable() { - SET_BIT(instance->tim->CR1, TIM_CR1_CEN); - } - inline void counter_disable() { - CLEAR_BIT(instance->tim->CR1, TIM_CR1_CEN); - } + inline void counter_enable() { SET_BIT(instance->tim->CR1, TIM_CR1_CEN); } + inline void counter_disable() { CLEAR_BIT(instance->tim->CR1, TIM_CR1_CEN); } - inline void clear_update_interrupt_flag() { - CLEAR_BIT(instance->tim->SR, TIM_SR_UIF); - } + inline void clear_update_interrupt_flag() { CLEAR_BIT(instance->tim->SR, TIM_SR_UIF); } /* Disabled by default */ - inline void enable_update_interrupt() { - SET_BIT(instance->tim->DIER, TIM_DIER_UIE); - } - inline void disable_update_interrupt() { - CLEAR_BIT(instance->tim->DIER, TIM_DIER_UIE); - } + inline void enable_update_interrupt() { SET_BIT(instance->tim->DIER, TIM_DIER_UIE); } + inline void disable_update_interrupt() { CLEAR_BIT(instance->tim->DIER, TIM_DIER_UIE); } /* Disabled by default */ - inline void enable_nvic() { - NVIC_EnableIRQ(TimerDomain::timer_irqn[instance->timer_idx]); - } - inline void disable_nvic() { - NVIC_DisableIRQ(TimerDomain::timer_irqn[instance->timer_idx]); - } + inline void enable_nvic() { NVIC_EnableIRQ(TimerDomain::timer_irqn[instance->timer_idx]); } + inline void disable_nvic() { NVIC_DisableIRQ(TimerDomain::timer_irqn[instance->timer_idx]); } /* Enable UEV. The Update (UEV) event is generated by one of the following events: * – Counter overflow/underflow * – Setting the UG bit - * – Update generation through the slave mode controller + * – Update generation through the slave mode controller * Enabled by default */ - inline void enable_update_event() { - CLEAR_BIT(instance->tim->CR1, TIM_CR1_UDIS); - } + inline void enable_update_event() { CLEAR_BIT(instance->tim->CR1, TIM_CR1_UDIS); } /* Disable UEV. The Update event is not generated, shadow registers keep their value * (ARR, PSC, CCRx). However the counter and the prescaler are reinitialized if the UG bit * is set or if a hardware reset is received from the slave mode controller. */ - inline void disable_update_event() { - SET_BIT(instance->tim->CR1, TIM_CR1_UDIS); - } + inline void disable_update_event() { SET_BIT(instance->tim->CR1, TIM_CR1_UDIS); } /* default: disabled */ inline void break_interrupt_enable() { - static_assert(dev.e.request == TimerRequest::Advanced_1 || dev.e.request == TimerRequest::Advanced_8, - "Error: Break interrupt enable only allowed in advanced timers {TIM1, TIM8}"); + static_assert( + dev.e.request == TimerRequest::Advanced_1 || dev.e.request == TimerRequest::Advanced_8, + "Error: Break interrupt enable only allowed in advanced timers {TIM1, TIM8}" + ); SET_BIT(instance->tim->DIER, TIM_DIER_BIE); } inline void break_interrupt_disable() { - static_assert(dev.e.request == TimerRequest::Advanced_1 || dev.e.request == TimerRequest::Advanced_8, - "Error: Break interrupt enable only allowed in advanced timers {TIM1, TIM8}"); + static_assert( + dev.e.request == TimerRequest::Advanced_1 || dev.e.request == TimerRequest::Advanced_8, + "Error: Break interrupt enable only allowed in advanced timers {TIM1, TIM8}" + ); CLEAR_BIT(instance->tim->DIER, TIM_DIER_BIE); } /* interrupt gets called only once, counter needs to be reenabled */ - inline void set_one_pulse_mode() { - SET_BIT(instance->tim->CR1, TIM_CR1_OPM); - } - inline void set_multi_interrupt_mode() { - CLEAR_BIT(instance->tim->CR1, TIM_CR1_OPM); - } + inline void set_one_pulse_mode() { SET_BIT(instance->tim->CR1, TIM_CR1_OPM); } + inline void set_multi_interrupt_mode() { CLEAR_BIT(instance->tim->CR1, TIM_CR1_OPM); } - inline TIM_HandleTypeDef *get_hal_handle() { - return instance->hal_tim; - } - inline TIM_TypeDef *get_cmsis_handle() { - return instance->tim; - } + inline TIM_HandleTypeDef* get_hal_handle() { return instance->hal_tim; } + inline TIM_TypeDef* get_cmsis_handle() { return instance->tim; } - inline void set_prescaler(uint16_t psc) { - instance->tim->PSC = psc; - } + inline void set_prescaler(uint16_t psc) { instance->tim->PSC = psc; } - inline void configure32bit(void (*callback)(void*), void *callback_data, uint32_t period) - { - static_assert(this->is_32bit_instance, "Only timers {TIM2, TIM5, TIM23, TIM24} have a 32-bit resolution"); + inline void configure32bit(void (*callback)(void*), void* callback_data, uint32_t period) { + static_assert( + this->is_32bit_instance, + "Only timers {TIM2, TIM5, TIM23, TIM24} have a 32-bit resolution" + ); instance->tim->ARR = period; TimerDomain::callbacks[instance->timer_idx] = callback; @@ -313,8 +337,7 @@ struct TimerWrapper { this->counter_enable(); } - inline void configure16bit(void (*callback)(void*), void *callback_data, uint16_t period) - { + inline void configure16bit(void (*callback)(void*), void* callback_data, uint16_t period) { instance->tim->ARR = period; TimerDomain::callbacks[instance->timer_idx] = callback; TimerDomain::callback_data[instance->timer_idx] = callback_data; @@ -322,11 +345,13 @@ struct TimerWrapper { } /* WARNING: The counter _must_ be disabled to switch from edge-aligned to center-aligned mode */ - template - inline void set_counting_mode(void) { + template inline void set_counting_mode(void) { constexpr uint8_t reqint = static_cast(dev.e.request); - static_assert(!this->only_supports_upcounting, - "Error: In request reqidx: Timers other than {Advanced{TIM1, TIM8}, TIM2, TIM3, TIM4, TIM5, TIM23, TIM24} only support upcounting"); + static_assert( + !this->only_supports_upcounting, + "Error: In request reqidx: Timers other than {Advanced{TIM1, TIM8}, TIM2, TIM3, TIM4, " + "TIM5, TIM23, TIM24} only support upcounting" + ); if constexpr (mode == CountingMode::UP) { MODIFY_REG(instance->tim->CR1, TIM_CR1_CMS, 0); @@ -346,67 +371,76 @@ struct TimerWrapper { /////////////////////////////////////////// // Below are methods used by other objects - template + template void set_pwm_frequency(uint32_t frequency) { /* If it's center aligned duplicate the frequency */ - if((instance->tim->CR1 & TIM_CR1_CMS) != 0) { - frequency = 2*frequency; + if ((instance->tim->CR1 & TIM_CR1_CMS) != 0) { + frequency = 2 * frequency; } pwm_frequency = frequency; - if constexpr(mode == ST_LIB::PWM_Frequency_Mode::SPEED) { - instance->tim->ARR = (this->get_clock_frequency() / (instance->tim->PSC + 1)) / frequency; - } else if constexpr(mode == ST_LIB::PWM_Frequency_Mode::PRECISION) { + if constexpr (mode == ST_LIB::PWM_Frequency_Mode::SPEED) { + instance->tim->ARR = + (this->get_clock_frequency() / (instance->tim->PSC + 1)) / frequency; + } else if constexpr (mode == ST_LIB::PWM_Frequency_Mode::PRECISION) { float psc_plus_1_mul_freq = (float)(instance->tim->PSC + 1) * (float)frequency; - instance->tim->ARR = (uint32_t)((float)get_clock_frequency() / psc_plus_1_mul_freq - 0.5f); + instance->tim->ARR = + (uint32_t)((float)get_clock_frequency() / psc_plus_1_mul_freq - 0.5f); } - if(pwm_channel_duties[0] != 0.0f) { - if constexpr(this->is_32bit_instance) { - uint32_t raw_duty = (uint32_t)((float)(instance->tim->ARR + 1) * (pwm_channel_duties[0] / 100.0f)); + if (pwm_channel_duties[0] != 0.0f) { + if constexpr (this->is_32bit_instance) { + uint32_t raw_duty = + (uint32_t)((float)(instance->tim->ARR + 1) * (pwm_channel_duties[0] / 100.0f)); instance->tim->CCR1 = raw_duty; } else { - uint16_t raw_duty = (uint16_t)((float)(instance->tim->ARR + 1) * (pwm_channel_duties[0] / 100.0f)); + uint16_t raw_duty = + (uint16_t)((float)(instance->tim->ARR + 1) * (pwm_channel_duties[0] / 100.0f)); instance->tim->CCR1 = raw_duty; - } + } } - if(pwm_channel_duties[1] != 0.0f) { - if constexpr(this->is_32bit_instance) { - uint32_t raw_duty = (uint32_t)((float)(instance->tim->ARR + 1) * (pwm_channel_duties[1] / 100.0f)); + if (pwm_channel_duties[1] != 0.0f) { + if constexpr (this->is_32bit_instance) { + uint32_t raw_duty = + (uint32_t)((float)(instance->tim->ARR + 1) * (pwm_channel_duties[1] / 100.0f)); instance->tim->CCR2 = raw_duty; } else { - uint16_t raw_duty = (uint16_t)((float)(instance->tim->ARR + 1) * (pwm_channel_duties[1] / 100.0f)); + uint16_t raw_duty = + (uint16_t)((float)(instance->tim->ARR + 1) * (pwm_channel_duties[1] / 100.0f)); instance->tim->CCR2 = raw_duty; - } + } } - if(pwm_channel_duties[2] != 0.0f) { - if constexpr(this->is_32bit_instance) { - uint32_t raw_duty = (uint32_t)((float)(instance->tim->ARR + 1) * (pwm_channel_duties[2] / 100.0f)); + if (pwm_channel_duties[2] != 0.0f) { + if constexpr (this->is_32bit_instance) { + uint32_t raw_duty = + (uint32_t)((float)(instance->tim->ARR + 1) * (pwm_channel_duties[2] / 100.0f)); instance->tim->CCR3 = raw_duty; } else { - uint16_t raw_duty = (uint16_t)((float)(instance->tim->ARR + 1) * (pwm_channel_duties[2] / 100.0f)); + uint16_t raw_duty = + (uint16_t)((float)(instance->tim->ARR + 1) * (pwm_channel_duties[2] / 100.0f)); instance->tim->CCR3 = raw_duty; - } + } } - if(pwm_channel_duties[3] != 0.0f) { - if constexpr(this->is_32bit_instance) { - uint32_t raw_duty = (uint32_t)((float)(instance->tim->ARR + 1) * (pwm_channel_duties[3] / 100.0f)); + if (pwm_channel_duties[3] != 0.0f) { + if constexpr (this->is_32bit_instance) { + uint32_t raw_duty = + (uint32_t)((float)(instance->tim->ARR + 1) * (pwm_channel_duties[3] / 100.0f)); instance->tim->CCR4 = raw_duty; } else { - uint16_t raw_duty = (uint16_t)((float)(instance->tim->ARR + 1) * (pwm_channel_duties[3] / 100.0f)); + uint16_t raw_duty = + (uint16_t)((float)(instance->tim->ARR + 1) * (pwm_channel_duties[3] / 100.0f)); instance->tim->CCR4 = raw_duty; - } + } } } - template - inline void config_output_compare_channel(const TIM_OC_InitTypeDef *OC_Config) - { - if constexpr(!((ch == TimerChannel::CHANNEL_1) || (ch == TimerChannel::CHANNEL_2) || - (ch == TimerChannel::CHANNEL_3) || (ch == TimerChannel::CHANNEL_4) || - (ch == TimerChannel::CHANNEL_5) || (ch == TimerChannel::CHANNEL_6))) - { - ST_LIB::compile_error("config_output_compare_channel should only be called with channels [1..6] (not negated)"); + template + inline void config_output_compare_channel(const TIM_OC_InitTypeDef* OC_Config) { + if constexpr (!((ch == TimerChannel::CHANNEL_1) || (ch == TimerChannel::CHANNEL_2) || + (ch == TimerChannel::CHANNEL_3) || (ch == TimerChannel::CHANNEL_4) || + (ch == TimerChannel::CHANNEL_5) || (ch == TimerChannel::CHANNEL_6))) { + ST_LIB::compile_error("config_output_compare_channel should only be called with " + "channels [1..6] (not negated)"); } uint32_t tmpccmrx; @@ -416,16 +450,16 @@ struct TimerWrapper { tmpccer = instance->tim->CCER; tmpcr2 = instance->tim->CR2; - - if constexpr(ch == TimerChannel::CHANNEL_1 || ch == TimerChannel::CHANNEL_2) { + + if constexpr (ch == TimerChannel::CHANNEL_1 || ch == TimerChannel::CHANNEL_2) { tmpccmrx = instance->tim->CCMR1; - } else if constexpr(ch == TimerChannel::CHANNEL_3 || ch == TimerChannel::CHANNEL_4) { + } else if constexpr (ch == TimerChannel::CHANNEL_3 || ch == TimerChannel::CHANNEL_4) { tmpccmrx = instance->tim->CCMR2; - } else if constexpr(ch == TimerChannel::CHANNEL_5 || ch == TimerChannel::CHANNEL_6) { + } else if constexpr (ch == TimerChannel::CHANNEL_5 || ch == TimerChannel::CHANNEL_6) { tmpccmrx = instance->tim->CCMR3; } - if constexpr(ch == TimerChannel::CHANNEL_1) { + if constexpr (ch == TimerChannel::CHANNEL_1) { CLEAR_BIT(instance->tim->CCER, TIM_CCER_CC1E); CLEAR_BIT(tmpccmrx, TIM_CCMR1_OC1M); @@ -434,7 +468,7 @@ struct TimerWrapper { SET_BIT(tmpccmrx, OC_Config->OCMode); CLEAR_BIT(tmpccer, TIM_CCER_CC1P); SET_BIT(tmpccer, OC_Config->OCPolarity); - } else if constexpr(ch == TimerChannel::CHANNEL_2) { + } else if constexpr (ch == TimerChannel::CHANNEL_2) { CLEAR_BIT(instance->tim->CCER, TIM_CCER_CC2E); CLEAR_BIT(tmpccmrx, TIM_CCMR1_OC2M); @@ -443,7 +477,7 @@ struct TimerWrapper { SET_BIT(tmpccmrx, OC_Config->OCMode << 8U); CLEAR_BIT(tmpccer, TIM_CCER_CC2P); SET_BIT(tmpccer, OC_Config->OCPolarity << 4U); - } else if constexpr(ch == TimerChannel::CHANNEL_3) { + } else if constexpr (ch == TimerChannel::CHANNEL_3) { CLEAR_BIT(instance->tim->CCER, TIM_CCER_CC3E); CLEAR_BIT(tmpccmrx, TIM_CCMR2_OC3M); @@ -452,7 +486,7 @@ struct TimerWrapper { SET_BIT(tmpccmrx, OC_Config->OCMode); CLEAR_BIT(tmpccer, TIM_CCER_CC3P); SET_BIT(tmpccer, OC_Config->OCPolarity << 8U); - } else if constexpr(ch == TimerChannel::CHANNEL_4) { + } else if constexpr (ch == TimerChannel::CHANNEL_4) { CLEAR_BIT(instance->tim->CCER, TIM_CCER_CC4E); CLEAR_BIT(tmpccmrx, TIM_CCMR2_OC4M); @@ -461,7 +495,7 @@ struct TimerWrapper { SET_BIT(tmpccmrx, OC_Config->OCMode); CLEAR_BIT(tmpccer, TIM_CCER_CC4P); SET_BIT(tmpccer, OC_Config->OCPolarity << 12U); - } else if constexpr(ch == TimerChannel::CHANNEL_5) { + } else if constexpr (ch == TimerChannel::CHANNEL_5) { CLEAR_BIT(instance->tim->CCER, TIM_CCER_CC5E); CLEAR_BIT(tmpccmrx, TIM_CCMR3_OC5M); @@ -469,7 +503,7 @@ struct TimerWrapper { SET_BIT(tmpccmrx, OC_Config->OCMode); CLEAR_BIT(tmpccer, TIM_CCER_CC5P); SET_BIT(tmpccer, OC_Config->OCPolarity << 16U); - } else if constexpr(ch == TimerChannel::CHANNEL_6) { + } else if constexpr (ch == TimerChannel::CHANNEL_6) { CLEAR_BIT(instance->tim->CCER, TIM_CCER_CC6E); CLEAR_BIT(tmpccmrx, TIM_CCMR3_OC6M); @@ -479,107 +513,112 @@ struct TimerWrapper { SET_BIT(tmpccer, OC_Config->OCPolarity << 20U); } - if(this->is_ccxn_instance(ch)) { + if (this->is_ccxn_instance(ch)) { assert_param(IS_TIM_OCN_POLARITY(OC_Config->OCNPolarity)); - if constexpr(ch == TimerChannel::CHANNEL_1) { + if constexpr (ch == TimerChannel::CHANNEL_1) { CLEAR_BIT(tmpccer, TIM_CCER_CC1NP); SET_BIT(tmpccer, OC_Config->OCNPolarity); CLEAR_BIT(tmpccer, TIM_CCER_CC1NE); - } else if constexpr(ch == TimerChannel::CHANNEL_2) { + } else if constexpr (ch == TimerChannel::CHANNEL_2) { CLEAR_BIT(tmpccer, TIM_CCER_CC2NP); SET_BIT(tmpccer, OC_Config->OCNPolarity << 4U); CLEAR_BIT(tmpccer, TIM_CCER_CC2NE); - } else if constexpr(ch == TimerChannel::CHANNEL_3) { + } else if constexpr (ch == TimerChannel::CHANNEL_3) { CLEAR_BIT(tmpccer, TIM_CCER_CC3NP); SET_BIT(tmpccer, OC_Config->OCNPolarity << 8U); CLEAR_BIT(tmpccer, TIM_CCER_CC3NE); } } - if(this->is_break_instance) { + if (this->is_break_instance) { assert_param(IS_TIM_OCNIDLE_STATE(OC_Config->OCNIdleState)); assert_param(IS_TIM_OCIDLE_STATE(OC_Config->OCIdleState)); - if constexpr(ch == TimerChannel::CHANNEL_1) { + if constexpr (ch == TimerChannel::CHANNEL_1) { CLEAR_BIT(tmpcr2, TIM_CR2_OIS1); CLEAR_BIT(tmpcr2, TIM_CR2_OIS1N); SET_BIT(tmpcr2, OC_Config->OCNIdleState); SET_BIT(tmpcr2, OC_Config->OCIdleState); - } else if constexpr(ch == TimerChannel::CHANNEL_2) { + } else if constexpr (ch == TimerChannel::CHANNEL_2) { CLEAR_BIT(tmpcr2, TIM_CR2_OIS2); CLEAR_BIT(tmpcr2, TIM_CR2_OIS2N); SET_BIT(tmpcr2, OC_Config->OCNIdleState << 2U); SET_BIT(tmpcr2, OC_Config->OCIdleState << 2U); - } else if constexpr(ch == TimerChannel::CHANNEL_3) { + } else if constexpr (ch == TimerChannel::CHANNEL_3) { CLEAR_BIT(tmpcr2, TIM_CR2_OIS3); CLEAR_BIT(tmpcr2, TIM_CR2_OIS3N); SET_BIT(tmpcr2, OC_Config->OCNIdleState << 4U); SET_BIT(tmpcr2, OC_Config->OCIdleState << 4U); - } else if constexpr(ch == TimerChannel::CHANNEL_4) { + } else if constexpr (ch == TimerChannel::CHANNEL_4) { CLEAR_BIT(tmpcr2, TIM_CR2_OIS4); SET_BIT(tmpcr2, OC_Config->OCIdleState << 6U); - } else if constexpr(ch == TimerChannel::CHANNEL_5) { + } else if constexpr (ch == TimerChannel::CHANNEL_5) { CLEAR_BIT(tmpcr2, TIM_CR2_OIS5); SET_BIT(tmpcr2, OC_Config->OCIdleState << 8U); - } else if constexpr(ch == TimerChannel::CHANNEL_6) { + } else if constexpr (ch == TimerChannel::CHANNEL_6) { CLEAR_BIT(tmpcr2, TIM_CR2_OIS6); SET_BIT(tmpcr2, OC_Config->OCIdleState << 10U); } } instance->tim->CR2 = tmpcr2; - if constexpr(ch == TimerChannel::CHANNEL_1 || ch == TimerChannel::CHANNEL_2) { + if constexpr (ch == TimerChannel::CHANNEL_1 || ch == TimerChannel::CHANNEL_2) { instance->tim->CCMR1 = tmpccmrx; - } else if constexpr(ch == TimerChannel::CHANNEL_3 || ch == TimerChannel::CHANNEL_4) { + } else if constexpr (ch == TimerChannel::CHANNEL_3 || ch == TimerChannel::CHANNEL_4) { instance->tim->CCMR2 = tmpccmrx; - } else if constexpr(ch == TimerChannel::CHANNEL_5 || ch == TimerChannel::CHANNEL_6) { + } else if constexpr (ch == TimerChannel::CHANNEL_5 || ch == TimerChannel::CHANNEL_6) { instance->tim->CCMR3 = tmpccmrx; } - if constexpr(ch == TimerChannel::CHANNEL_1) instance->tim->CCR1 = OC_Config->Pulse; - else if constexpr(ch == TimerChannel::CHANNEL_2) instance->tim->CCR2 = OC_Config->Pulse; - else if constexpr(ch == TimerChannel::CHANNEL_3) instance->tim->CCR3 = OC_Config->Pulse; - else if constexpr(ch == TimerChannel::CHANNEL_4) instance->tim->CCR4 = OC_Config->Pulse; - else if constexpr(ch == TimerChannel::CHANNEL_5) instance->tim->CCR5 = OC_Config->Pulse; - else if constexpr(ch == TimerChannel::CHANNEL_6) instance->tim->CCR6 = OC_Config->Pulse; + if constexpr (ch == TimerChannel::CHANNEL_1) + instance->tim->CCR1 = OC_Config->Pulse; + else if constexpr (ch == TimerChannel::CHANNEL_2) + instance->tim->CCR2 = OC_Config->Pulse; + else if constexpr (ch == TimerChannel::CHANNEL_3) + instance->tim->CCR3 = OC_Config->Pulse; + else if constexpr (ch == TimerChannel::CHANNEL_4) + instance->tim->CCR4 = OC_Config->Pulse; + else if constexpr (ch == TimerChannel::CHANNEL_5) + instance->tim->CCR5 = OC_Config->Pulse; + else if constexpr (ch == TimerChannel::CHANNEL_6) + instance->tim->CCR6 = OC_Config->Pulse; instance->tim->CCER = tmpccer; } - template - inline void set_output_compare_preload_enable() - { - if constexpr((ch == TimerChannel::CHANNEL_1) || (ch == TimerChannel::CHANNEL_1_NEGATED)) { + template inline void set_output_compare_preload_enable() { + if constexpr ((ch == TimerChannel::CHANNEL_1) || (ch == TimerChannel::CHANNEL_1_NEGATED)) { SET_BIT(instance->tim->CCMR1, TIM_CCMR1_OC1PE); - } else if constexpr((ch == TimerChannel::CHANNEL_2) || (ch == TimerChannel::CHANNEL_2_NEGATED)) { + } else if constexpr ((ch == TimerChannel::CHANNEL_2) || (ch == TimerChannel::CHANNEL_2_NEGATED)) { SET_BIT(instance->tim->CCMR1, TIM_CCMR1_OC2PE); - } else if constexpr((ch == TimerChannel::CHANNEL_3) || (ch == TimerChannel::CHANNEL_3_NEGATED)) { + } else if constexpr ((ch == TimerChannel::CHANNEL_3) || (ch == TimerChannel::CHANNEL_3_NEGATED)) { SET_BIT(instance->tim->CCMR2, TIM_CCMR2_OC3PE); - } else if constexpr(ch == TimerChannel::CHANNEL_4) { + } else if constexpr (ch == TimerChannel::CHANNEL_4) { SET_BIT(instance->tim->CCMR2, TIM_CCMR2_OC4PE); - } else if constexpr(ch == TimerChannel::CHANNEL_5) { + } else if constexpr (ch == TimerChannel::CHANNEL_5) { SET_BIT(instance->tim->CCMR3, TIM_CCMR3_OC5PE); - } else if constexpr(ch == TimerChannel::CHANNEL_6) { + } else if constexpr (ch == TimerChannel::CHANNEL_6) { SET_BIT(instance->tim->CCMR3, TIM_CCMR3_OC6PE); - } else ST_LIB::compile_error("Unknown timer channel, there are only 6 channels [1..6]"); + } else + ST_LIB::compile_error("Unknown timer channel, there are only 6 channels [1..6]"); } - template - inline void set_capture_compare(uint32_t val) { - if constexpr((ch == TimerChannel::CHANNEL_1) || (ch == TimerChannel::CHANNEL_1_NEGATED)) { + template inline void set_capture_compare(uint32_t val) { + if constexpr ((ch == TimerChannel::CHANNEL_1) || (ch == TimerChannel::CHANNEL_1_NEGATED)) { instance->tim->CCR1 = val; - } else if constexpr((ch == TimerChannel::CHANNEL_2) || (ch == TimerChannel::CHANNEL_2_NEGATED)) { + } else if constexpr ((ch == TimerChannel::CHANNEL_2) || (ch == TimerChannel::CHANNEL_2_NEGATED)) { instance->tim->CCR2 = val; - } else if constexpr((ch == TimerChannel::CHANNEL_3) || (ch == TimerChannel::CHANNEL_3_NEGATED)) { + } else if constexpr ((ch == TimerChannel::CHANNEL_3) || (ch == TimerChannel::CHANNEL_3_NEGATED)) { instance->tim->CCR3 = val; - } else if constexpr(ch == TimerChannel::CHANNEL_4) { + } else if constexpr (ch == TimerChannel::CHANNEL_4) { instance->tim->CCR4 = val; - } else if constexpr(ch == TimerChannel::CHANNEL_5) { + } else if constexpr (ch == TimerChannel::CHANNEL_5) { instance->tim->CCR5 = val; - } else if constexpr(ch == TimerChannel::CHANNEL_6) { + } else if constexpr (ch == TimerChannel::CHANNEL_6) { instance->tim->CCR6 = val; - } else ST_LIB::compile_error("Unknown timer channel, there are only 6 channels [1..6]"); + } else + ST_LIB::compile_error("Unknown timer channel, there are only 6 channels [1..6]"); } inline bool are_all_channels_free() { @@ -588,15 +627,9 @@ struct TimerWrapper { } // leftover from old TimerPeripheral, maybe this was useful? - inline uint16_t get_prescaler() { - return instance->tim->PSC; - } - inline uint32_t get_period() { - return instance->tim->ARR; - } + inline uint16_t get_prescaler() { return instance->tim->PSC; } + inline uint32_t get_period() { return instance->tim->ARR; } }; } // namespace ST_LIB #endif // HAL_TIM_MODULE_ENABLED - - diff --git a/Inc/HALAL/Services/Watchdog/Watchdog.hpp b/Inc/HALAL/Services/Watchdog/Watchdog.hpp index de29501e1..4a64476c5 100644 --- a/Inc/HALAL/Services/Watchdog/Watchdog.hpp +++ b/Inc/HALAL/Services/Watchdog/Watchdog.hpp @@ -14,9 +14,9 @@ extern bool reset_by_iwdg; * Watchdog is not active until it is started. */ class Watchdog { - public: +public: static std::chrono::microseconds watchdog_time; - + static void start() { if ((chrono::duration_cast(watchdog_time)).count() > 32000000) { ErrorHandler("Watchdog refresh interval is too big"); @@ -40,18 +40,14 @@ class Watchdog { HAL_IWDG_Init(&watchdog_handle); } - static void refresh() { - HAL_IWDG_Refresh(&watchdog_handle); - } - static void check_reset_flag(){ + static void refresh() { HAL_IWDG_Refresh(&watchdog_handle); } + static void check_reset_flag() { if (__HAL_RCC_GET_FLAG(RCC_FLAG_IWDG1RST)) { reset_by_iwdg = true; - } __HAL_RCC_CLEAR_RESET_FLAGS(); } - template - Watchdog(chrono::duration period) { + template Watchdog(chrono::duration period) { watchdog_time = std::chrono::duration_cast(period); } }; diff --git a/Inc/MockedDrivers/NVIC.hpp b/Inc/MockedDrivers/NVIC.hpp index bb85c5ee0..e48a44d3e 100644 --- a/Inc/MockedDrivers/NVIC.hpp +++ b/Inc/MockedDrivers/NVIC.hpp @@ -7,50 +7,52 @@ #include "MockedDrivers/Register.hpp" enum class NVICReg { - Reg_ISER, Reg_ICER, Reg_ISPR, Reg_ICPR, Reg_IABR, Reg_IP, + Reg_ISER, + Reg_ICER, + Reg_ISPR, + Reg_ICPR, + Reg_IABR, + Reg_IP, }; -template -class NVICRegister : public RegisterBase { +template class NVICRegister : public RegisterBase { public: using RegisterBase::RegisterBase; using RegisterBase::operator=; }; - -class NVIC_Type -{ +class NVIC_Type { struct ICER_Register : public RegisterBase { ICER_Register& operator=(uint32_t val) { - volatile uint32_t *ISER_offset = (volatile uint32_t*)((volatile uint8_t*)&this->reg - offsetof(NVIC_Type, ICER)); + volatile uint32_t* ISER_offset = + (volatile uint32_t*)((volatile uint8_t*)&this->reg - offsetof(NVIC_Type, ICER)); *ISER_offset = *ISER_offset & ~val; return *this; } }; - public: - volatile uint32_t ISER[8U]; /*!< Offset: 0x000 (R/W) Interrupt Set Enable Register */ - uint32_t RESERVED0[24U]; - ICER_Register ICER[8U]; /*!< Offset: 0x080 (R/W) Interrupt Clear Enable Register */ - uint32_t RESERVED1[24U]; - volatile uint32_t ISPR[8U]; /*!< Offset: 0x100 (R/W) Interrupt Set Pending Register */ - uint32_t RESERVED2[24U]; - volatile uint32_t ICPR[8U]; /*!< Offset: 0x180 (R/W) Interrupt Clear Pending Register */ - uint32_t RESERVED3[24U]; - volatile uint32_t IABR[8U]; /*!< Offset: 0x200 (R/W) Interrupt Active bit Register */ - uint32_t RESERVED4[56U]; - volatile uint8_t IP[240U]; /*!< Offset: 0x300 (R/W) Interrupt Priority Register (8Bit wide) */ - uint32_t RESERVED5[644U]; - volatile uint32_t STIR; /*!< Offset: 0xE00 ( /W) Software Trigger Interrupt Register */ +public: + volatile uint32_t ISER[8U]; /*!< Offset: 0x000 (R/W) Interrupt Set Enable Register */ + uint32_t RESERVED0[24U]; + ICER_Register ICER[8U]; /*!< Offset: 0x080 (R/W) Interrupt Clear Enable Register */ + uint32_t RESERVED1[24U]; + volatile uint32_t ISPR[8U]; /*!< Offset: 0x100 (R/W) Interrupt Set Pending Register */ + uint32_t RESERVED2[24U]; + volatile uint32_t ICPR[8U]; /*!< Offset: 0x180 (R/W) Interrupt Clear Pending Register */ + uint32_t RESERVED3[24U]; + volatile uint32_t IABR[8U]; /*!< Offset: 0x200 (R/W) Interrupt Active bit Register */ + uint32_t RESERVED4[56U]; + volatile uint8_t IP[240U]; /*!< Offset: 0x300 (R/W) Interrupt Priority Register (8Bit wide) */ + uint32_t RESERVED5[644U]; + volatile uint32_t STIR; /*!< Offset: 0xE00 ( /W) Software Trigger Interrupt Register */ }; extern NVIC_Type* NVIC; -extern "C"{ +extern "C" { void NVIC_EnableIRQ(IRQn_Type IRQn); uint32_t NVIC_GetEnableIRQ(IRQn_Type IRQn); void NVIC_DisableIRQ(IRQn_Type IRQn); - } \ No newline at end of file diff --git a/Inc/MockedDrivers/Register.hpp b/Inc/MockedDrivers/Register.hpp index bce0d5dba..b38d5c56d 100644 --- a/Inc/MockedDrivers/Register.hpp +++ b/Inc/MockedDrivers/Register.hpp @@ -2,17 +2,11 @@ #include #include - -template -struct RegisterTraits { - static void write(uint32_t& target, uint32_t val) { - target = val; - } +template struct RegisterTraits { + static void write(uint32_t& target, uint32_t val) { target = val; } }; - -template -class RegisterBase { +template class RegisterBase { public: uint32_t reg = 0; @@ -23,27 +17,27 @@ class RegisterBase { set(val); return *this; } - RegisterBase& operator+=(uint32_t val){ - set(reg+val); + RegisterBase& operator+=(uint32_t val) { + set(reg + val); return *this; } - RegisterBase& operator-=(uint32_t val){ - set(reg-val); + RegisterBase& operator-=(uint32_t val) { + set(reg - val); return *this; } RegisterBase& operator&=(uint32_t mask) { - set(reg & mask); + set(reg & mask); return *this; } RegisterBase& operator|=(uint32_t mask) { - set(reg | mask); + set(reg | mask); return *this; } RegisterBase& operator^=(uint32_t mask) { - set(reg ^ mask); + set(reg ^ mask); return *this; } RegisterBase& operator<<=(int shift) { @@ -58,13 +52,9 @@ class RegisterBase { // --- Shift Read (Does NOT modify Register) --- // Usage: uint32_t val = REG >> 4; - uint32_t operator<<(int shift) const { - return reg << shift; - } + uint32_t operator<<(int shift) const { return reg << shift; } - uint32_t operator>>(int shift) const { - return reg >> shift; - } + uint32_t operator>>(int shift) const { return reg >> shift; } RegisterBase& operator++() { set(reg + 1); return *this; @@ -75,7 +65,7 @@ class RegisterBase { uint32_t operator++(int) { uint32_t old_val = reg; set(reg + 1); - return old_val; + return old_val; } RegisterBase& operator--() { @@ -91,27 +81,15 @@ class RegisterBase { /** * COMPARISON */ - bool operator!=(uint32_t val) const { - return reg != val; - } - bool operator==(uint32_t val) const { - return reg == val; - } + bool operator!=(uint32_t val) const { return reg != val; } + bool operator==(uint32_t val) const { return reg == val; } /** * BITWISE COMPARISONS */ - operator uint32_t(){ - return reg; - } - operator uint32_t() const volatile { - return reg; - } - operator uint32_t() const { - return reg; - } - + operator uint32_t() { return reg; } + operator uint32_t() const volatile { return reg; } + operator uint32_t() const { return reg; } + protected: - void set(uint32_t val) { - RegisterTraits::write(this->reg, val); - } + void set(uint32_t val) { RegisterTraits::write(this->reg, val); } }; \ No newline at end of file diff --git a/Inc/MockedDrivers/common.hpp b/Inc/MockedDrivers/common.hpp index 57a62719d..388040146 100644 --- a/Inc/MockedDrivers/common.hpp +++ b/Inc/MockedDrivers/common.hpp @@ -1,8 +1,8 @@ #pragma once #ifndef glue -#define glue_(a,b) a##b -#define glue(a,b) glue_(a,b) +#define glue_(a, b) a##b +#define glue(a, b) glue_(a, b) #endif #undef __I @@ -13,33 +13,35 @@ #define IS_FUNCTIONAL_STATE(STATE) (((STATE) == DISABLE) || ((STATE) == ENABLE)) #ifdef SET_BIT -# undef SET_BIT +#undef SET_BIT #endif #ifdef CLEAR_BIT -# undef CLEAR_BIT +#undef CLEAR_BIT #endif #ifdef WRITE_REG -# undef WRITE_REG +#undef WRITE_REG #endif #ifdef MODIFY_REG -# undef MODIFY_REG +#undef MODIFY_REG #endif // this is needed because later I will do a #define uint32_t size_t typedef uint32_t u32; -#define SET_BIT(REG, BIT) ((REG) |= (BIT)) +#define SET_BIT(REG, BIT) ((REG) |= (BIT)) -#define CLEAR_BIT(REG, BIT) ((REG) = static_cast(static_cast(REG) & ~static_cast(BIT))) +#define CLEAR_BIT(REG, BIT) \ + ((REG) = static_cast(static_cast(REG) & ~static_cast(BIT))) -#define READ_BIT(REG, BIT) ((REG) & (BIT)) +#define READ_BIT(REG, BIT) ((REG) & (BIT)) -#define CLEAR_REG(REG) ((REG) = (0x0)) +#define CLEAR_REG(REG) ((REG) = (0x0)) -#define WRITE_REG(REG, VAL) ((REG) = static_cast(VAL)) +#define WRITE_REG(REG, VAL) ((REG) = static_cast(VAL)) -#define READ_REG(REG) ((REG)) +#define READ_REG(REG) ((REG)) -#define MODIFY_REG(REG, CLEARMASK, SETMASK) WRITE_REG((REG), (((READ_REG(REG)) & (~(u32)(CLEARMASK))) | (SETMASK))) +#define MODIFY_REG(REG, CLEARMASK, SETMASK) \ + WRITE_REG((REG), (((READ_REG(REG)) & (~(u32)(CLEARMASK))) | (SETMASK))) -#define POSITION_VAL(VAL) (__CLZ(__RBIT(VAL))) +#define POSITION_VAL(VAL) (__CLZ(__RBIT(VAL))) diff --git a/Inc/MockedDrivers/compiler_specific.hpp b/Inc/MockedDrivers/compiler_specific.hpp index c841e35d3..6901ec853 100644 --- a/Inc/MockedDrivers/compiler_specific.hpp +++ b/Inc/MockedDrivers/compiler_specific.hpp @@ -38,36 +38,36 @@ static inline uint32_t __RBIT(uint32_t val) { return val; } -#define __CLZ __builtin_clz +#define __CLZ __builtin_clz #if defined(_MSC_VER) && (_MSC_VER > 1200) && !defined(__clang__) void _ReadWriteBarrier(void); #pragma intrinsic(_ReadWriteBarrier) -#define __COMPILER_BARRIER() _ReadWriteBarrier() -#define __DSB() __COMPILER_BARRIER() -#define __ISB() __COMPILER_BARRIER() +#define __COMPILER_BARRIER() _ReadWriteBarrier() +#define __DSB() __COMPILER_BARRIER() +#define __ISB() __COMPILER_BARRIER() #else -#define __COMPILER_BARRIER() asm volatile("" ::: "memory") +#define __COMPILER_BARRIER() asm volatile("" ::: "memory") // Architecture-specific definitions for barrier intrinsics used in mocks #if defined(__x86_64__) || defined(_M_X64) // Host x86_64 -# define __DSB() __asm__ volatile("mfence" ::: "memory") -# define __ISB() __asm__ volatile("lfence" ::: "memory") +#define __DSB() __asm__ volatile("mfence" ::: "memory") +#define __ISB() __asm__ volatile("lfence" ::: "memory") #elif defined(__aarch64__) || defined(_M_ARM64) // Host ARM64 -# define __DSB() __asm__ volatile("dmb ish" ::: "memory") -# define __ISB() __asm__ volatile("isb" ::: "memory") +#define __DSB() __asm__ volatile("dmb ish" ::: "memory") +#define __ISB() __asm__ volatile("isb" ::: "memory") #else // Any other host architecture: compiler barrier only -# define __DSB() __COMPILER_BARRIER() -# define __ISB() __COMPILER_BARRIER() +#define __DSB() __COMPILER_BARRIER() +#define __ISB() __COMPILER_BARRIER() #endif #endif \ No newline at end of file diff --git a/Inc/MockedDrivers/mocked_hal_adc.hpp b/Inc/MockedDrivers/mocked_hal_adc.hpp index c1837ce5f..cb3dc2f3b 100644 --- a/Inc/MockedDrivers/mocked_hal_adc.hpp +++ b/Inc/MockedDrivers/mocked_hal_adc.hpp @@ -8,12 +8,12 @@ namespace ST_LIB::MockedHAL { void adc_reset(); -void adc_set_channel_raw(ADC_TypeDef *adc, uint32_t channel, uint32_t raw_value); +void adc_set_channel_raw(ADC_TypeDef* adc, uint32_t channel, uint32_t raw_value); -void adc_set_poll_timeout(ADC_TypeDef *adc, bool enabled); +void adc_set_poll_timeout(ADC_TypeDef* adc, bool enabled); -uint32_t adc_get_last_channel(ADC_TypeDef *adc); +uint32_t adc_get_last_channel(ADC_TypeDef* adc); -bool adc_is_running(ADC_TypeDef *adc); +bool adc_is_running(ADC_TypeDef* adc); } // namespace ST_LIB::MockedHAL diff --git a/Inc/MockedDrivers/mocked_hal_dma.hpp b/Inc/MockedDrivers/mocked_hal_dma.hpp index 92f478e73..4d06719af 100644 --- a/Inc/MockedDrivers/mocked_hal_dma.hpp +++ b/Inc/MockedDrivers/mocked_hal_dma.hpp @@ -7,9 +7,9 @@ namespace ST_LIB::MockedHAL { enum class DMAOperation : uint8_t { - Init = 0, - StartIT, - IRQHandler, + Init = 0, + StartIT, + IRQHandler, }; void dma_reset(); @@ -17,12 +17,11 @@ void dma_set_init_status(HAL_StatusTypeDef status); void dma_set_start_status(HAL_StatusTypeDef status); std::size_t dma_get_call_count(DMAOperation op); -DMA_HandleTypeDef *dma_get_last_init_handle(); -DMA_HandleTypeDef *dma_get_last_start_handle(); -DMA_HandleTypeDef *dma_get_last_irq_handle(); +DMA_HandleTypeDef* dma_get_last_init_handle(); +DMA_HandleTypeDef* dma_get_last_start_handle(); +DMA_HandleTypeDef* dma_get_last_irq_handle(); uint32_t dma_get_last_start_src(); uint32_t dma_get_last_start_dst(); uint32_t dma_get_last_start_length(); } // namespace ST_LIB::MockedHAL - diff --git a/Inc/MockedDrivers/mocked_hal_spi.hpp b/Inc/MockedDrivers/mocked_hal_spi.hpp index 4602c147b..cf431cf3f 100644 --- a/Inc/MockedDrivers/mocked_hal_spi.hpp +++ b/Inc/MockedDrivers/mocked_hal_spi.hpp @@ -9,15 +9,15 @@ namespace ST_LIB::MockedHAL { enum class SPIOperation : uint8_t { - Init = 0, - Abort, - Transmit, - Receive, - TransmitReceive, - TransmitDMA, - ReceiveDMA, - TransmitReceiveDMA, - IRQHandler, + Init = 0, + Abort, + Transmit, + Receive, + TransmitReceive, + TransmitDMA, + ReceiveDMA, + TransmitReceiveDMA, + IRQHandler, }; void spi_reset(); @@ -28,8 +28,8 @@ void spi_set_rx_pattern(std::span pattern); std::size_t spi_get_call_count(SPIOperation op); std::size_t spi_get_last_size_words(); -const uint8_t *spi_get_last_tx_data(); +const uint8_t* spi_get_last_tx_data(); std::size_t spi_get_last_tx_size_bytes(); -SPI_HandleTypeDef *spi_get_last_handle(); +SPI_HandleTypeDef* spi_get_last_handle(); } // namespace ST_LIB::MockedHAL diff --git a/Inc/MockedDrivers/mocked_ll_tim.hpp b/Inc/MockedDrivers/mocked_ll_tim.hpp index 8bee782e6..eb2a155a8 100644 --- a/Inc/MockedDrivers/mocked_ll_tim.hpp +++ b/Inc/MockedDrivers/mocked_ll_tim.hpp @@ -8,72 +8,115 @@ #include enum class TimReg { - Reg_CR1, Reg_CR2, Reg_SMCR, Reg_DIER, Reg_SR, Reg_EGR, - Reg_CCMR1, Reg_CCMR2, Reg_CCER, Reg_CNT, Reg_PSC, Reg_ARR, Reg_RCR, - Reg_CCR1, Reg_CCR2, Reg_CCR3, Reg_CCR4, Reg_BDTR, Reg_DCR, - Reg_DMAR, Reg_CCMR3, Reg_CCR5, Reg_CCR6, Reg_AF1, Reg_AF2, Reg_TISEL + Reg_CR1, + Reg_CR2, + Reg_SMCR, + Reg_DIER, + Reg_SR, + Reg_EGR, + Reg_CCMR1, + Reg_CCMR2, + Reg_CCER, + Reg_CNT, + Reg_PSC, + Reg_ARR, + Reg_RCR, + Reg_CCR1, + Reg_CCR2, + Reg_CCR3, + Reg_CCR4, + Reg_BDTR, + Reg_DCR, + Reg_DMAR, + Reg_CCMR3, + Reg_CCR5, + Reg_CCR6, + Reg_AF1, + Reg_AF2, + Reg_TISEL }; using enum TimReg; -template -class TimerRegister : public RegisterBase { +template class TimerRegister : public RegisterBase { public: using RegisterBase::RegisterBase; using RegisterBase::operator=; }; -static_assert(sizeof(TimerRegister) == sizeof(uint32_t) ); - +static_assert(sizeof(TimerRegister) == sizeof(uint32_t)); struct TIM_TypeDef { - TIM_TypeDef(void(* irq_handler)(void),IRQn_Type irq_n): -// PSC(*this), callback{irq_handler}, irq_n{irq_n} - callback{irq_handler}, irq_n{irq_n} - {} + TIM_TypeDef(void (*irq_handler)(void), IRQn_Type irq_n) + : // PSC(*this), callback{irq_handler}, irq_n{irq_n} + callback{irq_handler}, irq_n{irq_n} {} - template - struct PrescalerRegister : public RegisterBase { + template struct PrescalerRegister : public RegisterBase { PrescalerRegister& operator=(uint32_t val) { this->set(val); // esto es lo más feo que he hecho en mucho tiempo pero no he conseguido otra cosa - TIM_TypeDef *parent = (TIM_TypeDef*)((uint8_t*)&this->reg - offsetof(TIM_TypeDef, PSC)); + TIM_TypeDef* parent = (TIM_TypeDef*)((uint8_t*)&this->reg - offsetof(TIM_TypeDef, PSC)); parent->active_PSC = val; return *this; } }; void generate_update(); - TimerRegister CR1; /*!< TIM control register 1, Address offset: 0x00 */ - TimerRegister CR2; /*!< TIM control register 2, Address offset: 0x04 */ - TimerRegister SMCR; /*!< TIM slave mode control register, Address offset: 0x08 */ - TimerRegister DIER; /*!< TIM DMA/interrupt enable register, Address offset: 0x0C */ - TimerRegister SR; /*!< TIM status register, Address offset: 0x10 */ - TimerRegister EGR; /*!< TIM event generation register, Address offset: 0x14 */ - TimerRegister CCMR1; /*!< TIM capture/compare mode register 1, Address offset: 0x18 */ - TimerRegister CCMR2; /*!< TIM capture/compare mode register 2, Address offset: 0x1C */ - TimerRegister CCER; /*!< TIM capture/compare enable register, Address offset: 0x20 */ - TimerRegister CNT; /*!< TIM counter register, Address offset: 0x24 */ - PrescalerRegister PSC; /*!< TIM prescaler, Address offset: 0x28 */ - TimerRegister ARR; /*!< TIM auto-reload register, Address offset: 0x2C */ - TimerRegister RCR; /*!< TIM repetition counter register, Address offset: 0x30 */ - TimerRegister CCR1; /*!< TIM capture/compare register 1, Address offset: 0x34 */ - TimerRegister CCR2; /*!< TIM capture/compare register 2, Address offset: 0x38 */ - TimerRegister CCR3; /*!< TIM capture/compare register 3, Address offset: 0x3C */ - TimerRegister CCR4; /*!< TIM capture/compare register 4, Address offset: 0x40 */ - TimerRegister BDTR; /*!< TIM break and dead-time register, Address offset: 0x44 */ - TimerRegister DCR; /*!< TIM DMA control register, Address offset: 0x48 */ - TimerRegister DMAR; /*!< TIM DMA address for full transfer, Address offset: 0x4C */ - uint32_t RESERVED1; /*!< Reserved, 0x50 */ - TimerRegister CCMR3; /*!< TIM capture/compare mode register 3, Address offset: 0x54 */ - TimerRegister CCR5; /*!< TIM capture/compare register5, Address offset: 0x58 */ - TimerRegister CCR6; /*!< TIM capture/compare register6, Address offset: 0x5C */ - TimerRegister AF1; /*!< TIM alternate function option register 1, Address offset: 0x60 */ - TimerRegister AF2; /*!< TIM alternate function option register 2, Address offset: 0x64 */ - TimerRegister TISEL; /*!< TIM Input Selection register, Address offset: 0x68 */ + TimerRegister + CR1; /*!< TIM control register 1, Address offset: 0x00 */ + TimerRegister + CR2; /*!< TIM control register 2, Address offset: 0x04 */ + TimerRegister + SMCR; /*!< TIM slave mode control register, Address offset: 0x08 */ + TimerRegister + DIER; /*!< TIM DMA/interrupt enable register, Address offset: 0x0C */ + TimerRegister SR; /*!< TIM status register, Address offset: 0x10 */ + TimerRegister + EGR; /*!< TIM event generation register, Address offset: 0x14 */ + TimerRegister + CCMR1; /*!< TIM capture/compare mode register 1, Address offset: 0x18 */ + TimerRegister + CCMR2; /*!< TIM capture/compare mode register 2, Address offset: 0x1C */ + TimerRegister + CCER; /*!< TIM capture/compare enable register, Address offset: 0x20 */ + TimerRegister + CNT; /*!< TIM counter register, Address offset: 0x24 */ + PrescalerRegister + PSC; /*!< TIM prescaler, Address offset: 0x28 */ + TimerRegister + ARR; /*!< TIM auto-reload register, Address offset: 0x2C */ + TimerRegister + RCR; /*!< TIM repetition counter register, Address offset: 0x30 */ + TimerRegister + CCR1; /*!< TIM capture/compare register 1, Address offset: 0x34 */ + TimerRegister + CCR2; /*!< TIM capture/compare register 2, Address offset: 0x38 */ + TimerRegister + CCR3; /*!< TIM capture/compare register 3, Address offset: 0x3C */ + TimerRegister + CCR4; /*!< TIM capture/compare register 4, Address offset: 0x40 */ + TimerRegister + BDTR; /*!< TIM break and dead-time register, Address offset: 0x44 */ + TimerRegister + DCR; /*!< TIM DMA control register, Address offset: 0x48 */ + TimerRegister + DMAR; /*!< TIM DMA address for full transfer, Address offset: 0x4C */ + uint32_t RESERVED1; /*!< Reserved, 0x50 */ + TimerRegister + CCMR3; /*!< TIM capture/compare mode register 3, Address offset: 0x54 */ + TimerRegister + CCR5; /*!< TIM capture/compare register5, Address offset: 0x58 */ + TimerRegister + CCR6; /*!< TIM capture/compare register6, Address offset: 0x5C */ + TimerRegister + AF1; /*!< TIM alternate function option register 1, Address offset: 0x60 */ + TimerRegister + AF2; /*!< TIM alternate function option register 2, Address offset: 0x64 */ + TimerRegister + TISEL; /*!< TIM Input Selection register, Address offset: 0x68 */ // ======================================================================== // Internal Hardware State (Shadow Registers & Hidden Counters) // ======================================================================== - + // Internal counter for the prescaler (counts 0 to active_PSC) uint32_t internal_psc_cnt = 0; @@ -86,20 +129,20 @@ struct TIM_TypeDef { uint32_t active_ARR = 0; uint32_t active_RCR = 0; - void(*callback)(); + void (*callback)(); IRQn_Type irq_n; - bool check_CNT_increase_preconditions(){ - // Bit definitions for clarity - const uint32_t CR1_CEN = (1U << 0); // Counter Enable + bool check_CNT_increase_preconditions() { + // Bit definitions for clarity + const uint32_t CR1_CEN = (1U << 0); // Counter Enable // 1. Check if Counter is Enabled if (!(CR1 & CR1_CEN)) { printf("TIMER IS NOT ENABLED!!\n"); - return false; + return false; } // 2. Prescaler Logic // The internal prescaler counts from 0 up to active_PSC. - // We check if we reached the limit *before* incrementing to ensure accurate + // We check if we reached the limit *before* incrementing to ensure accurate // behavior for PSC=0 or PSC=1. bool main_counter_tick = false; @@ -107,7 +150,7 @@ struct TIM_TypeDef { internal_psc_cnt = 0; // Rollover main_counter_tick = true; } else { - internal_psc_cnt++; // Increment + internal_psc_cnt++; // Increment } // If prescaler didn't overflow, the main counter doesn't move. @@ -150,35 +193,65 @@ struct RegisterTraits { #undef TIM17_BASE extern TIM_TypeDef* TIM2_BASE; -extern "C"{ void TIM2_IRQHandler(void); } +extern "C" { +void TIM2_IRQHandler(void); +} extern TIM_TypeDef* TIM3_BASE; -extern "C"{ void TIM3_IRQHandler(void); } +extern "C" { +void TIM3_IRQHandler(void); +} extern TIM_TypeDef* TIM4_BASE; -extern "C"{ void TIM4_IRQHandler(void); } +extern "C" { +void TIM4_IRQHandler(void); +} extern TIM_TypeDef* TIM5_BASE; -extern "C"{ void TIM5_IRQHandler(void); } +extern "C" { +void TIM5_IRQHandler(void); +} extern TIM_TypeDef* TIM6_BASE; -extern "C"{ void TIM6_DAC_IRQHandler(void); } +extern "C" { +void TIM6_DAC_IRQHandler(void); +} extern TIM_TypeDef* TIM7_BASE; -extern "C"{ void TIM7_IRQHandler(void); } +extern "C" { +void TIM7_IRQHandler(void); +} extern TIM_TypeDef* TIM12_BASE; -extern "C"{ void TIM8_BRK_TIM12_IRQHandler(void); } +extern "C" { +void TIM8_BRK_TIM12_IRQHandler(void); +} extern TIM_TypeDef* TIM13_BASE; extern TIM_TypeDef* TIM14_BASE; -extern "C"{ void TIM8_TRG_COM_TIM14_IRQHandler(void); } +extern "C" { +void TIM8_TRG_COM_TIM14_IRQHandler(void); +} extern TIM_TypeDef* TIM23_BASE; -extern "C"{ void TIM23_IRQHandler(void); } +extern "C" { +void TIM23_IRQHandler(void); +} extern TIM_TypeDef* TIM24_BASE; -extern "C"{ void TIM24_IRQHandler(void); } +extern "C" { +void TIM24_IRQHandler(void); +} extern TIM_TypeDef* TIM1_BASE; -extern "C"{ void TIM1_UP_IRQHandler(void); } +extern "C" { +void TIM1_UP_IRQHandler(void); +} extern TIM_TypeDef* TIM8_BASE; -extern "C"{ void TIM8_UP_TIM13_IRQHandler(void); } +extern "C" { +void TIM8_UP_TIM13_IRQHandler(void); +} extern TIM_TypeDef* TIM15_BASE; -extern "C"{ void TIM15_IRQHandler(void); } +extern "C" { +void TIM15_IRQHandler(void); +} extern TIM_TypeDef* TIM16_BASE; -extern "C"{ void TIM16_IRQHandler(void); } +extern "C" { +void TIM16_IRQHandler(void); +} extern TIM_TypeDef* TIM17_BASE; -extern "C"{ void TIM17_IRQHandler(void); } +extern "C" { +void TIM17_IRQHandler(void); +} diff --git a/Inc/MockedDrivers/stm32h7xx_hal_mock.h b/Inc/MockedDrivers/stm32h7xx_hal_mock.h index 704e581d2..f556b313c 100644 --- a/Inc/MockedDrivers/stm32h7xx_hal_mock.h +++ b/Inc/MockedDrivers/stm32h7xx_hal_mock.h @@ -12,55 +12,55 @@ typedef enum { DISABLE = 0U, ENABLE = !DISABLE } FunctionalState; typedef int32_t IRQn_Type; enum { - TIM2_IRQn = 28, - TIM3_IRQn = 29, - TIM4_IRQn = 30, - TIM5_IRQn = 50, - SPI1_IRQn = 35, - SPI2_IRQn = 36, - SPI3_IRQn = 51, - SPI4_IRQn = 84, - SPI5_IRQn = 85, - SPI6_IRQn = 86, - DMA1_Stream0_IRQn = 11, - DMA1_Stream1_IRQn = 12, - DMA1_Stream2_IRQn = 13, - DMA1_Stream3_IRQn = 14, - DMA1_Stream4_IRQn = 15, - DMA1_Stream5_IRQn = 16, - DMA1_Stream6_IRQn = 17, - DMA1_Stream7_IRQn = 47, - DMA2_Stream0_IRQn = 56, - DMA2_Stream1_IRQn = 57, - DMA2_Stream2_IRQn = 58, - DMA2_Stream3_IRQn = 59, - DMA2_Stream4_IRQn = 60, - DMA2_Stream5_IRQn = 61, - DMA2_Stream6_IRQn = 62, - DMA2_Stream7_IRQn = 63, - TIM6_DAC_IRQn = 54, - TIM7_IRQn = 55, - TIM8_BRK_TIM12_IRQn = 43, - TIM8_UP_TIM13_IRQn = 44, - TIM8_TRG_COM_TIM14_IRQn = 45, - TIM1_UP_IRQn = 25, - TIM15_IRQn = 68, - TIM16_IRQn = 69, - TIM17_IRQn = 70, - TIM23_IRQn = 71, - TIM24_IRQn = 72, + TIM2_IRQn = 28, + TIM3_IRQn = 29, + TIM4_IRQn = 30, + TIM5_IRQn = 50, + SPI1_IRQn = 35, + SPI2_IRQn = 36, + SPI3_IRQn = 51, + SPI4_IRQn = 84, + SPI5_IRQn = 85, + SPI6_IRQn = 86, + DMA1_Stream0_IRQn = 11, + DMA1_Stream1_IRQn = 12, + DMA1_Stream2_IRQn = 13, + DMA1_Stream3_IRQn = 14, + DMA1_Stream4_IRQn = 15, + DMA1_Stream5_IRQn = 16, + DMA1_Stream6_IRQn = 17, + DMA1_Stream7_IRQn = 47, + DMA2_Stream0_IRQn = 56, + DMA2_Stream1_IRQn = 57, + DMA2_Stream2_IRQn = 58, + DMA2_Stream3_IRQn = 59, + DMA2_Stream4_IRQn = 60, + DMA2_Stream5_IRQn = 61, + DMA2_Stream6_IRQn = 62, + DMA2_Stream7_IRQn = 63, + TIM6_DAC_IRQn = 54, + TIM7_IRQn = 55, + TIM8_BRK_TIM12_IRQn = 43, + TIM8_UP_TIM13_IRQn = 44, + TIM8_TRG_COM_TIM14_IRQn = 45, + TIM1_UP_IRQn = 25, + TIM15_IRQn = 68, + TIM16_IRQn = 69, + TIM17_IRQn = 70, + TIM23_IRQn = 71, + TIM24_IRQn = 72, }; typedef struct { - volatile uint32_t D1CFGR; - volatile uint32_t D2CFGR; - volatile uint32_t APB1LENR; - volatile uint32_t APB1HENR; - volatile uint32_t APB2ENR; - volatile uint32_t APB4ENR; - volatile uint32_t AHB4ENR; + volatile uint32_t D1CFGR; + volatile uint32_t D2CFGR; + volatile uint32_t APB1LENR; + volatile uint32_t APB1HENR; + volatile uint32_t APB2ENR; + volatile uint32_t APB4ENR; + volatile uint32_t AHB4ENR; } RCC_TypeDef; -extern RCC_TypeDef *RCC; +extern RCC_TypeDef* RCC; extern uint32_t SystemCoreClock; #define RCC_D1CFGR_HPRE_Msk (0xFU << 0U) @@ -127,62 +127,59 @@ extern uint32_t SystemCoreClock; #define __HAL_RCC_DMA2_CLK_ENABLE() ((void)0U) typedef struct __DMA_Stream_TypeDef { - volatile uint32_t CR; - volatile uint32_t NDTR; - volatile uint32_t PAR; - volatile uint32_t M0AR; - volatile uint32_t M1AR; - volatile uint32_t FCR; + volatile uint32_t CR; + volatile uint32_t NDTR; + volatile uint32_t PAR; + volatile uint32_t M0AR; + volatile uint32_t M1AR; + volatile uint32_t FCR; } DMA_Stream_TypeDef; -extern DMA_Stream_TypeDef *DMA1_Stream0; -extern DMA_Stream_TypeDef *DMA1_Stream1; -extern DMA_Stream_TypeDef *DMA1_Stream2; -extern DMA_Stream_TypeDef *DMA1_Stream3; -extern DMA_Stream_TypeDef *DMA1_Stream4; -extern DMA_Stream_TypeDef *DMA1_Stream5; -extern DMA_Stream_TypeDef *DMA1_Stream6; -extern DMA_Stream_TypeDef *DMA1_Stream7; -extern DMA_Stream_TypeDef *DMA2_Stream0; -extern DMA_Stream_TypeDef *DMA2_Stream1; -extern DMA_Stream_TypeDef *DMA2_Stream2; -extern DMA_Stream_TypeDef *DMA2_Stream3; -extern DMA_Stream_TypeDef *DMA2_Stream4; -extern DMA_Stream_TypeDef *DMA2_Stream5; -extern DMA_Stream_TypeDef *DMA2_Stream6; -extern DMA_Stream_TypeDef *DMA2_Stream7; +extern DMA_Stream_TypeDef* DMA1_Stream0; +extern DMA_Stream_TypeDef* DMA1_Stream1; +extern DMA_Stream_TypeDef* DMA1_Stream2; +extern DMA_Stream_TypeDef* DMA1_Stream3; +extern DMA_Stream_TypeDef* DMA1_Stream4; +extern DMA_Stream_TypeDef* DMA1_Stream5; +extern DMA_Stream_TypeDef* DMA1_Stream6; +extern DMA_Stream_TypeDef* DMA1_Stream7; +extern DMA_Stream_TypeDef* DMA2_Stream0; +extern DMA_Stream_TypeDef* DMA2_Stream1; +extern DMA_Stream_TypeDef* DMA2_Stream2; +extern DMA_Stream_TypeDef* DMA2_Stream3; +extern DMA_Stream_TypeDef* DMA2_Stream4; +extern DMA_Stream_TypeDef* DMA2_Stream5; +extern DMA_Stream_TypeDef* DMA2_Stream6; +extern DMA_Stream_TypeDef* DMA2_Stream7; typedef struct { - volatile uint32_t MODER; - volatile uint32_t OTYPER; - volatile uint32_t OSPEEDR; - volatile uint32_t PUPDR; - volatile uint32_t IDR; - volatile uint32_t ODR; - volatile uint32_t BSRR; - volatile uint32_t LCKR; - volatile uint32_t AFR[2]; + volatile uint32_t MODER; + volatile uint32_t OTYPER; + volatile uint32_t OSPEEDR; + volatile uint32_t PUPDR; + volatile uint32_t IDR; + volatile uint32_t ODR; + volatile uint32_t BSRR; + volatile uint32_t LCKR; + volatile uint32_t AFR[2]; } GPIO_TypeDef; -extern GPIO_TypeDef *GPIOA; -extern GPIO_TypeDef *GPIOB; -extern GPIO_TypeDef *GPIOC; -extern GPIO_TypeDef *GPIOD; -extern GPIO_TypeDef *GPIOE; -extern GPIO_TypeDef *GPIOF; -extern GPIO_TypeDef *GPIOG; -extern GPIO_TypeDef *GPIOH; +extern GPIO_TypeDef* GPIOA; +extern GPIO_TypeDef* GPIOB; +extern GPIO_TypeDef* GPIOC; +extern GPIO_TypeDef* GPIOD; +extern GPIO_TypeDef* GPIOE; +extern GPIO_TypeDef* GPIOF; +extern GPIO_TypeDef* GPIOG; +extern GPIO_TypeDef* GPIOH; -typedef enum { - GPIO_PIN_RESET = 0U, - GPIO_PIN_SET -} GPIO_PinState; +typedef enum { GPIO_PIN_RESET = 0U, GPIO_PIN_SET } GPIO_PinState; typedef struct { - uint32_t Pin; - uint32_t Mode; - uint32_t Pull; - uint32_t Speed; - uint32_t Alternate; + uint32_t Pin; + uint32_t Mode; + uint32_t Pull; + uint32_t Speed; + uint32_t Alternate; } GPIO_InitTypeDef; #define GPIO_PIN_0 (1U << 0) @@ -223,69 +220,66 @@ typedef struct { #define GPIO_SPEED_FREQ_VERY_HIGH 0x00000003U typedef struct { - volatile uint32_t CR; - volatile uint32_t CFGR; - volatile uint32_t SR; - volatile uint32_t DR; - volatile uint32_t SMPR1; - volatile uint32_t SMPR2; - volatile uint32_t SQR1; - volatile uint32_t SQR2; - volatile uint32_t SQR3; - volatile uint32_t SQR4; + volatile uint32_t CR; + volatile uint32_t CFGR; + volatile uint32_t SR; + volatile uint32_t DR; + volatile uint32_t SMPR1; + volatile uint32_t SMPR2; + volatile uint32_t SQR1; + volatile uint32_t SQR2; + volatile uint32_t SQR3; + volatile uint32_t SQR4; } ADC_TypeDef; -extern ADC_TypeDef *ADC1; -extern ADC_TypeDef *ADC2; -extern ADC_TypeDef *ADC3; +extern ADC_TypeDef* ADC1; +extern ADC_TypeDef* ADC2; +extern ADC_TypeDef* ADC3; typedef enum { - HAL_OK = 0x00U, - HAL_ERROR = 0x01U, - HAL_BUSY = 0x02U, - HAL_TIMEOUT = 0x03U + HAL_OK = 0x00U, + HAL_ERROR = 0x01U, + HAL_BUSY = 0x02U, + HAL_TIMEOUT = 0x03U } HAL_StatusTypeDef; -typedef enum { - HAL_UNLOCKED = 0x00U, - HAL_LOCKED = 0x01U -} HAL_LockTypeDef; +typedef enum { HAL_UNLOCKED = 0x00U, HAL_LOCKED = 0x01U } HAL_LockTypeDef; typedef struct { - uint32_t ClockPrescaler; - uint32_t Resolution; - uint32_t ScanConvMode; - uint32_t EOCSelection; - uint32_t LowPowerAutoWait; - uint32_t ContinuousConvMode; - uint32_t NbrOfConversion; - uint32_t DiscontinuousConvMode; - uint32_t NbrOfDiscConversion; - uint32_t ExternalTrigConv; - uint32_t ExternalTrigConvEdge; - uint32_t ConversionDataManagement; - uint32_t Overrun; - uint32_t LeftBitShift; - uint32_t OversamplingMode; - uint32_t DMAContinuousRequests; - uint32_t SamplingMode; + uint32_t ClockPrescaler; + uint32_t Resolution; + uint32_t ScanConvMode; + uint32_t EOCSelection; + uint32_t LowPowerAutoWait; + uint32_t ContinuousConvMode; + uint32_t NbrOfConversion; + uint32_t DiscontinuousConvMode; + uint32_t NbrOfDiscConversion; + uint32_t ExternalTrigConv; + uint32_t ExternalTrigConvEdge; + uint32_t ConversionDataManagement; + uint32_t Overrun; + uint32_t LeftBitShift; + uint32_t OversamplingMode; + uint32_t DMAContinuousRequests; + uint32_t SamplingMode; } ADC_InitTypeDef; typedef struct __ADC_HandleTypeDef { - ADC_TypeDef *Instance; - ADC_InitTypeDef Init; - HAL_LockTypeDef Lock; - volatile uint32_t State; - volatile uint32_t ErrorCode; + ADC_TypeDef* Instance; + ADC_InitTypeDef Init; + HAL_LockTypeDef Lock; + volatile uint32_t State; + volatile uint32_t ErrorCode; } ADC_HandleTypeDef; typedef struct { - uint32_t Channel; - uint32_t Rank; - uint32_t SamplingTime; - uint32_t SingleDiff; - uint32_t OffsetNumber; - uint32_t Offset; - uint32_t OffsetSignedSaturation; + uint32_t Channel; + uint32_t Rank; + uint32_t SamplingTime; + uint32_t SingleDiff; + uint32_t OffsetNumber; + uint32_t Offset; + uint32_t OffsetSignedSaturation; } ADC_ChannelConfTypeDef; #define ADC_VER_V5_V90 @@ -361,24 +355,24 @@ typedef struct { #define HAL_ADC_STATE_TIMEOUT 0x00000400U typedef struct { - uint32_t Request; - uint32_t Direction; - uint32_t PeriphInc; - uint32_t MemInc; - uint32_t PeriphDataAlignment; - uint32_t MemDataAlignment; - uint32_t Mode; - uint32_t Priority; - uint32_t FIFOMode; - uint32_t FIFOThreshold; - uint32_t MemBurst; - uint32_t PeriphBurst; + uint32_t Request; + uint32_t Direction; + uint32_t PeriphInc; + uint32_t MemInc; + uint32_t PeriphDataAlignment; + uint32_t MemDataAlignment; + uint32_t Mode; + uint32_t Priority; + uint32_t FIFOMode; + uint32_t FIFOThreshold; + uint32_t MemBurst; + uint32_t PeriphBurst; } DMA_InitTypeDef; typedef struct __DMA_HandleTypeDef { - DMA_Stream_TypeDef *Instance; - DMA_InitTypeDef Init; - void *Parent; + DMA_Stream_TypeDef* Instance; + DMA_InitTypeDef Init; + void* Parent; } DMA_HandleTypeDef; #define DMA_REQUEST_MEM2MEM 0x000U @@ -436,27 +430,27 @@ typedef struct __DMA_HandleTypeDef { #define DMA_PBURST_SINGLE 0x000U typedef struct { - volatile uint32_t CR1; - volatile uint32_t CR2; - volatile uint32_t CFG1; - volatile uint32_t CFG2; - volatile uint32_t IER; - volatile uint32_t SR; - volatile uint32_t IFCR; - volatile uint32_t TXDR; - volatile uint32_t RXDR; - volatile uint32_t CRCPOLY; - volatile uint32_t TXCRC; - volatile uint32_t RXCRC; - volatile uint32_t UDRDR; + volatile uint32_t CR1; + volatile uint32_t CR2; + volatile uint32_t CFG1; + volatile uint32_t CFG2; + volatile uint32_t IER; + volatile uint32_t SR; + volatile uint32_t IFCR; + volatile uint32_t TXDR; + volatile uint32_t RXDR; + volatile uint32_t CRCPOLY; + volatile uint32_t TXCRC; + volatile uint32_t RXCRC; + volatile uint32_t UDRDR; } SPI_TypeDef; -extern SPI_TypeDef *SPI1; -extern SPI_TypeDef *SPI2; -extern SPI_TypeDef *SPI3; -extern SPI_TypeDef *SPI4; -extern SPI_TypeDef *SPI5; -extern SPI_TypeDef *SPI6; +extern SPI_TypeDef* SPI1; +extern SPI_TypeDef* SPI2; +extern SPI_TypeDef* SPI3; +extern SPI_TypeDef* SPI4; +extern SPI_TypeDef* SPI5; +extern SPI_TypeDef* SPI6; #define SPI1_BASE (0x40013000UL) #define SPI2_BASE (0x40003800UL) @@ -466,38 +460,38 @@ extern SPI_TypeDef *SPI6; #define SPI6_BASE (0x58001400UL) typedef struct { - uint32_t Mode; - uint32_t Direction; - uint32_t DataSize; - uint32_t CLKPolarity; - uint32_t CLKPhase; - uint32_t NSS; - uint32_t BaudRatePrescaler; - uint32_t FirstBit; - uint32_t TIMode; - uint32_t CRCCalculation; - uint32_t CRCPolynomial; - uint32_t CRCLength; - uint32_t NSSPMode; - uint32_t NSSPolarity; - uint32_t FifoThreshold; - uint32_t TxCRCInitializationPattern; - uint32_t RxCRCInitializationPattern; - uint32_t MasterSSIdleness; - uint32_t MasterInterDataIdleness; - uint32_t MasterReceiverAutoSusp; - uint32_t MasterKeepIOState; - uint32_t IOSwap; + uint32_t Mode; + uint32_t Direction; + uint32_t DataSize; + uint32_t CLKPolarity; + uint32_t CLKPhase; + uint32_t NSS; + uint32_t BaudRatePrescaler; + uint32_t FirstBit; + uint32_t TIMode; + uint32_t CRCCalculation; + uint32_t CRCPolynomial; + uint32_t CRCLength; + uint32_t NSSPMode; + uint32_t NSSPolarity; + uint32_t FifoThreshold; + uint32_t TxCRCInitializationPattern; + uint32_t RxCRCInitializationPattern; + uint32_t MasterSSIdleness; + uint32_t MasterInterDataIdleness; + uint32_t MasterReceiverAutoSusp; + uint32_t MasterKeepIOState; + uint32_t IOSwap; } SPI_InitTypeDef; typedef struct __SPI_HandleTypeDef { - SPI_TypeDef *Instance; - SPI_InitTypeDef Init; - DMA_HandleTypeDef *hdmatx; - DMA_HandleTypeDef *hdmarx; - HAL_LockTypeDef Lock; - volatile uint32_t State; - volatile uint32_t ErrorCode; + SPI_TypeDef* Instance; + SPI_InitTypeDef Init; + DMA_HandleTypeDef* hdmatx; + DMA_HandleTypeDef* hdmarx; + HAL_LockTypeDef Lock; + volatile uint32_t State; + volatile uint32_t ErrorCode; } SPI_HandleTypeDef; #define SPI_MODE_SLAVE 0x00000000U @@ -558,10 +552,10 @@ typedef struct __SPI_HandleTypeDef { #define HAL_SPI_ERROR_NONE 0x00000000U typedef struct { - uint32_t PeriphClockSelection; - uint32_t Spi123ClockSelection; - uint32_t Spi45ClockSelection; - uint32_t Spi6ClockSelection; + uint32_t PeriphClockSelection; + uint32_t Spi123ClockSelection; + uint32_t Spi45ClockSelection; + uint32_t Spi6ClockSelection; } RCC_PeriphCLKInitTypeDef; #define RCC_PERIPHCLK_SPI1 0x00000001U @@ -578,22 +572,22 @@ typedef struct { #define RCC_SPI6CLKSOURCE_PLL2 0x00000002U typedef struct TIM_TypeDef TIM_TypeDef; -extern TIM_TypeDef *TIM1_BASE; -extern TIM_TypeDef *TIM2_BASE; -extern TIM_TypeDef *TIM3_BASE; -extern TIM_TypeDef *TIM4_BASE; -extern TIM_TypeDef *TIM5_BASE; -extern TIM_TypeDef *TIM6_BASE; -extern TIM_TypeDef *TIM7_BASE; -extern TIM_TypeDef *TIM8_BASE; -extern TIM_TypeDef *TIM12_BASE; -extern TIM_TypeDef *TIM13_BASE; -extern TIM_TypeDef *TIM14_BASE; -extern TIM_TypeDef *TIM15_BASE; -extern TIM_TypeDef *TIM16_BASE; -extern TIM_TypeDef *TIM17_BASE; -extern TIM_TypeDef *TIM23_BASE; -extern TIM_TypeDef *TIM24_BASE; +extern TIM_TypeDef* TIM1_BASE; +extern TIM_TypeDef* TIM2_BASE; +extern TIM_TypeDef* TIM3_BASE; +extern TIM_TypeDef* TIM4_BASE; +extern TIM_TypeDef* TIM5_BASE; +extern TIM_TypeDef* TIM6_BASE; +extern TIM_TypeDef* TIM7_BASE; +extern TIM_TypeDef* TIM8_BASE; +extern TIM_TypeDef* TIM12_BASE; +extern TIM_TypeDef* TIM13_BASE; +extern TIM_TypeDef* TIM14_BASE; +extern TIM_TypeDef* TIM15_BASE; +extern TIM_TypeDef* TIM16_BASE; +extern TIM_TypeDef* TIM17_BASE; +extern TIM_TypeDef* TIM23_BASE; +extern TIM_TypeDef* TIM24_BASE; #define TIM1 TIM1_BASE #define TIM2 TIM2_BASE @@ -613,78 +607,78 @@ extern TIM_TypeDef *TIM24_BASE; #define TIM24 TIM24_BASE typedef struct { - uint32_t Prescaler; - uint32_t CounterMode; - uint32_t Period; - uint32_t ClockDivision; - uint32_t RepetitionCounter; - uint32_t AutoReloadPreload; + uint32_t Prescaler; + uint32_t CounterMode; + uint32_t Period; + uint32_t ClockDivision; + uint32_t RepetitionCounter; + uint32_t AutoReloadPreload; } TIM_Base_InitTypeDef; typedef enum { - HAL_TIM_STATE_RESET = 0x00U, - HAL_TIM_STATE_READY = 0x01U, - HAL_TIM_STATE_BUSY = 0x02U + HAL_TIM_STATE_RESET = 0x00U, + HAL_TIM_STATE_READY = 0x01U, + HAL_TIM_STATE_BUSY = 0x02U } HAL_TIM_StateTypeDef; typedef enum { - HAL_TIM_CHANNEL_STATE_RESET = 0x00U, - HAL_TIM_CHANNEL_STATE_READY = 0x01U, - HAL_TIM_CHANNEL_STATE_BUSY = 0x02U + HAL_TIM_CHANNEL_STATE_RESET = 0x00U, + HAL_TIM_CHANNEL_STATE_READY = 0x01U, + HAL_TIM_CHANNEL_STATE_BUSY = 0x02U } HAL_TIM_ChannelStateTypeDef; typedef struct __TIM_HandleTypeDef { - TIM_TypeDef *Instance; - TIM_Base_InitTypeDef Init; - HAL_TIM_ChannelStateTypeDef ChannelState[6]; - HAL_TIM_ChannelStateTypeDef ChannelNState[6]; - uint32_t DMABurstState; - HAL_LockTypeDef Lock; - HAL_TIM_StateTypeDef State; + TIM_TypeDef* Instance; + TIM_Base_InitTypeDef Init; + HAL_TIM_ChannelStateTypeDef ChannelState[6]; + HAL_TIM_ChannelStateTypeDef ChannelNState[6]; + uint32_t DMABurstState; + HAL_LockTypeDef Lock; + HAL_TIM_StateTypeDef State; } TIM_HandleTypeDef; typedef struct { - uint32_t OCMode; - uint32_t Pulse; - uint32_t OCPolarity; - uint32_t OCNPolarity; - uint32_t OCFastMode; - uint32_t OCIdleState; - uint32_t OCNIdleState; + uint32_t OCMode; + uint32_t Pulse; + uint32_t OCPolarity; + uint32_t OCNPolarity; + uint32_t OCFastMode; + uint32_t OCIdleState; + uint32_t OCNIdleState; } TIM_OC_InitTypeDef; typedef struct { - uint32_t EncoderMode; - uint32_t IC1Polarity; - uint32_t IC1Selection; - uint32_t IC1Prescaler; - uint32_t IC1Filter; - uint32_t IC2Polarity; - uint32_t IC2Selection; - uint32_t IC2Prescaler; - uint32_t IC2Filter; + uint32_t EncoderMode; + uint32_t IC1Polarity; + uint32_t IC1Selection; + uint32_t IC1Prescaler; + uint32_t IC1Filter; + uint32_t IC2Polarity; + uint32_t IC2Selection; + uint32_t IC2Prescaler; + uint32_t IC2Filter; } TIM_Encoder_InitTypeDef; typedef struct { - uint32_t MasterOutputTrigger; - uint32_t MasterOutputTrigger2; - uint32_t MasterSlaveMode; + uint32_t MasterOutputTrigger; + uint32_t MasterOutputTrigger2; + uint32_t MasterSlaveMode; } TIM_MasterConfigTypeDef; typedef struct { - uint32_t OffStateRunMode; - uint32_t OffStateIDLEMode; - uint32_t LockLevel; - uint32_t DeadTime; - uint32_t BreakState; - uint32_t BreakPolarity; - uint32_t BreakFilter; - uint32_t BreakAFMode; - uint32_t Break2State; - uint32_t Break2Polarity; - uint32_t Break2Filter; - uint32_t Break2AFMode; - uint32_t AutomaticOutput; + uint32_t OffStateRunMode; + uint32_t OffStateIDLEMode; + uint32_t LockLevel; + uint32_t DeadTime; + uint32_t BreakState; + uint32_t BreakPolarity; + uint32_t BreakFilter; + uint32_t BreakAFMode; + uint32_t Break2State; + uint32_t Break2Polarity; + uint32_t Break2Filter; + uint32_t Break2AFMode; + uint32_t AutomaticOutput; } TIM_BreakDeadTimeConfigTypeDef; #define HAL_DMA_BURST_STATE_READY 0U @@ -785,8 +779,8 @@ typedef struct { #define TIM_CCER_CC5P (1U << 17) #define TIM_CCER_CC6E (1U << 20) #define TIM_CCER_CC6P (1U << 21) -#define TIM_CCER_CCxE_MASK (TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC3E | \ - TIM_CCER_CC4E | TIM_CCER_CC5E | TIM_CCER_CC6E) +#define TIM_CCER_CCxE_MASK \ + (TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC3E | TIM_CCER_CC4E | TIM_CCER_CC5E | TIM_CCER_CC6E) #define TIM_CCER_CCxNE_MASK (TIM_CCER_CC1NE | TIM_CCER_CC2NE | TIM_CCER_CC3NE) #define TIM_BDTR_MOE (1U << 15) @@ -797,162 +791,165 @@ typedef struct { #define LL_TIM_EnableCounter(__TIMx) ((__TIMx)->CR1 |= TIM_CR1_CEN) #define LL_TIM_DisableCounter(__TIMx) ((__TIMx)->CR1 &= ~TIM_CR1_CEN) -#define IS_TIM_SLAVEMODE_TRIGGER_ENABLED(__SMCR__) \ - ((((__SMCR__) & TIM_SMCR_SMS) != 0U) ? 1U : 0U) +#define IS_TIM_SLAVEMODE_TRIGGER_ENABLED(__SMCR__) ((((__SMCR__) & TIM_SMCR_SMS) != 0U) ? 1U : 0U) #define SYSCFG_PMCR_PC2SO (1U << 2) #define SYSCFG_PMCR_PC3SO (1U << 3) -HAL_StatusTypeDef HAL_ADC_Init(ADC_HandleTypeDef *hadc); -HAL_StatusTypeDef HAL_ADC_DeInit(ADC_HandleTypeDef *hadc); -HAL_StatusTypeDef HAL_ADC_ConfigChannel(ADC_HandleTypeDef *hadc, - ADC_ChannelConfTypeDef *sConfig); -HAL_StatusTypeDef HAL_ADC_Start(ADC_HandleTypeDef *hadc); -HAL_StatusTypeDef HAL_ADC_PollForConversion(ADC_HandleTypeDef *hadc, - uint32_t Timeout); -uint32_t HAL_ADC_GetValue(const ADC_HandleTypeDef *hadc); -HAL_StatusTypeDef HAL_ADC_Stop(ADC_HandleTypeDef *hadc); -uint32_t HAL_ADC_GetState(const ADC_HandleTypeDef *hadc); -uint32_t HAL_ADC_GetError(const ADC_HandleTypeDef *hadc); - -HAL_StatusTypeDef HAL_SPI_Init(SPI_HandleTypeDef *hspi); -HAL_StatusTypeDef HAL_SPI_Abort(SPI_HandleTypeDef *hspi); -HAL_StatusTypeDef HAL_SPI_Transmit(SPI_HandleTypeDef *hspi, uint8_t *pData, - uint16_t Size, uint32_t Timeout); -HAL_StatusTypeDef HAL_SPI_Receive(SPI_HandleTypeDef *hspi, uint8_t *pData, - uint16_t Size, uint32_t Timeout); -HAL_StatusTypeDef HAL_SPI_TransmitReceive(SPI_HandleTypeDef *hspi, - uint8_t *pTxData, uint8_t *pRxData, - uint16_t Size, uint32_t Timeout); -HAL_StatusTypeDef HAL_SPI_Transmit_DMA(SPI_HandleTypeDef *hspi, uint8_t *pData, - uint16_t Size); -HAL_StatusTypeDef HAL_SPI_Receive_DMA(SPI_HandleTypeDef *hspi, uint8_t *pData, - uint16_t Size); -HAL_StatusTypeDef HAL_SPI_TransmitReceive_DMA(SPI_HandleTypeDef *hspi, - uint8_t *pTxData, - uint8_t *pRxData, uint16_t Size); -void HAL_SPI_IRQHandler(SPI_HandleTypeDef *hspi); - -void HAL_SYSCFG_AnalogSwitchConfig(uint32_t SYSCFG_AnalogSwitch, - uint32_t SYSCFG_SwitchState); +HAL_StatusTypeDef HAL_ADC_Init(ADC_HandleTypeDef* hadc); +HAL_StatusTypeDef HAL_ADC_DeInit(ADC_HandleTypeDef* hadc); +HAL_StatusTypeDef HAL_ADC_ConfigChannel(ADC_HandleTypeDef* hadc, ADC_ChannelConfTypeDef* sConfig); +HAL_StatusTypeDef HAL_ADC_Start(ADC_HandleTypeDef* hadc); +HAL_StatusTypeDef HAL_ADC_PollForConversion(ADC_HandleTypeDef* hadc, uint32_t Timeout); +uint32_t HAL_ADC_GetValue(const ADC_HandleTypeDef* hadc); +HAL_StatusTypeDef HAL_ADC_Stop(ADC_HandleTypeDef* hadc); +uint32_t HAL_ADC_GetState(const ADC_HandleTypeDef* hadc); +uint32_t HAL_ADC_GetError(const ADC_HandleTypeDef* hadc); + +HAL_StatusTypeDef HAL_SPI_Init(SPI_HandleTypeDef* hspi); +HAL_StatusTypeDef HAL_SPI_Abort(SPI_HandleTypeDef* hspi); +HAL_StatusTypeDef +HAL_SPI_Transmit(SPI_HandleTypeDef* hspi, uint8_t* pData, uint16_t Size, uint32_t Timeout); +HAL_StatusTypeDef +HAL_SPI_Receive(SPI_HandleTypeDef* hspi, uint8_t* pData, uint16_t Size, uint32_t Timeout); +HAL_StatusTypeDef HAL_SPI_TransmitReceive( + SPI_HandleTypeDef* hspi, + uint8_t* pTxData, + uint8_t* pRxData, + uint16_t Size, + uint32_t Timeout +); +HAL_StatusTypeDef HAL_SPI_Transmit_DMA(SPI_HandleTypeDef* hspi, uint8_t* pData, uint16_t Size); +HAL_StatusTypeDef HAL_SPI_Receive_DMA(SPI_HandleTypeDef* hspi, uint8_t* pData, uint16_t Size); +HAL_StatusTypeDef HAL_SPI_TransmitReceive_DMA( + SPI_HandleTypeDef* hspi, + uint8_t* pTxData, + uint8_t* pRxData, + uint16_t Size +); +void HAL_SPI_IRQHandler(SPI_HandleTypeDef* hspi); + +void HAL_SYSCFG_AnalogSwitchConfig(uint32_t SYSCFG_AnalogSwitch, uint32_t SYSCFG_SwitchState); void NVIC_EnableIRQ(IRQn_Type IRQn); void NVIC_DisableIRQ(IRQn_Type IRQn); -HAL_StatusTypeDef MockedHAL_DMA_Init_Impl(DMA_HandleTypeDef *hdma); -HAL_StatusTypeDef MockedHAL_DMA_Start_IT_Impl(DMA_HandleTypeDef *hdma, - uint32_t SrcAddress, - uint32_t DstAddress, - uint32_t DataLength); -void MockedHAL_DMA_IRQHandler_Impl(DMA_HandleTypeDef *hdma); +HAL_StatusTypeDef MockedHAL_DMA_Init_Impl(DMA_HandleTypeDef* hdma); +HAL_StatusTypeDef MockedHAL_DMA_Start_IT_Impl( + DMA_HandleTypeDef* hdma, + uint32_t SrcAddress, + uint32_t DstAddress, + uint32_t DataLength +); +void MockedHAL_DMA_IRQHandler_Impl(DMA_HandleTypeDef* hdma); static inline uint32_t HAL_RCC_GetPCLK1Freq(void) { return SystemCoreClock; } static inline uint32_t HAL_RCC_GetPCLK2Freq(void) { return SystemCoreClock; } -static inline void HAL_NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority, - uint32_t subpriority) { - (void)IRQn; - (void)priority; - (void)subpriority; +static inline void HAL_NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority, uint32_t subpriority) { + (void)IRQn; + (void)priority; + (void)subpriority; } static inline void HAL_NVIC_EnableIRQ(IRQn_Type IRQn) { NVIC_EnableIRQ(IRQn); } static inline void HAL_NVIC_DisableIRQ(IRQn_Type IRQn) { NVIC_DisableIRQ(IRQn); } -static inline HAL_StatusTypeDef -HAL_RCCEx_PeriphCLKConfig(RCC_PeriphCLKInitTypeDef *PeriphClkInit) { - (void)PeriphClkInit; - return HAL_OK; +static inline HAL_StatusTypeDef HAL_RCCEx_PeriphCLKConfig(RCC_PeriphCLKInitTypeDef* PeriphClkInit) { + (void)PeriphClkInit; + return HAL_OK; } static inline uint32_t HAL_RCCEx_GetPeriphCLKFreq(uint32_t PeriphClk) { - (void)PeriphClk; - return SystemCoreClock; + (void)PeriphClk; + return SystemCoreClock; } -static inline HAL_StatusTypeDef HAL_DMA_Init(DMA_HandleTypeDef *hdma) { - return MockedHAL_DMA_Init_Impl(hdma); +static inline HAL_StatusTypeDef HAL_DMA_Init(DMA_HandleTypeDef* hdma) { + return MockedHAL_DMA_Init_Impl(hdma); } -static inline HAL_StatusTypeDef -HAL_DMA_Start_IT(DMA_HandleTypeDef *hdma, uint32_t SrcAddress, - uint32_t DstAddress, uint32_t DataLength) { - return MockedHAL_DMA_Start_IT_Impl(hdma, SrcAddress, DstAddress, DataLength); +static inline HAL_StatusTypeDef HAL_DMA_Start_IT( + DMA_HandleTypeDef* hdma, + uint32_t SrcAddress, + uint32_t DstAddress, + uint32_t DataLength +) { + return MockedHAL_DMA_Start_IT_Impl(hdma, SrcAddress, DstAddress, DataLength); } -static inline void HAL_DMA_IRQHandler(DMA_HandleTypeDef *hdma) { - MockedHAL_DMA_IRQHandler_Impl(hdma); +static inline void HAL_DMA_IRQHandler(DMA_HandleTypeDef* hdma) { + MockedHAL_DMA_IRQHandler_Impl(hdma); } -static inline void HAL_GPIO_Init(GPIO_TypeDef *GPIOx, GPIO_InitTypeDef *GPIO_Init) { - (void)GPIOx; - (void)GPIO_Init; +static inline void HAL_GPIO_Init(GPIO_TypeDef* GPIOx, GPIO_InitTypeDef* GPIO_Init) { + (void)GPIOx; + (void)GPIO_Init; } -static inline void HAL_GPIO_WritePin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin, - GPIO_PinState PinState) { - if (PinState == GPIO_PIN_SET) { - GPIOx->ODR |= GPIO_Pin; - } else { - GPIOx->ODR &= ~((uint32_t)GPIO_Pin); - } +static inline void +HAL_GPIO_WritePin(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin, GPIO_PinState PinState) { + if (PinState == GPIO_PIN_SET) { + GPIOx->ODR |= GPIO_Pin; + } else { + GPIOx->ODR &= ~((uint32_t)GPIO_Pin); + } } -static inline void HAL_GPIO_TogglePin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin) { - GPIOx->ODR ^= GPIO_Pin; +static inline void HAL_GPIO_TogglePin(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) { + GPIOx->ODR ^= GPIO_Pin; } -static inline GPIO_PinState HAL_GPIO_ReadPin(GPIO_TypeDef *GPIOx, - uint16_t GPIO_Pin) { - return (GPIOx->ODR & GPIO_Pin) ? GPIO_PIN_SET : GPIO_PIN_RESET; +static inline GPIO_PinState HAL_GPIO_ReadPin(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin) { + return (GPIOx->ODR & GPIO_Pin) ? GPIO_PIN_SET : GPIO_PIN_RESET; } static inline HAL_StatusTypeDef -HAL_TIM_Encoder_Init(TIM_HandleTypeDef *htim, const TIM_Encoder_InitTypeDef *sConfig) { - (void)sConfig; - if (htim == NULL) { - return HAL_ERROR; - } - htim->State = HAL_TIM_STATE_READY; - return HAL_OK; +HAL_TIM_Encoder_Init(TIM_HandleTypeDef* htim, const TIM_Encoder_InitTypeDef* sConfig) { + (void)sConfig; + if (htim == NULL) { + return HAL_ERROR; + } + htim->State = HAL_TIM_STATE_READY; + return HAL_OK; } -static inline HAL_StatusTypeDef -HAL_TIMEx_MasterConfigSynchronization(TIM_HandleTypeDef *htim, - const TIM_MasterConfigTypeDef *sMasterConfig) { - (void)sMasterConfig; - if (htim == NULL) { - return HAL_ERROR; - } - return HAL_OK; +static inline HAL_StatusTypeDef HAL_TIMEx_MasterConfigSynchronization( + TIM_HandleTypeDef* htim, + const TIM_MasterConfigTypeDef* sMasterConfig +) { + (void)sMasterConfig; + if (htim == NULL) { + return HAL_ERROR; + } + return HAL_OK; } -static inline HAL_TIM_StateTypeDef HAL_TIM_Encoder_GetState(const TIM_HandleTypeDef *htim) { - if (htim == NULL) { - return HAL_TIM_STATE_RESET; - } - return htim->State; +static inline HAL_TIM_StateTypeDef HAL_TIM_Encoder_GetState(const TIM_HandleTypeDef* htim) { + if (htim == NULL) { + return HAL_TIM_STATE_RESET; + } + return htim->State; } -static inline HAL_StatusTypeDef HAL_TIM_Encoder_Start(TIM_HandleTypeDef *htim, - uint32_t Channel) { - (void)Channel; - if (htim == NULL) { - return HAL_ERROR; - } - htim->State = HAL_TIM_STATE_BUSY; - return HAL_OK; +static inline HAL_StatusTypeDef HAL_TIM_Encoder_Start(TIM_HandleTypeDef* htim, uint32_t Channel) { + (void)Channel; + if (htim == NULL) { + return HAL_ERROR; + } + htim->State = HAL_TIM_STATE_BUSY; + return HAL_OK; } -static inline HAL_StatusTypeDef HAL_TIM_Encoder_Stop(TIM_HandleTypeDef *htim, - uint32_t Channel) { - (void)Channel; - if (htim == NULL) { - return HAL_ERROR; - } - htim->State = HAL_TIM_STATE_READY; - return HAL_OK; +static inline HAL_StatusTypeDef HAL_TIM_Encoder_Stop(TIM_HandleTypeDef* htim, uint32_t Channel) { + (void)Channel; + if (htim == NULL) { + return HAL_ERROR; + } + htim->State = HAL_TIM_STATE_READY; + return HAL_OK; } -static inline HAL_StatusTypeDef -HAL_TIMEx_ConfigBreakDeadTime(TIM_HandleTypeDef *htim, - const TIM_BreakDeadTimeConfigTypeDef *sBreakDeadTimeConfig) { - (void)htim; - (void)sBreakDeadTimeConfig; - return HAL_OK; +static inline HAL_StatusTypeDef HAL_TIMEx_ConfigBreakDeadTime( + TIM_HandleTypeDef* htim, + const TIM_BreakDeadTimeConfigTypeDef* sBreakDeadTimeConfig +) { + (void)htim; + (void)sBreakDeadTimeConfig; + return HAL_OK; } #ifdef __cplusplus diff --git a/Inc/ST-LIB.hpp b/Inc/ST-LIB.hpp index 2d29c437e..7126130cd 100644 --- a/Inc/ST-LIB.hpp +++ b/Inc/ST-LIB.hpp @@ -9,225 +9,229 @@ class STLIB { public: #ifdef STLIB_ETH - static void start(MAC mac, IPV4 ip, IPV4 subnet_mask, IPV4 gateway, - UART::Peripheral &printf_peripheral); - - static void start(const std::string &mac = "00:80:e1:00:00:00", - const std::string &ip = "192.168.1.4", - const std::string &subnet_mask = "255.255.0.0", - const std::string &gateway = "192.168.1.1", - UART::Peripheral &printf_peripheral = UART::uart2); + static void + start(MAC mac, IPV4 ip, IPV4 subnet_mask, IPV4 gateway, UART::Peripheral& printf_peripheral); + + static void start( + const std::string& mac = "00:80:e1:00:00:00", + const std::string& ip = "192.168.1.4", + const std::string& subnet_mask = "255.255.0.0", + const std::string& gateway = "192.168.1.1", + UART::Peripheral& printf_peripheral = UART::uart2 + ); #else - static void start(UART::Peripheral &printf_peripheral = UART::uart2); + static void start(UART::Peripheral& printf_peripheral = UART::uart2); #endif - static void update(); + static void update(); }; namespace ST_LIB { -extern void compile_error(const char *msg); +extern void compile_error(const char* msg); template struct BuildCtx { - template using Decl = typename D::Entry; - template - static constexpr std::size_t max_count_v = D::max_instances; - - std::tuple, max_count_v>...> storage{}; - std::tuple>...> owners{}; - std::array sizes{}; - - template - static consteval std::size_t domain_index() { - if constexpr (I >= sizeof...(Domains)) { - static_assert([] { return false; }(), "Domain not found"); - return 0; - } else if constexpr (std::is_same_v>>) { - return I; - } else { - return domain_index(); + template using Decl = typename D::Entry; + template static constexpr std::size_t max_count_v = D::max_instances; + + std::tuple, max_count_v>...> storage{}; + std::tuple>...> owners{}; + std::array sizes{}; + + template static consteval std::size_t domain_index() { + if constexpr (I >= sizeof...(Domains)) { + static_assert([] { return false; }(), "Domain not found"); + return 0; + } else if constexpr (std::is_same_v>>) { + return I; + } else { + return domain_index(); + } + } + + template + consteval std::size_t add(typename D::Entry e, const Owner* owner) { + constexpr std::size_t I = domain_index(); + auto& arr = std::get(storage); + auto& own = std::get(owners); + auto& size = sizes[I]; + + const auto idx = size; + arr[size] = e; + own[size] = owner; + ++size; + return idx; + } + + template consteval auto span() const { + constexpr std::size_t I = domain_index(); + auto const& arr = std::get(storage); + auto const size = sizes[I]; + using E = typename D::Entry; + return std::span{arr.data(), size}; + } + + template consteval auto owners_span() const { + constexpr std::size_t I = domain_index(); + auto const& arr = std::get(owners); + auto const size = sizes[I]; + return std::span{arr.data(), size}; + } + + template consteval std::size_t size() const { + constexpr std::size_t I = domain_index(); + return sizes[I]; } - } - - template - consteval std::size_t add(typename D::Entry e, const Owner *owner) { - constexpr std::size_t I = domain_index(); - auto &arr = std::get(storage); - auto &own = std::get(owners); - auto &size = sizes[I]; - - const auto idx = size; - arr[size] = e; - own[size] = owner; - ++size; - return idx; - } - - template consteval auto span() const { - constexpr std::size_t I = domain_index(); - auto const &arr = std::get(storage); - auto const size = sizes[I]; - using E = typename D::Entry; - return std::span{arr.data(), size}; - } - - template consteval auto owners_span() const { - constexpr std::size_t I = domain_index(); - auto const &arr = std::get(owners); - auto const size = sizes[I]; - return std::span{arr.data(), size}; - } - - template consteval std::size_t size() const { - constexpr std::size_t I = domain_index(); - return sizes[I]; - } }; -using DomainsCtx = - BuildCtx; - -template struct Board { - static consteval auto build_ctx() { - DomainsCtx ctx{}; - (devs.inscribe(ctx), ...); - return ctx; - } - - static constexpr auto ctx = build_ctx(); - - template static consteval std::size_t domain_size() { - return ctx.template span().size(); - } - - static consteval auto build() { - constexpr std::size_t mpuN = domain_size(); - constexpr std::size_t gpioN = domain_size(); - constexpr std::size_t timN = domain_size(); - constexpr std::size_t dmaN = domain_size(); - constexpr std::size_t spiN = domain_size(); - constexpr std::size_t doutN = domain_size(); - constexpr std::size_t dinN = domain_size(); - constexpr std::size_t mdmaPacketN = domain_size(); - constexpr std::size_t sdN = domain_size(); - constexpr std::size_t ethN = domain_size(); - constexpr std::size_t adcN = domain_size(); - // ... - - struct ConfigBundle { - std::array mpu_cfgs; - std::array gpio_cfgs; - std::array tim_cfgs; - std::array dma_cfgs; - std::array spi_cfgs; - std::array dout_cfgs; - std::array din_cfgs; - std::array mdma_packet_cfgs; - std::array sd_cfgs; - std::array eth_cfgs; - std::array adc_cfgs; - // ... - }; - - return ConfigBundle{ - .mpu_cfgs = - MPUDomain::template build(ctx.template span()), - .gpio_cfgs = - GPIODomain::template build(ctx.template span()), - .tim_cfgs = - TimerDomain::template build(ctx.template span()), - .dma_cfgs = DMA_Domain::template build( - ctx.template span()), - .spi_cfgs = SPIDomain::template build( - ctx.template span()), - .dout_cfgs = DigitalOutputDomain::template build( - ctx.template span()), - .din_cfgs = DigitalInputDomain::template build( - ctx.template span()), - .mdma_packet_cfgs = MdmaPacketDomain::template build( - ctx.template span()), - .sd_cfgs = SdDomain::template build(ctx.template span()), - .eth_cfgs = EthernetDomain::template build( - ctx.template span()), - .adc_cfgs = - ADCDomain::template build(ctx.template span()), +using DomainsCtx = BuildCtx< + MPUDomain, + GPIODomain, + TimerDomain, + DMA_Domain, + SPIDomain, + DigitalOutputDomain, + DigitalInputDomain, + MdmaPacketDomain, + SdDomain, + EthernetDomain, + ADCDomain /*, PWMDomain, ...*/>; + +template struct Board { + static consteval auto build_ctx() { + DomainsCtx ctx{}; + (devs.inscribe(ctx), ...); + return ctx; + } + + static constexpr auto ctx = build_ctx(); + + template static consteval std::size_t domain_size() { + return ctx.template span().size(); + } + + static consteval auto build() { + constexpr std::size_t mpuN = domain_size(); + constexpr std::size_t gpioN = domain_size(); + constexpr std::size_t timN = domain_size(); + constexpr std::size_t dmaN = domain_size(); + constexpr std::size_t spiN = domain_size(); + constexpr std::size_t doutN = domain_size(); + constexpr std::size_t dinN = domain_size(); + constexpr std::size_t mdmaPacketN = domain_size(); + constexpr std::size_t sdN = domain_size(); + constexpr std::size_t ethN = domain_size(); + constexpr std::size_t adcN = domain_size(); + // ... + + struct ConfigBundle { + std::array mpu_cfgs; + std::array gpio_cfgs; + std::array tim_cfgs; + std::array dma_cfgs; + std::array spi_cfgs; + std::array dout_cfgs; + std::array din_cfgs; + std::array mdma_packet_cfgs; + std::array sd_cfgs; + std::array eth_cfgs; + std::array adc_cfgs; + // ... + }; + + return ConfigBundle{ + .mpu_cfgs = MPUDomain::template build(ctx.template span()), + .gpio_cfgs = GPIODomain::template build(ctx.template span()), + .tim_cfgs = TimerDomain::template build(ctx.template span()), + .dma_cfgs = DMA_Domain::template build(ctx.template span()), + .spi_cfgs = SPIDomain::template build(ctx.template span()), + .dout_cfgs = + DigitalOutputDomain::template build(ctx.template span() + ), + .din_cfgs = + DigitalInputDomain::template build(ctx.template span()), + .mdma_packet_cfgs = + MdmaPacketDomain::template build(ctx.template span() + ), + .sd_cfgs = SdDomain::template build(ctx.template span()), + .eth_cfgs = EthernetDomain::template build(ctx.template span()), + .adc_cfgs = ADCDomain::template build(ctx.template span()), + // ... + }; + } + + static constexpr auto cfg = build(); + + static void init() { + constexpr std::size_t mpuN = domain_size(); + constexpr std::size_t gpioN = domain_size(); + constexpr std::size_t timN = domain_size(); + constexpr std::size_t dmaN = domain_size(); + constexpr std::size_t spiN = domain_size(); + constexpr std::size_t doutN = domain_size(); + constexpr std::size_t dinN = domain_size(); + constexpr std::size_t mdmaPacketN = domain_size(); + constexpr std::size_t sdN = domain_size(); + constexpr std::size_t ethN = domain_size(); + constexpr std::size_t adcN = domain_size(); // ... - }; - } - - static constexpr auto cfg = build(); - - static void init() { - constexpr std::size_t mpuN = domain_size(); - constexpr std::size_t gpioN = domain_size(); - constexpr std::size_t timN = domain_size(); - constexpr std::size_t dmaN = domain_size(); - constexpr std::size_t spiN = domain_size(); - constexpr std::size_t doutN = domain_size(); - constexpr std::size_t dinN = domain_size(); - constexpr std::size_t mdmaPacketN = domain_size(); - constexpr std::size_t sdN = domain_size(); - constexpr std::size_t ethN = domain_size(); - constexpr std::size_t adcN = domain_size(); - // ... #ifdef HAL_IWDG_MODULE_ENABLED - Watchdog::check_reset_flag(); + Watchdog::check_reset_flag(); #endif - HAL_Init(); - HALconfig::system_clock(); - HALconfig::peripheral_clock(); - - MPUDomain::Init::init(); - GPIODomain::Init::init(cfg.gpio_cfgs); - TimerDomain::Init::init(cfg.tim_cfgs); - DMA_Domain::Init::init(cfg.dma_cfgs); - SPIDomain::Init::init(cfg.spi_cfgs, - GPIODomain::Init::instances, - DMA_Domain::Init::instances); - DigitalOutputDomain::Init::init(cfg.dout_cfgs, - GPIODomain::Init::instances); - DigitalInputDomain::Init::init(cfg.din_cfgs, - GPIODomain::Init::instances); - MdmaPacketDomain::Init::init( - cfg.mdma_packet_cfgs, MPUDomain::Init::instances); - SdDomain::Init::init(cfg.sd_cfgs, - MPUDomain::Init::instances, - DigitalInputDomain::Init::instances); - EthernetDomain::Init::init( - cfg.eth_cfgs, DigitalOutputDomain::Init::instances); - ADCDomain::Init::init(cfg.adc_cfgs, - GPIODomain::Init::instances); - // ... - } - - template - static consteval std::size_t owner_index_of() { - constexpr auto owners = ctx.template owners_span(); - - if constexpr (I >= owners.size()) { - compile_error("Device not registered in domain"); - return 0; - } else { - return owners[I] == &Target ? I : owner_index_of(); + HAL_Init(); + HALconfig::system_clock(); + HALconfig::peripheral_clock(); + + MPUDomain::Init::init(); + GPIODomain::Init::init(cfg.gpio_cfgs); + TimerDomain::Init::init(cfg.tim_cfgs); + DMA_Domain::Init::init(cfg.dma_cfgs); + SPIDomain::Init::init( + cfg.spi_cfgs, + GPIODomain::Init::instances, + DMA_Domain::Init::instances + ); + DigitalOutputDomain::Init::init(cfg.dout_cfgs, GPIODomain::Init::instances); + DigitalInputDomain::Init::init(cfg.din_cfgs, GPIODomain::Init::instances); + MdmaPacketDomain::Init::init( + cfg.mdma_packet_cfgs, + MPUDomain::Init::instances + ); + SdDomain::Init::init( + cfg.sd_cfgs, + MPUDomain::Init::instances, + DigitalInputDomain::Init::instances + ); + EthernetDomain::Init::init(cfg.eth_cfgs, DigitalOutputDomain::Init::instances); + ADCDomain::Init::init(cfg.adc_cfgs, GPIODomain::Init::instances); + // ... + } + + template + static consteval std::size_t owner_index_of() { + constexpr auto owners = ctx.template owners_span(); + + if constexpr (I >= owners.size()) { + compile_error("Device not registered in domain"); + return 0; + } else { + return owners[I] == &Target ? I : owner_index_of(); + } } - } - template static auto &instance_of() { - using DevT = std::remove_cvref_t; - using Domain = typename DevT::domain; + template static auto& instance_of() { + using DevT = std::remove_cvref_t; + using Domain = typename DevT::domain; - constexpr std::size_t idx = owner_index_of(); + constexpr std::size_t idx = owner_index_of(); - constexpr std::size_t N = domain_size(); + constexpr std::size_t N = domain_size(); - if constexpr (std::is_same_v) { - return Domain::template Init::instances[idx]; - } else { - return Domain::template Init::instances[idx]; + if constexpr (std::is_same_v) { + return Domain::template Init::instances[idx]; + } else { + return Domain::template Init::instances[idx]; + } } - } }; } // namespace ST_LIB diff --git a/Inc/ST-LIB_HIGH/Control/Blocks/Adder.hpp b/Inc/ST-LIB_HIGH/Control/Blocks/Adder.hpp index dd9a23006..c4b661ac9 100644 --- a/Inc/ST-LIB_HIGH/Control/Blocks/Adder.hpp +++ b/Inc/ST-LIB_HIGH/Control/Blocks/Adder.hpp @@ -1,33 +1,30 @@ -#pragma once +#pragma once #include "../FeedbackControlBlock.hpp" template -concept Sumable = requires(T a, U b){ +concept Sumable = requires(T a, U b) { { a + b }; { b + a }; }; -template -class Adder: public FeedbackControlBlock { - public: - int sign = 1; - Adder() = default; - Adder(SumableType default_input, SumableType default_input_b): FeedbackControlBlock(default_input, default_input_b){} - void execute() override { - this->output_value = this->input_value + sign*this->input_b; - } - void set_sign(char sign){ - switch (sign) - { - case '+': - sign = 1; - break; - case '-': - sign = -1; - break; - default: - break; - } +template class Adder : public FeedbackControlBlock { +public: + int sign = 1; + Adder() = default; + Adder(SumableType default_input, SumableType default_input_b) + : FeedbackControlBlock(default_input, default_input_b) {} + void execute() override { this->output_value = this->input_value + sign * this->input_b; } + void set_sign(char sign) { + switch (sign) { + case '+': + sign = 1; + break; + case '-': + sign = -1; + break; + default: + break; } + } }; diff --git a/Inc/ST-LIB_HIGH/Control/Blocks/Derivator.hpp b/Inc/ST-LIB_HIGH/Control/Blocks/Derivator.hpp index e5dc6312f..3797874bb 100644 --- a/Inc/ST-LIB_HIGH/Control/Blocks/Derivator.hpp +++ b/Inc/ST-LIB_HIGH/Control/Blocks/Derivator.hpp @@ -2,98 +2,104 @@ #include "../ControlBlock.hpp" -enum FilterDerivatorType{ - None, - ForwardEuler, - BackwardEuler, - Trapezoidal, - Moving_Average -}; +enum FilterDerivatorType { None, ForwardEuler, BackwardEuler, Trapezoidal, Moving_Average }; + +template class BDFDerivator : public ControlBlock { +public: + double period; + static constexpr int order = N - 1; + double buffer[N + 1] = {0.0}; + double (&ks)[order]; + int index = 0; + uint64_t counter = 0; -template -class BDFDerivator : public ControlBlock{ - public: - double period; - static constexpr int order = N-1; - double buffer[N+1] = {0.0}; - double (&ks)[order]; - int index = 0; - uint64_t counter = 0; - public: - BDFDerivator(double period, double(&ks)[order]): period(period), ks(ks){this->output_value = 0.0;} - BDFDerivator(double period, double(&&ks)[order]): period(period), ks(move(ks)){this->output_value = 0.0;} - void execute()override{ - if(counter++output_value = 0.0; + } + BDFDerivator(double period, double (&&ks)[order]) : period(period), ks(move(ks)) { + this->output_value = 0.0; + } + void execute() override { + if (counter++ < order) { buffer[index] = input_value; - output_value = 0.0; - for(int i = 1; i <= order; i++){ - output_value += ks[i-1]*(buffer[index]-buffer[((index-i)%(N+1) + (N+1))%(N+1)])/i; - } - output_value /= (period*order); index++; - index %= (N+1); - counter--; + index %= (N + 1); + output_value = 0.0; + return; + } + buffer[index] = input_value; + output_value = 0.0; + for (int i = 1; i <= order; i++) { + output_value += ks[i - 1] * + (buffer[index] - buffer[((index - i) % (N + 1) + (N + 1)) % (N + 1)]) / + i; } + output_value /= (period * order); + index++; + index %= (N + 1); + counter--; + } - void reset(){ - output_value = 0; - counter = 0.0; - for(double& e : buffer){ - e = 0.0; - } + void reset() { + output_value = 0; + counter = 0.0; + for (double& e : buffer) { + e = 0.0; } + } }; -class SimpleDerivator : public ControlBlock{ - public: - double period; - static constexpr int N = 2; - double buffer[N] = {0.0}; - int index = 0; - public: - SimpleDerivator(double period): ControlBlock(0.0), period(period){ output_value = 0.0;} - void execute()override{ - buffer[index] = input_value; - output_value = (buffer[index] - buffer[((index-1)%(N) + (N))%(N)])/period; - index++; - index %= N; - return; - } - void reset(){ - output_value = 0; - index = 0; - for(int i = 0; i < N; i++){ - buffer[i] = 0.0; - } +class SimpleDerivator : public ControlBlock { +public: + double period; + static constexpr int N = 2; + double buffer[N] = {0.0}; + int index = 0; + +public: + SimpleDerivator(double period) : ControlBlock(0.0), period(period) { + output_value = 0.0; + } + void execute() override { + buffer[index] = input_value; + output_value = (buffer[index] - buffer[((index - 1) % (N) + (N)) % (N)]) / period; + index++; + index %= N; + return; + } + void reset() { + output_value = 0; + index = 0; + for (int i = 0; i < N; i++) { + buffer[i] = 0.0; } + } }; -class SimpleFloatDerivator : public ControlBlock{ - public: - float period; - static constexpr int N = 2; - float buffer[N] = {0.0}; - int index = 0; - public: - SimpleFloatDerivator(float period): ControlBlock(0.0), period(period){ output_value = 0.0;} - void execute()override{ - buffer[index] = input_value; - output_value = (buffer[index] - buffer[((index-1)%(N) + (N))%(N)])/period; - index++; - index %= N; - return; - } - void reset(){ - output_value = 0; - index = 0; - for(int i = 0; i < N; i++){ - buffer[i] = 0.0; - } +class SimpleFloatDerivator : public ControlBlock { +public: + float period; + static constexpr int N = 2; + float buffer[N] = {0.0}; + int index = 0; + +public: + SimpleFloatDerivator(float period) : ControlBlock(0.0), period(period) { + output_value = 0.0; + } + void execute() override { + buffer[index] = input_value; + output_value = (buffer[index] - buffer[((index - 1) % (N) + (N)) % (N)]) / period; + index++; + index %= N; + return; + } + void reset() { + output_value = 0; + index = 0; + for (int i = 0; i < N; i++) { + buffer[i] = 0.0; } + } }; diff --git a/Inc/ST-LIB_HIGH/Control/Blocks/Integrator.hpp b/Inc/ST-LIB_HIGH/Control/Blocks/Integrator.hpp index f699aa08e..a8e87788d 100644 --- a/Inc/ST-LIB_HIGH/Control/Blocks/Integrator.hpp +++ b/Inc/ST-LIB_HIGH/Control/Blocks/Integrator.hpp @@ -3,166 +3,170 @@ #pragma once -enum class IntegratorType{ - ForwardEuler, - BackwardEuler, - Trapezoidal -}; +enum class IntegratorType { ForwardEuler, BackwardEuler, Trapezoidal }; + +template class Integrator; -template -class Integrator; +template class FloatIntegrator; -template -class FloatIntegrator; +template <> class Integrator : public ControlBlock { +public: + static constexpr int N = 2; + double period; + double buffer[N] = {0.0}; + int index = 0; + double integral = 0.0; + double ki; + bool first_execution = true; -template<> -class Integrator : public ControlBlock{ - public: - static constexpr int N = 2; - double period; - double buffer[N] = {0.0}; - int index = 0; - double integral = 0.0; - double ki; - bool first_execution = true; - - Integrator() = default; - Integrator(double period, double ki):ControlBlock(0.0), period(period), ki(ki){output_value = 0.0;} - void execute() override { - buffer[index] = input_value; - if(first_execution){ - first_execution = false; - index++; - index %= N; - output_value = 0.0; - return; - } - integral += ki*period*(buffer[index] + buffer[((index-1)%(N) + (N))%(N)])/2.0; - output_value = integral; + Integrator() = default; + Integrator(double period, double ki) + : ControlBlock(0.0), period(period), ki(ki) { + output_value = 0.0; + } + void execute() override { + buffer[index] = input_value; + if (first_execution) { + first_execution = false; index++; index %= N; + output_value = 0.0; return; } - void reset(){ - output_value = 0; - first_execution = true; - integral = 0.0; - index = 0; - for(int i = 0; i < N; i++){ - buffer[i] = 0.0; - } + integral += ki * period * (buffer[index] + buffer[((index - 1) % (N) + (N)) % (N)]) / 2.0; + output_value = integral; + index++; + index %= N; + return; + } + void reset() { + output_value = 0; + first_execution = true; + integral = 0.0; + index = 0; + for (int i = 0; i < N; i++) { + buffer[i] = 0.0; } + } }; -template<> -class FloatIntegrator : public ControlBlock{ - public: - static constexpr int N = 2; - float period; - float buffer[N] = {0.0}; - int index = 0; - float integral = 0.0; - float ki; - bool first_execution = true; +template <> class FloatIntegrator : public ControlBlock { +public: + static constexpr int N = 2; + float period; + float buffer[N] = {0.0}; + int index = 0; + float integral = 0.0; + float ki; + bool first_execution = true; - FloatIntegrator() = default; - FloatIntegrator(float period, float ki):ControlBlock(0.0), period(period), ki(ki){output_value = 0.0;} - void execute() override { - buffer[index] = input_value; - if(first_execution){ - first_execution = false; - index++; - index %= N; - output_value = 0.0; - return; - } - integral += ki*period*(buffer[index] + buffer[((index-1)%(N) + (N))%(N)])/2.0; - output_value = integral; + FloatIntegrator() = default; + FloatIntegrator(float period, float ki) + : ControlBlock(0.0), period(period), ki(ki) { + output_value = 0.0; + } + void execute() override { + buffer[index] = input_value; + if (first_execution) { + first_execution = false; index++; index %= N; + output_value = 0.0; return; } - void reset(){ - output_value = 0; - first_execution = true; - integral = 0.0; - index = 0; - for(int i = 0; i < N; i++){ - buffer[i] = 0.0; - } + integral += ki * period * (buffer[index] + buffer[((index - 1) % (N) + (N)) % (N)]) / 2.0; + output_value = integral; + index++; + index %= N; + return; + } + void reset() { + output_value = 0; + first_execution = true; + integral = 0.0; + index = 0; + for (int i = 0; i < N; i++) { + buffer[i] = 0.0; } + } }; -template<> -class Integrator : public ControlBlock{ - public: - static constexpr int N = 2; - double period; - double buffer[N] = {0.0}; - int index = 0; - double integral = 0.0; - double ki; - bool first_execution = true; - public: - Integrator(double period, double ki):ControlBlock(0.0), period(period), ki(ki) {this->output_value = 0.0;} - void execute() override { - buffer[index] = input_value; - if(first_execution){ - first_execution = false; - index++; - index %= N; - output_value = 0.0; - return; - } - integral += ki*period*buffer[((index-1)%(N) + (N))%(N)]; - output_value = integral; +template <> class Integrator : public ControlBlock { +public: + static constexpr int N = 2; + double period; + double buffer[N] = {0.0}; + int index = 0; + double integral = 0.0; + double ki; + bool first_execution = true; + +public: + Integrator(double period, double ki) + : ControlBlock(0.0), period(period), ki(ki) { + this->output_value = 0.0; + } + void execute() override { + buffer[index] = input_value; + if (first_execution) { + first_execution = false; index++; index %= N; + output_value = 0.0; return; } - void reset(){ - first_execution = true; - integral = 0.0; - index = 0; - for(int i = 0; i < N; i++){ - buffer[i] = 0.0; - } + integral += ki * period * buffer[((index - 1) % (N) + (N)) % (N)]; + output_value = integral; + index++; + index %= N; + return; + } + void reset() { + first_execution = true; + integral = 0.0; + index = 0; + for (int i = 0; i < N; i++) { + buffer[i] = 0.0; } + } }; -template<> -class Integrator: public ControlBlock{ - public: - static constexpr int N = 2; - double period; - double buffer[N] = {0.0}; - int index = 0; - double integral = 0.0; - double ki; - bool first_execution = true; - public: - Integrator(double period, double ki):ControlBlock(0.0), period(period), ki(ki) {output_value = 0.0;} - void execute() override { - buffer[index] = input_value; - if(first_execution){ - first_execution = false; - index++; - index %= N; - output_value = 0.0; - return; - } - integral += ki*period*buffer[index]; - output_value = integral; +template <> class Integrator : public ControlBlock { +public: + static constexpr int N = 2; + double period; + double buffer[N] = {0.0}; + int index = 0; + double integral = 0.0; + double ki; + bool first_execution = true; + +public: + Integrator(double period, double ki) + : ControlBlock(0.0), period(period), ki(ki) { + output_value = 0.0; + } + void execute() override { + buffer[index] = input_value; + if (first_execution) { + first_execution = false; index++; index %= N; + output_value = 0.0; return; } - void reset(){ - first_execution = true; - integral = 0.0; - index = 0; - for(int i = 0; i < N; i++){ - buffer[i] = 0.0; - } + integral += ki * period * buffer[index]; + output_value = integral; + index++; + index %= N; + return; + } + void reset() { + first_execution = true; + integral = 0.0; + index = 0; + for (int i = 0; i < N; i++) { + buffer[i] = 0.0; } + } }; diff --git a/Inc/ST-LIB_HIGH/Control/Blocks/MatrixMultiplier.hpp b/Inc/ST-LIB_HIGH/Control/Blocks/MatrixMultiplier.hpp index c329082f9..70ab549d5 100644 --- a/Inc/ST-LIB_HIGH/Control/Blocks/MatrixMultiplier.hpp +++ b/Inc/ST-LIB_HIGH/Control/Blocks/MatrixMultiplier.hpp @@ -1,51 +1,57 @@ #pragma once #include "../ControlBlock.hpp" -template class MatrixMultiplier; +template class MatrixMultiplier; + +template + requires(U > 1 && V > 1 && W > 1) +class MatrixMultiplier : public ControlBlock { + float (&A)[U][V], (&B)[V][W], (&result)[U][W]; -template requires (U > 1 && V > 1 && W > 1) -class MatrixMultiplier : public ControlBlock{ - float(&A)[U][V], (&B)[V][W], (&result)[U][W]; public: - MatrixMultiplier(float(&A)[U][V], float(&B)[V][W], float(&result)[U][W]) : A(A), B(B), result(result){ + MatrixMultiplier(float (&A)[U][V], float (&B)[V][W], float (&result)[U][W]) + : A(A), B(B), result(result) { this->input_value = &A; this->output_value = &result; - } - - void execute() override{ - for (size_t i = 0; i < U; i++) { - for (size_t j = 0; j < W; j++) { - result[i][j] = 0; - for (size_t k = 0; k < V; k++) - result[i][j] += A[i][k] * B[k][j]; - } - } - } - - void reset(){ - for (size_t i = 0; i < U; i++) { - for (size_t j = 0; j < W; j++) - result[i][j] = 0.0; - } - } + } + + void execute() override { + for (size_t i = 0; i < U; i++) { + for (size_t j = 0; j < W; j++) { + result[i][j] = 0; + for (size_t k = 0; k < V; k++) + result[i][j] += A[i][k] * B[k][j]; + } + } + } + + void reset() { + for (size_t i = 0; i < U; i++) { + for (size_t j = 0; j < W; j++) + result[i][j] = 0.0; + } + } }; -//CTAD +// CTAD #if __cpp_deduction_guides >= 201606 -template requires (U > 1 && V > 1 && W > 1) -MatrixMultiplier(float(&)[U][V], float(&)[V][W], float(&)[U][W])->MatrixMultiplier; +template + requires(U > 1 && V > 1 && W > 1) +MatrixMultiplier(float (&)[U][V], float (&)[V][W], float (&)[U][W]) -> MatrixMultiplier; #endif -template -class MatrixMultiplier : public ControlBlock{ - float(&A)[U][V], (&B)[V], (&result)[U]; +template +class MatrixMultiplier : public ControlBlock { + float (&A)[U][V], (&B)[V], (&result)[U]; + public: - MatrixMultiplier(float(&A)[U][V], float(&B)[V], float(&result)[U]) : A(A), B(B), result(result){ + MatrixMultiplier(float (&A)[U][V], float (&B)[V], float (&result)[U]) + : A(A), B(B), result(result) { this->input_value = &A; this->output_value = &result; } - void execute() override{ + void execute() override { for (size_t i = 0; i < U; i++) { result[i] = 0; for (size_t k = 0; k < V; k++) @@ -53,29 +59,32 @@ class MatrixMultiplier : public ControlBlock } } - void reset(){ - for(size_t i = 0; i < U; i++){ - result[i] = 0.0; - } + void reset() { + for (size_t i = 0; i < U; i++) { + result[i] = 0.0; + } } }; -//CTAD +// CTAD #if __cpp_deduction_guides >= 201606 -template requires (U > 1 && V > 1) -MatrixMultiplier(float(&)[U][V], float(&)[V], float(&)[U])->MatrixMultiplier; +template + requires(U > 1 && V > 1) +MatrixMultiplier(float (&)[U][V], float (&)[V], float (&)[U]) -> MatrixMultiplier; #endif -template -class MatrixMultiplier<1,U,V> : public ControlBlock{ - float(&A)[U], (&B)[U][V], (&result)[V]; +template +class MatrixMultiplier<1, U, V> : public ControlBlock { + float (&A)[U], (&B)[U][V], (&result)[V]; + public: - MatrixMultiplier(float(&A)[U], float(&B)[U][V], float(&result)[V]) : A(A), B(B), result(result){ + MatrixMultiplier(float (&A)[U], float (&B)[U][V], float (&result)[V]) + : A(A), B(B), result(result) { this->input_value = &A; this->output_value = &result; } - void execute() override{ + void execute() override { for (size_t j = 0; j < V; j++) { result[j] = 0; for (size_t k = 0; k < U; k++) @@ -83,41 +92,41 @@ class MatrixMultiplier<1,U,V> : public ControlBlock{ } } - void reset(){ - for(size_t i = 0; i < V; i++){ - result[i] = 0.0; - } + void reset() { + for (size_t i = 0; i < V; i++) { + result[i] = 0.0; + } } }; -//CTAD +// CTAD #if __cpp_deduction_guides >= 201606 -template requires (U > 1 && V > 1) -MatrixMultiplier(float(&)[U], float(&)[U][V], float(&)[V])->MatrixMultiplier<1,U,V>; +template + requires(U > 1 && V > 1) +MatrixMultiplier(float (&)[U], float (&)[U][V], float (&)[V]) -> MatrixMultiplier<1, U, V>; #endif -template -class MatrixMultiplier<1,U,1> : public ControlBlock{ - float(&A)[U], (&B)[U], &result; +template class MatrixMultiplier<1, U, 1> : public ControlBlock { + float (&A)[U], (&B)[U], &result; + public: - MatrixMultiplier(float(&A)[U], float(&B)[U], float(&result)) : A(A), B(B), result(result){ + MatrixMultiplier(float (&A)[U], float (&B)[U], float(&result)) : A(A), B(B), result(result) { this->input_value = &A; this->output_value = &result; } - void execute() override{ + void execute() override { result = 0; for (size_t k = 0; k < U; k++) result += A[k] * B[k]; } - void reset(){ - result = 0; - } + void reset() { result = 0; } }; -//CTAD +// CTAD #if __cpp_deduction_guides >= 201606 -template requires (U > 1) -MatrixMultiplier(float(&)[U], float(&)[U], float(&))->MatrixMultiplier<1,U,1>; +template + requires(U > 1) +MatrixMultiplier(float (&)[U], float (&)[U], float(&)) -> MatrixMultiplier<1, U, 1>; #endif diff --git a/Inc/ST-LIB_HIGH/Control/Blocks/MeanCalculator.hpp b/Inc/ST-LIB_HIGH/Control/Blocks/MeanCalculator.hpp index c45e48f8a..4ccaf9c7b 100644 --- a/Inc/ST-LIB_HIGH/Control/Blocks/MeanCalculator.hpp +++ b/Inc/ST-LIB_HIGH/Control/Blocks/MeanCalculator.hpp @@ -2,24 +2,25 @@ #include "../ControlBlock.hpp" #include "ErrorHandler/ErrorHandler.hpp" -template -class MeanCalculator : public ControlBlock{ +template class MeanCalculator : public ControlBlock { public: int index = 0; double mean = 0.0; + public: - MeanCalculator():ControlBlock(0.0){ - output_value = 0.0; - } - void execute() override { - mean += input_value/N; + MeanCalculator() : ControlBlock(0.0) { output_value = 0.0; } + void execute() override { + mean += input_value / N; index++; - if(index > N) ErrorHandler("MeanCalculator is receiving just 0"); - if(index == N) output_value = mean; - else output_value = 0.0; + if (index > N) + ErrorHandler("MeanCalculator is receiving just 0"); + if (index == N) + output_value = mean; + else + output_value = 0.0; return; } - void reset(){ + void reset() { mean = 0.0; index = 0; output_value = 0.0; diff --git a/Inc/ST-LIB_HIGH/Control/Blocks/MovingAverage.hpp b/Inc/ST-LIB_HIGH/Control/Blocks/MovingAverage.hpp index b5be8a2ad..7a7c0e65b 100644 --- a/Inc/ST-LIB_HIGH/Control/Blocks/MovingAverage.hpp +++ b/Inc/ST-LIB_HIGH/Control/Blocks/MovingAverage.hpp @@ -7,153 +7,149 @@ #include "../ControlSystem.hpp" #include "HALAL/Models/Concepts/Concepts.hpp" -template -class MovingAverage : public ControlBlock { - private: - double buffer[N] = {0.0}; - uint32_t first = 0, last = -1; - uint32_t counter = 0; - double accumulator = 0.0; - - public: - MovingAverage(): ControlBlock(){output_value = 0.0;} - - void execute() override { - if(counter < N) { - last++; - buffer[last] = input_value; - accumulator += input_value/N; - counter++; - return; - } - accumulator -= buffer[first] / N; - first = (first + 1) % N; - last = (last + 1) % N; +template class MovingAverage : public ControlBlock { +private: + double buffer[N] = {0.0}; + uint32_t first = 0, last = -1; + uint32_t counter = 0; + double accumulator = 0.0; + +public: + MovingAverage() : ControlBlock() { output_value = 0.0; } + + void execute() override { + if (counter < N) { + last++; buffer[last] = input_value; - accumulator += buffer[last] / N; - output_value = accumulator; + accumulator += input_value / N; + counter++; return; } - - double compute(double input_v) { - this->input(input_v); - execute(); - return this->output_value; - } - - void reset(){ - first = 0; - last = -1; - counter = 0; - accumulator = 0; - output_value = 0; - for(double& e: buffer){ - e = 0; - } + accumulator -= buffer[first] / N; + first = (first + 1) % N; + last = (last + 1) % N; + buffer[last] = input_value; + accumulator += buffer[last] / N; + output_value = accumulator; + return; + } + + double compute(double input_v) { + this->input(input_v); + execute(); + return this->output_value; + } + + void reset() { + first = 0; + last = -1; + counter = 0; + accumulator = 0; + output_value = 0; + for (double& e : buffer) { + e = 0; } + } }; +template class FloatMovingAverage : public ControlBlock { +private: + float buffer[N] = {0.0}; + uint32_t first = 0, last = -1; + uint32_t counter = 0; + float accumulator = 0.0; + +public: + FloatMovingAverage() : ControlBlock() { output_value = 0.0; } -template -class FloatMovingAverage : public ControlBlock { - private: - float buffer[N] = {0.0}; - uint32_t first = 0, last = -1; - uint32_t counter = 0; - float accumulator = 0.0; - - public: - FloatMovingAverage(): ControlBlock(){output_value = 0.0;} - - void execute() override { - if(counter < N) { - last++; - buffer[last] = input_value; - accumulator += input_value/N; - counter++; - return; - } - accumulator -= buffer[first] / N; - first = (first + 1) % N; - last = (last + 1) % N; + void execute() override { + if (counter < N) { + last++; buffer[last] = input_value; - accumulator += buffer[last] / N; - output_value = accumulator; + accumulator += input_value / N; + counter++; return; } - - float compute(float input_v) { - this->input(input_v); - execute(); - return this->output_value; - } - - void reset(){ - first = 0; - last = -1; - counter = 0; - accumulator = 0; - output_value = 0; - for(float& e: buffer){ - e = 0; - } + accumulator -= buffer[first] / N; + first = (first + 1) % N; + last = (last + 1) % N; + buffer[last] = input_value; + accumulator += buffer[last] / N; + output_value = accumulator; + return; + } + + float compute(float input_v) { + this->input(input_v); + execute(); + return this->output_value; + } + + void reset() { + first = 0; + last = -1; + counter = 0; + accumulator = 0; + output_value = 0; + for (float& e : buffer) { + e = 0; } + } }; - - - - /** - * @brief version of moving average for integers. Shifts the values by DecimalBitCount before adding to transform them into fixed arithmetic logic + * @brief version of moving average for integers. Shifts the values by DecimalBitCount before adding + * to transform them into fixed arithmetic logic * - * This version of the moving average implements the same logic as the MovingAverage class, but for any kind of integral values. - * For any DecimalBitCount greater than one, it is recommended that the OutputType is of greater size than the InputType to avoid overflows. - * The result is given in the Fixed Point Arithmetic of DecimalBitCount number of decimal bits. That is to say, it is shifted to the left by that amount. - * To translate back, shift the value to the right by DecimalBitCount bits. Needless to say, that will remove the decimal data. + * This version of the moving average implements the same logic as the MovingAverage class, but for + * any kind of integral values. For any DecimalBitCount greater than one, it is recommended that the + * OutputType is of greater size than the InputType to avoid overflows. The result is given in the + * Fixed Point Arithmetic of DecimalBitCount number of decimal bits. That is to say, it is shifted + * to the left by that amount. To translate back, shift the value to the right by DecimalBitCount + * bits. Needless to say, that will remove the decimal data. */ -template +template class IntegerMovingAverage : public ControlBlock { - private: - FollowingUint::Value buffer[N] = {0}; - uint32_t first = 0, last = -1; - uint32_t counter = 0; - FollowingUint::Value accumulator = 0.0; - - public: - IntegerMovingAverage(): ControlBlock(){this->output_value = 0;} - - void execute() override { - if(counter < N) { - last++; - buffer[last] = ((decltype(accumulator)) this->input_value) << DecimalBitCount; - accumulator += buffer[last]/N; - counter++; - return; - } - accumulator -= buffer[first] / N; - first = (first + 1) % N; - last = (last + 1) % N; - buffer[last] = ((decltype(accumulator)) this->input_value) << DecimalBitCount; - accumulator += buffer[last]/ N; - this->output_value = (OutputType) accumulator; +private: + FollowingUint::Value buffer[N] = {0}; + uint32_t first = 0, last = -1; + uint32_t counter = 0; + FollowingUint::Value accumulator = 0.0; + +public: + IntegerMovingAverage() : ControlBlock() { this->output_value = 0; } + + void execute() override { + if (counter < N) { + last++; + buffer[last] = ((decltype(accumulator))this->input_value) << DecimalBitCount; + accumulator += buffer[last] / N; + counter++; return; } - - OutputType compute(InputType input_v) { - this->input(input_v); - execute(); - return this->output_value; - } - - void reset(){ - first = 0; - last = -1; - counter = 0; - accumulator = 0; - this->output_value = 0; - for(InputType& e: buffer){ - e = 0; - } + accumulator -= buffer[first] / N; + first = (first + 1) % N; + last = (last + 1) % N; + buffer[last] = ((decltype(accumulator))this->input_value) << DecimalBitCount; + accumulator += buffer[last] / N; + this->output_value = (OutputType)accumulator; + return; + } + + OutputType compute(InputType input_v) { + this->input(input_v); + execute(); + return this->output_value; + } + + void reset() { + first = 0; + last = -1; + counter = 0; + accumulator = 0; + this->output_value = 0; + for (InputType& e : buffer) { + e = 0; } + } }; diff --git a/Inc/ST-LIB_HIGH/Control/Blocks/PI.hpp b/Inc/ST-LIB_HIGH/Control/Blocks/PI.hpp index c70de2f7c..cae277359 100644 --- a/Inc/ST-LIB_HIGH/Control/Blocks/PI.hpp +++ b/Inc/ST-LIB_HIGH/Control/Blocks/PI.hpp @@ -3,29 +3,25 @@ #include "../ControlBlock.hpp" #include "Integrator.hpp" -template -class PI: public ControlBlock{ - public: - double kp; - double error; - Integrator integrator; - public: - PI() = default; - PI(double kp, double ki, double period): kp(kp), integrator(period,ki) {} - void execute() override { - integrator.input(this->input_value); - integrator.execute(); - error = this -> input_value; - this -> output_value = kp*error + integrator.output_value; - } - void set_kp(double kp){ - this->kp = kp; - } - void set_ki(double ki){ - integrator.ki = ki; - integrator.reset(); - } - void reset(){ - integrator.reset(); - } +template class PI : public ControlBlock { +public: + double kp; + double error; + Integrator integrator; + +public: + PI() = default; + PI(double kp, double ki, double period) : kp(kp), integrator(period, ki) {} + void execute() override { + integrator.input(this->input_value); + integrator.execute(); + error = this->input_value; + this->output_value = kp * error + integrator.output_value; + } + void set_kp(double kp) { this->kp = kp; } + void set_ki(double ki) { + integrator.ki = ki; + integrator.reset(); + } + void reset() { integrator.reset(); } }; diff --git a/Inc/ST-LIB_HIGH/Control/Blocks/PID.hpp b/Inc/ST-LIB_HIGH/Control/Blocks/PID.hpp index fc050386b..90b6d537d 100644 --- a/Inc/ST-LIB_HIGH/Control/Blocks/PID.hpp +++ b/Inc/ST-LIB_HIGH/Control/Blocks/PID.hpp @@ -4,80 +4,85 @@ #pragma once -template class PID; +template +class PID; -template -class PID{ - public: - double kp, ki, kd; - double error = 0; - SimpleDerivator derivator; - Integrator integrator; - double output_value = 0.0; - public: - PID(double kp, double ki, double kd, double period): kp(kp), ki(ki), kd(kd), derivator(period), integrator(period,ki) {} - double execute(double input_value){ - derivator.input(input_value); - derivator.execute(); - integrator.input(input_value); - integrator.execute(); - error = input_value; - this -> output_value = kp*error + integrator.output_value + kd*derivator.output_value; - return output_value; - } - void set_kp(double kp){ - this->kp = kp; - reset(); - } - void set_ki(double ki){ - this->ki = ki; - reset(); - } - void set_kd(double kd){ - this->kd = kd; - reset(); - } - void reset(){ - integrator.reset(); - derivator.reset(); - } +template class PID { +public: + double kp, ki, kd; + double error = 0; + SimpleDerivator derivator; + Integrator integrator; + double output_value = 0.0; + +public: + PID(double kp, double ki, double kd, double period) + : kp(kp), ki(ki), kd(kd), derivator(period), integrator(period, ki) {} + double execute(double input_value) { + derivator.input(input_value); + derivator.execute(); + integrator.input(input_value); + integrator.execute(); + error = input_value; + this->output_value = kp * error + integrator.output_value + kd * derivator.output_value; + return output_value; + } + void set_kp(double kp) { + this->kp = kp; + reset(); + } + void set_ki(double ki) { + this->ki = ki; + reset(); + } + void set_kd(double kd) { + this->kd = kd; + reset(); + } + void reset() { + integrator.reset(); + derivator.reset(); + } }; -template -class PID : public ControlBlock{ - public: - double kp, ki, kd; - double error = 0; - SimpleDerivator derivator; - Integrator integrator; - MovingAverage filter_derivative; - public: - PID(double kp, double ki, double kd, double period): kp(kp), ki(ki), kd(kd), derivator(period), integrator(period,ki) {} - void execute() override{ - derivator.input(input_value); - derivator.execute(); - integrator.input(input_value); - integrator.execute(); - error = input_value; - filter_derivative.input(kd*derivator.output_value); - filter_derivative.execute(); - this->output_value = kp*error + integrator.output_value + filter_derivative.output_value; - } - void set_kp(double kp){ - this->kp = kp; - reset(); - } - void set_ki(double ki){ - this->ki = ki; - reset(); - } - void set_kd(double kd){ - this->kd = kd; - reset(); - } - void reset(){ - integrator.reset(); - derivator.reset(); - filter_derivative.reset(); - } +template +class PID + : public ControlBlock { +public: + double kp, ki, kd; + double error = 0; + SimpleDerivator derivator; + Integrator integrator; + MovingAverage filter_derivative; + +public: + PID(double kp, double ki, double kd, double period) + : kp(kp), ki(ki), kd(kd), derivator(period), integrator(period, ki) {} + void execute() override { + derivator.input(input_value); + derivator.execute(); + integrator.input(input_value); + integrator.execute(); + error = input_value; + filter_derivative.input(kd * derivator.output_value); + filter_derivative.execute(); + this->output_value = kp * error + integrator.output_value + filter_derivative.output_value; + } + void set_kp(double kp) { + this->kp = kp; + reset(); + } + void set_ki(double ki) { + this->ki = ki; + reset(); + } + void set_kd(double kd) { + this->kd = kd; + reset(); + } + void reset() { + integrator.reset(); + derivator.reset(); + filter_derivative.reset(); + } }; diff --git a/Inc/ST-LIB_HIGH/Control/Blocks/Saturator.hpp b/Inc/ST-LIB_HIGH/Control/Blocks/Saturator.hpp index de2dca6f0..880cef6b3 100644 --- a/Inc/ST-LIB_HIGH/Control/Blocks/Saturator.hpp +++ b/Inc/ST-LIB_HIGH/Control/Blocks/Saturator.hpp @@ -2,34 +2,35 @@ #pragma once template -concept Comparable = requires(T a, U b){ +concept Comparable = requires(T a, U b) { { a < b }; { b > a }; }; template -class Saturator: public ControlBlock { - public: - ComparableType lower_bound = 0, upper_bound = 0; - Saturator(ComparableType default_input,ComparableType lower_bound,ComparableType upper_bound): ControlBlock(default_input), - lower_bound(lower_bound), upper_bound(upper_bound){} - Saturator(Saturator&& new_ref): ControlBlock(new_ref.default_input) { - upper_bound = new_ref.upper_bound; - lower_bound = new_ref.lower_bound; - } - void execute() override { - if(this->input_value > upper_bound){ - this->output_value = upper_bound; - } else if(this->input_value < lower_bound){ - this->output_value = lower_bound; - } else { - this->output_value = this->input_value; - } +class Saturator : public ControlBlock { +public: + ComparableType lower_bound = 0, upper_bound = 0; + Saturator(ComparableType default_input, ComparableType lower_bound, ComparableType upper_bound) + : ControlBlock(default_input), lower_bound(lower_bound), + upper_bound(upper_bound) {} + Saturator(Saturator&& new_ref) + : ControlBlock(new_ref.default_input) { + upper_bound = new_ref.upper_bound; + lower_bound = new_ref.lower_bound; + } + void execute() override { + if (this->input_value > upper_bound) { + this->output_value = upper_bound; + } else if (this->input_value < lower_bound) { + this->output_value = lower_bound; + } else { + this->output_value = this->input_value; } + } }; #if __cpp_deduction_guides >= 201606 -template -Saturator(ComparableType,ComparableType,ComparableType) -> Saturator; +template +Saturator(ComparableType, ComparableType, ComparableType) -> Saturator; #endif - diff --git a/Inc/ST-LIB_HIGH/Control/Blocks/Zeroing.hpp b/Inc/ST-LIB_HIGH/Control/Blocks/Zeroing.hpp index 9e0c5c448..4d69e5d9c 100644 --- a/Inc/ST-LIB_HIGH/Control/Blocks/Zeroing.hpp +++ b/Inc/ST-LIB_HIGH/Control/Blocks/Zeroing.hpp @@ -2,32 +2,26 @@ #include "../ControlBlock.hpp" #include "MeanCalculator.hpp" #include "Sensors/LinearSensor/LinearSensor.hpp" -template -class Zeroing : public ControlBlock { - public: +template class Zeroing : public ControlBlock { +public: LinearSensor& sensor; MeanCalculator mean_calculator; Type max_value_offset; bool has_max_value = false; - Zeroing(LinearSensor& sensor) : sensor(sensor), mean_calculator() {}; + Zeroing(LinearSensor& sensor) : sensor(sensor), mean_calculator(){}; Zeroing(LinearSensor& sensor, Type max_value_offset) - : mean_calculator(), - sensor(sensor), - max_value_offset(max_value_offset), - has_max_value(true) {}; + : mean_calculator(), sensor(sensor), max_value_offset(max_value_offset), + has_max_value(true){}; void execute() { while (mean_calculator.output_value == 0) { sensor.read(); mean_calculator.input(*sensor.get_value_pointer()); mean_calculator.execute(); } - if (has_max_value && - abs(mean_calculator.output_value) > max_value_offset) { - ErrorHandler( - "Zeroing offset is calculated to be above specified maximum"); + if (has_max_value && abs(mean_calculator.output_value) > max_value_offset) { + ErrorHandler("Zeroing offset is calculated to be above specified maximum"); } else - sensor.set_offset(sensor.get_offset() - - mean_calculator.output_value); + sensor.set_offset(sensor.get_offset() - mean_calculator.output_value); mean_calculator.reset(); } }; diff --git a/Inc/ST-LIB_HIGH/Control/ControlBlock.hpp b/Inc/ST-LIB_HIGH/Control/ControlBlock.hpp index 9940ff1d5..a0e2c8e59 100644 --- a/Inc/ST-LIB_HIGH/Control/ControlBlock.hpp +++ b/Inc/ST-LIB_HIGH/Control/ControlBlock.hpp @@ -2,27 +2,24 @@ #include "C++Utilities/CppUtils.hpp" -template -class ControlBlock{ - public: - using Input = InputType; - using Output = OutputType; - InputType input_value; - OutputType output_value; - ControlBlock() = default; - ControlBlock(InputType default_input); - virtual void input (InputType input); - virtual void execute() = 0; +template class ControlBlock { +public: + using Input = InputType; + using Output = OutputType; + InputType input_value; + OutputType output_value; + ControlBlock() = default; + ControlBlock(InputType default_input); + virtual void input(InputType input); + virtual void execute() = 0; }; -template -ControlBlock::ControlBlock(InputType default_input){ +template +ControlBlock::ControlBlock(InputType default_input) { input_value = default_input; } -template -void ControlBlock::input(InputType input_value){ +template +void ControlBlock::input(InputType input_value) { this->input_value = input_value; } - - diff --git a/Inc/ST-LIB_HIGH/Control/ControlSystem.hpp b/Inc/ST-LIB_HIGH/Control/ControlSystem.hpp index c50b0d386..f3f9f3098 100644 --- a/Inc/ST-LIB_HIGH/Control/ControlSystem.hpp +++ b/Inc/ST-LIB_HIGH/Control/ControlSystem.hpp @@ -1,125 +1,160 @@ -#pragma once +#pragma once #include "ControlBlock.hpp" -template -class ControlSystem; - -template -concept ControlSystemConcept = requires(System system){ - system.is_control_system; -}; - -template<> -class ControlSystem<>{ - public: - static constexpr bool is_control_system = true; - static constexpr bool is_empty = true; - ControlSystem() = default; - template - ControlSystem connect(ControlBlock& next); - template - void execute(ControlSystem* parent); - template - void input(ControlSystem* parent, InputType input); - template - static auto create_control_system(System base, ControlBlock& block, ControlBlocks&... blocks) requires ControlSystemConcept; - template - static auto create_control_system(ControlBlock& block, ControlBlocks&... blocks); +template class ControlSystem; + +template +concept ControlSystemConcept = requires(System system) { system.is_control_system; }; + +template <> class ControlSystem<> { +public: + static constexpr bool is_control_system = true; + static constexpr bool is_empty = true; + ControlSystem() = default; + template + ControlSystem connect(ControlBlock& next); + template + void execute(ControlSystem* parent); + template + void input(ControlSystem* parent, InputType input); + template + static auto + create_control_system(System base, ControlBlock& block, ControlBlocks&... blocks) + requires ControlSystemConcept; + template + static auto create_control_system(ControlBlock& block, ControlBlocks&... blocks); }; -ControlSystem()->ControlSystem<>; - -template -class ControlSystem : public ControlSystem{ - public: - static constexpr bool is_control_system = true; - static constexpr bool is_empty = false; - ControlBlock* control_block; - ControlSystem() = default; - ControlSystem(ControlBlock& control_block, const ControlSystem& following_system); - template - ControlSystem connect(ControlBlock& next); - template - void execute(ControlSystem* parent); - void execute(); - template - void input(ControlSystem* parent, FirstInputType input_value); - template - void input(FirstInputType input_value); +ControlSystem() -> ControlSystem<>; + +template +class ControlSystem + : public ControlSystem { +public: + static constexpr bool is_control_system = true; + static constexpr bool is_empty = false; + ControlBlock* control_block; + ControlSystem() = default; + ControlSystem( + ControlBlock& control_block, + const ControlSystem& following_system + ); + template + ControlSystem + connect(ControlBlock& next); + template + void execute(ControlSystem< + IncomingOutputType, + IncomingInputType, + OutputType, + InputType, + FollowingTypes...>* parent); + void execute(); + template + void input( + ControlSystem* parent, + FirstInputType input_value + ); + template void input(FirstInputType input_value); }; -template -ControlSystem(ControlBlock&, ...)->ControlSystem; +template +ControlSystem(ControlBlock&, ...) + -> ControlSystem; -template -ControlSystem ControlSystem<>::connect(ControlBlock& next){ - ControlSystem new_system(next,*this); +template +ControlSystem +ControlSystem<>::connect(ControlBlock& next) { + ControlSystem new_system(next, *this); return new_system; } -template -auto ControlSystem<>::create_control_system(System base, ControlBlock& block, ControlBlocks&... blocks) requires ControlSystemConcept{ - if constexpr(sizeof...(ControlBlocks) < 1){ +template +auto ControlSystem<>::create_control_system( + System base, + ControlBlock& block, + ControlBlocks&... blocks +) + requires ControlSystemConcept +{ + if constexpr (sizeof...(ControlBlocks) < 1) { return base.connect(block); - }else{ - return create_control_system(base.connect(block),blocks...); + } else { + return create_control_system(base.connect(block), blocks...); } } -template -auto ControlSystem<>::create_control_system(ControlBlock& block, ControlBlocks&... blocks){ +template +auto ControlSystem<>::create_control_system( + ControlBlock& block, + ControlBlocks&... blocks +) { ControlSystem<> base; - return create_control_system(base,block,blocks...); + return create_control_system(base, block, blocks...); } -template -void ControlSystem<>::execute(ControlSystem* parent){} +template +void ControlSystem<>::execute(ControlSystem* parent) {} -template -void ControlSystem<>::input(ControlSystem* parent, InputType input_value){ +template +void ControlSystem<>::input(ControlSystem* parent, InputType input_value) { parent->control_block->input(input_value); } -template -ControlSystem::ControlSystem(ControlBlock& control_block, const ControlSystem& following_system) : ControlSystem(following_system){ - if(is_empty == false){ +template +ControlSystem::ControlSystem( + ControlBlock& control_block, + const ControlSystem& following_system +) + : ControlSystem(following_system) { + if (is_empty == false) { this->control_block = &control_block; } } -template -template -void ControlSystem::execute(ControlSystem* parent){ - if constexpr(is_empty == false){ +template +template +void ControlSystem::execute( + ControlSystem* + parent +) { + if constexpr (is_empty == false) { static_cast*>(this)->execute(this); control_block->execute(); parent->control_block->input(control_block->output_value); } } -template -template -void ControlSystem::input(ControlSystem* parent, FirstInputType input_value){ - if constexpr(is_empty == false){ - static_cast*>(this)->input(this,input_value); +template +template +void ControlSystem::input( + ControlSystem* parent, + FirstInputType input_value +) { + if constexpr (is_empty == false) { + static_cast*>(this)->input(this, input_value); } } -template -template -void ControlSystem::input(FirstInputType input_value){ - input(this,input_value); +template +template +void ControlSystem::input(FirstInputType input_value) { + input(this, input_value); } -template -void ControlSystem::execute(){ +template +void ControlSystem::execute() { static_cast*>(this)->execute(this); control_block->execute(); } -template -template -ControlSystem ControlSystem::connect(ControlBlock& next){ - ControlSystem new_system(next,*this); +template +template +ControlSystem +ControlSystem::connect( + ControlBlock& next +) { + ControlSystem + new_system(next, *this); return new_system; } diff --git a/Inc/ST-LIB_HIGH/Control/FeedbackControlBlock.hpp b/Inc/ST-LIB_HIGH/Control/FeedbackControlBlock.hpp index 118105aac..91053191c 100644 --- a/Inc/ST-LIB_HIGH/Control/FeedbackControlBlock.hpp +++ b/Inc/ST-LIB_HIGH/Control/FeedbackControlBlock.hpp @@ -1,24 +1,24 @@ #pragma once #include "ControlBlock.hpp" -template -class FeedbackControlBlock : public ControlBlock { +template +class FeedbackControlBlock : public ControlBlock { public: using Input = InputType; using Output = OutputType; InputType input_value_b; FeedbackControlBlock() = default; - FeedbackControlBlock(InputType default_input, InputType default_input_b): ControlBlock(default_input){} - FeedbackControlBlock(InputType default_input): ControlBlock(default_input){} + FeedbackControlBlock(InputType default_input, InputType default_input_b) + : ControlBlock(default_input) {} + FeedbackControlBlock(InputType default_input) + : ControlBlock(default_input) {} - void input (InputType input_a, InputType input_b){ + void input(InputType input_a, InputType input_b) { this->input_value = input_a; this->input_value_b = input_b; } virtual void execute() = 0; - void input_b (InputType input){ - this->input_value_b = input; - } + void input_b(InputType input) { this->input_value_b = input; } }; diff --git a/Inc/ST-LIB_HIGH/Control/SplitterBlock.hpp b/Inc/ST-LIB_HIGH/Control/SplitterBlock.hpp index 8de3ea52c..d3dc86918 100644 --- a/Inc/ST-LIB_HIGH/Control/SplitterBlock.hpp +++ b/Inc/ST-LIB_HIGH/Control/SplitterBlock.hpp @@ -2,25 +2,28 @@ #include "FeedbackControlBlock.hpp" -template -class SplitterBlock : public ControlBlock { +template class SplitterBlock : public ControlBlock { public: using Input = InputType; using Output = InputType; function feedback_input; SplitterBlock() = default; - SplitterBlock(InputType default_input): ControlBlock(default_input){} + SplitterBlock(InputType default_input) : ControlBlock(default_input) {} void execute() override { this->output_value = this->input_value; feedback(); } - void feedback(){ - if(feedback_input != nullptr){ + void feedback() { + if (feedback_input != nullptr) { feedback_input(this->output_value); } } - template - void connect(FeedbackControlBlock& next){ - feedback_input = bind(&FeedbackControlBlock::input_b, &next, placeholders::_1); + template + void connect(FeedbackControlBlock& next) { + feedback_input = bind( + &FeedbackControlBlock::input_b, + &next, + placeholders::_1 + ); } }; diff --git a/Inc/ST-LIB_HIGH/FlashStorer/FlashStorer.hpp b/Inc/ST-LIB_HIGH/FlashStorer/FlashStorer.hpp index 410f6d3c6..cf679c8b4 100644 --- a/Inc/ST-LIB_HIGH/FlashStorer/FlashStorer.hpp +++ b/Inc/ST-LIB_HIGH/FlashStorer/FlashStorer.hpp @@ -5,65 +5,62 @@ #include "HALAL/Models/Concepts/Concepts.hpp" #include "HALAL/Services/Flash/Flash.hpp" -#define FLASH_STORER_MAX_SIZE ((uint32_t)(1028*64)) -#define FLASH_STORER_START_ADDRESS (FLASH_STORER_MAX_ADDRESS - FLASH_STORER_MAX_SIZE) -#define FLASH_STORER_MAX_ADDRESS ((uint32_t)0x080DFFF0U) +#define FLASH_STORER_MAX_SIZE ((uint32_t)(1028 * 64)) +#define FLASH_STORER_START_ADDRESS (FLASH_STORER_MAX_ADDRESS - FLASH_STORER_MAX_SIZE) +#define FLASH_STORER_MAX_ADDRESS ((uint32_t)0x080DFFF0U) - -class FlashStorer{ +class FlashStorer { public: - static vector variable_list; - static uint32_t total_size; + static vector variable_list; + static uint32_t total_size; - template - static bool add_variables(Type&... var); - static void read(void); - static bool store_all(void); - template - static bool store(Type& var); - template requires requires{requires sizeof...(Type) != 1;} - static bool store(Type&... var); + template static bool add_variables(Type&... var); + static void read(void); + static bool store_all(void); + template static bool store(Type& var); + template + requires requires { requires sizeof...(Type) != 1; } + static bool store(Type&... var); private: - template - static void add_size(Type& var);//?Mirar, si no hace falta borrar + template static void add_size(Type& var); //?Mirar, si no hace falta borrar }; -template -bool FlashStorer::store(Type& var) { - uint32_t number_of_words = ((FlashStorer::total_size + FLASH_32BITS_WORLD - 1) / FLASH_32BITS_WORLD); //Fast ceiling the dvision result +template bool FlashStorer::store(Type& var) { + uint32_t number_of_words = + ((FlashStorer::total_size + FLASH_32BITS_WORLD - 1) / FLASH_32BITS_WORLD + ); // Fast ceiling the dvision result - uint32_t* data = (uint32_t*)malloc(number_of_words * FLASH_32BITS_WORLD); - Flash::read(FLASH_STORER_START_ADDRESS, (uint32_t*)data, number_of_words); + uint32_t* data = (uint32_t*)malloc(number_of_words * FLASH_32BITS_WORLD); + Flash::read(FLASH_STORER_START_ADDRESS, (uint32_t*)data, number_of_words); - uint32_t offset = 0; - for(FlashVariable& v: FlashStorer::variable_list){ - if (v.get_pointer() == &var) { - memcpy((((uint8_t*)data) + offset), v.get_pointer(), v.get_size()); - } + uint32_t offset = 0; + for (FlashVariable& v : FlashStorer::variable_list) { + if (v.get_pointer() == &var) { + memcpy((((uint8_t*)data) + offset), v.get_pointer(), v.get_size()); + } - offset += v.get_size(); - } + offset += v.get_size(); + } - if(not Flash::write(data, FLASH_STORER_START_ADDRESS, number_of_words)){ - return false; - } + if (not Flash::write(data, FLASH_STORER_START_ADDRESS, number_of_words)) { + return false; + } - free(data); - return true; + free(data); + return true; } -template requires requires{requires sizeof...(Type) != 1;} -bool FlashStorer::store(Type&... var){ - return (FlashStorer::store(var) & ...); +template + requires requires { requires sizeof...(Type) != 1; } +bool FlashStorer::store(Type&... var) { + return (FlashStorer::store(var) & ...); } -template -bool FlashStorer::add_variables(Type&... var){ - (FlashStorer::variable_list.push_back(FlashVariable(var)), ...); - FlashStorer::total_size = total_sizeof::value; +template bool FlashStorer::add_variables(Type&... var) { + (FlashStorer::variable_list.push_back(FlashVariable(var)), ...); + FlashStorer::total_size = total_sizeof::value; - return FlashStorer::total_size >= FLASH_STORER_MAX_SIZE; + return FlashStorer::total_size >= FLASH_STORER_MAX_SIZE; } - diff --git a/Inc/ST-LIB_HIGH/FlashStorer/FlashVariable.hpp b/Inc/ST-LIB_HIGH/FlashStorer/FlashVariable.hpp index 4919c2b25..9be7613bf 100644 --- a/Inc/ST-LIB_HIGH/FlashStorer/FlashVariable.hpp +++ b/Inc/ST-LIB_HIGH/FlashStorer/FlashVariable.hpp @@ -3,19 +3,18 @@ #include "C++Utilities/CppUtils.hpp" #include "ErrorHandler/ErrorHandler.hpp" -class FlashVariable{ +class FlashVariable { private: - void* v_pointer; - size_t size; + void* v_pointer; + size_t size; + public: - template - FlashVariable(T& value); - void* get_pointer(); - size_t get_size(); + template FlashVariable(T& value); + void* get_pointer(); + size_t get_size(); }; -template -FlashVariable::FlashVariable(T& value){ - FlashVariable::v_pointer = (void*)&value; - FlashVariable::size = sizeof(T); +template FlashVariable::FlashVariable(T& value) { + FlashVariable::v_pointer = (void*)&value; + FlashVariable::size = sizeof(T); } diff --git a/Inc/ST-LIB_HIGH/Protections/Boundary.hpp b/Inc/ST-LIB_HIGH/Protections/Boundary.hpp index d939f93e5..d24779816 100644 --- a/Inc/ST-LIB_HIGH/Protections/Boundary.hpp +++ b/Inc/ST-LIB_HIGH/Protections/Boundary.hpp @@ -8,8 +8,7 @@ #include "HALAL/Services/Time/RTC.hpp" using type_id_t = void (*)(); -template -void type_id() {} +template void type_id() {} namespace Protections { enum FaultType : uint8_t { FAULT = 0, WARNING, OK }; @@ -27,7 +26,7 @@ enum ProtectionType : uint8_t { }; struct BoundaryInterface { - public: +public: virtual Protections::FaultType check_bounds() = 0; HeapOrder* fault_message{nullptr}; HeapOrder* warn_message{nullptr}; @@ -35,36 +34,25 @@ struct BoundaryInterface { void update_name(char* n) { name = n; if (strlen(n) > NAME_MAX_LEN) { - ErrorHandler("Variable name is too long, max length is %d", - NAME_MAX_LEN); + ErrorHandler("Variable name is too long, max length is %d", NAME_MAX_LEN); return; } string_len = name.size(); } - virtual void update_error_handler_message( - [[maybe_unused]] const char* err_message) {} - virtual void update_warning_message( - [[maybe_unused]] const char* warn_message) {} - static const char* get_error_handler_string() { - return ErrorHandlerModel::description.c_str(); - } - static const char* get_warning_string() { - return InfoWarning::description.c_str(); - } + virtual void update_error_handler_message([[maybe_unused]] const char* err_message) {} + virtual void update_warning_message([[maybe_unused]] const char* warn_message) {} + static const char* get_error_handler_string() { return ErrorHandlerModel::description.c_str(); } + static const char* get_warning_string() { return InfoWarning::description.c_str(); } uint8_t boundary_type_id{}; // used to send messages only on raising/failing edges bool warning_already_triggered{false}; bool warning_is_up{false}; bool back_to_normal{false}; - protected: +protected: static const map format_look_up; - static int get_error_handler_string_size() { - return ErrorHandlerModel::description.size(); - } - static int get_warning_string_size() { - return InfoWarning::description.size(); - } + static int get_error_handler_string_size() { return ErrorHandlerModel::description.size(); } + static int get_warning_string_size() { return InfoWarning::description.size(); } // this will store the name of the variable string name; @@ -74,11 +62,9 @@ struct BoundaryInterface { uint8_t string_len{0}; }; -template -struct Boundary; +template struct Boundary; -template -struct Boundary : public BoundaryInterface { +template struct Boundary : public BoundaryInterface { static constexpr ProtectionType Protector = BELOW; bool has_warning_level{false}; Type* src = nullptr; @@ -95,9 +81,7 @@ struct Boundary : public BoundaryInterface { }; Boundary(Type boundary) : boundary(boundary) {} Boundary(Type* src, Boundary boundary) - : has_warning_level(boundary.has_warning_level), - src(src), - boundary(boundary.boundary) { + : has_warning_level(boundary.has_warning_level), src(src), boundary(boundary.boundary) { // we have to do this because we cannot take address of rvalue // (ProtectionType::BELOW) boundary_type_id = Protector; @@ -108,36 +92,68 @@ struct Boundary : public BoundaryInterface { if (this->has_warning_level) { warning_threshold = boundary.warning_threshold; warn_message = new HeapOrder( - uint16_t{2000}, &format_id, &boundary_type_id, &name, - &this->warning_threshold, &this->frozen_value, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{2000}, + &format_id, + &boundary_type_id, + &name, + &this->warning_threshold, + &this->frozen_value, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); ok_message = new HeapOrder( - uint16_t{3000}, &format_id, &boundary_type_id, &name, - &this->warning_threshold, &this->frozen_value, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{3000}, + &format_id, + &boundary_type_id, + &name, + &this->warning_threshold, + &this->frozen_value, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } else { ok_message = new HeapOrder( - uint16_t{3000}, &format_id, &boundary_type_id, &name, - &this->boundary, &this->frozen_value, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{3000}, + &format_id, + &boundary_type_id, + &name, + &this->boundary, + &this->frozen_value, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } fault_message = new HeapOrder( - uint16_t{1000}, &format_id, &boundary_type_id, &name, - &this->boundary, &this->frozen_value, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{1000}, + &format_id, + &boundary_type_id, + &name, + &this->boundary, + &this->frozen_value, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } Boundary(Type* src, Type boundary) : src(src), boundary(boundary) {} @@ -153,8 +169,7 @@ struct Boundary : public BoundaryInterface { } }; -template -struct Boundary : public BoundaryInterface { +template struct Boundary : public BoundaryInterface { static constexpr ProtectionType Protector = ABOVE; bool has_warning_level{false}; Type* src = nullptr; @@ -163,18 +178,14 @@ struct Boundary : public BoundaryInterface { Type frozen_value{}; Boundary(Type warning_threshold, Type boundary) - : has_warning_level{true}, - boundary(boundary), - warning_threshold(warning_threshold) { + : has_warning_level{true}, boundary(boundary), warning_threshold(warning_threshold) { if (warning_threshold > boundary) { ErrorHandler("Warning threshold is above boundary"); } }; - Boundary(Type boundary) : boundary(boundary) {}; + Boundary(Type boundary) : boundary(boundary){}; Boundary(Type* src, Boundary boundary) - : has_warning_level(boundary.has_warning_level), - src(src), - boundary(boundary.boundary) { + : has_warning_level(boundary.has_warning_level), src(src), boundary(boundary.boundary) { // we have to do this because we cannot take address of rvalue // (ProtectionType::BELOW) boundary_type_id = Protector; @@ -185,42 +196,75 @@ struct Boundary : public BoundaryInterface { if (this->has_warning_level) { warning_threshold = boundary.warning_threshold; warn_message = new HeapOrder( - uint16_t{2111}, &format_id, &boundary_type_id, &name, - &this->warning_threshold, &this->frozen_value, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{2111}, + &format_id, + &boundary_type_id, + &name, + &this->warning_threshold, + &this->frozen_value, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); ok_message = new HeapOrder( - uint16_t{3111}, &format_id, &boundary_type_id, &name, - &this->warning_threshold, &this->frozen_value, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{3111}, + &format_id, + &boundary_type_id, + &name, + &this->warning_threshold, + &this->frozen_value, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } else { ok_message = new HeapOrder( - uint16_t{3111}, &format_id, &boundary_type_id, &name, - &this->boundary, &this->frozen_value, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{3111}, + &format_id, + &boundary_type_id, + &name, + &this->boundary, + &this->frozen_value, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } fault_message = new HeapOrder( - uint16_t{1111}, &format_id, &boundary_type_id, &name, - &this->boundary, &this->frozen_value, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{1111}, + &format_id, + &boundary_type_id, + &name, + &this->boundary, + &this->frozen_value, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } Boundary(Type* src, Type boundary) : src(src), boundary(boundary) {} Protections::FaultType check_bounds() override { frozen_value = *src; - if (*src > boundary) return Protections::FAULT; + if (*src > boundary) + return Protections::FAULT; if (has_warning_level && *src > warning_threshold) { return Protections::WARNING; } @@ -228,79 +272,110 @@ struct Boundary : public BoundaryInterface { } }; -template -struct Boundary : public BoundaryInterface { +template struct Boundary : public BoundaryInterface { static constexpr ProtectionType Protector = EQUALS; Type* src = nullptr; Type boundary; Type frozen_value{}; - Boundary(Type boundary) : boundary(boundary) {}; + Boundary(Type boundary) : boundary(boundary){}; Boundary(Type* src, Boundary boundary) : src(src), boundary(boundary.boundary) { boundary_type_id = Protector; format_id = BoundaryInterface::format_look_up.at(type_id); name.reserve(NAME_MAX_LEN); fault_message = new HeapOrder( - uint16_t{1333}, &format_id, &boundary_type_id, &name, - &this->boundary, &this->frozen_value, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{1333}, + &format_id, + &boundary_type_id, + &name, + &this->boundary, + &this->frozen_value, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); ok_message = new HeapOrder( - uint16_t{2333}, &format_id, &boundary_type_id, &name, - &this->boundary, &this->frozen_value, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{2333}, + &format_id, + &boundary_type_id, + &name, + &this->boundary, + &this->frozen_value, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } Boundary(Type* src, Type boundary) : src(src), boundary(boundary) {} Protections::FaultType check_bounds() override { - if (*src == boundary) return Protections::FAULT; + if (*src == boundary) + return Protections::FAULT; return Protections::OK; } }; -template -struct Boundary : public BoundaryInterface { +template struct Boundary : public BoundaryInterface { static constexpr ProtectionType Protector = NOT_EQUALS; Type* src = nullptr; Type boundary; Type frozen_value{}; - Boundary(Type boundary) : boundary(boundary) {}; + Boundary(Type boundary) : boundary(boundary){}; Boundary(Type* src, Boundary boundary) : src(src), boundary(boundary.boundary) { boundary_type_id = Protector; format_id = BoundaryInterface::format_look_up.at(type_id); name.reserve(NAME_MAX_LEN); fault_message = new HeapOrder( - uint16_t{1444}, &format_id, &boundary_type_id, &name, - &this->boundary, &this->frozen_value, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{1444}, + &format_id, + &boundary_type_id, + &name, + &this->boundary, + &this->frozen_value, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); ok_message = new HeapOrder( - uint16_t{2444}, &format_id, &boundary_type_id, &name, - &this->boundary, &this->frozen_value, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{2444}, + &format_id, + &boundary_type_id, + &name, + &this->boundary, + &this->frozen_value, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } Boundary(Type* src, Type boundary) : src(src), boundary(boundary) {} Protections::FaultType check_bounds() override { frozen_value = *src; - if (*src != boundary) return Protections::FAULT; + if (*src != boundary) + return Protections::FAULT; return Protections::OK; } }; -template -struct Boundary : public BoundaryInterface { +template struct Boundary : public BoundaryInterface { static constexpr ProtectionType Protector = OUT_OF_RANGE; Type* src = nullptr; Type lower_warning; @@ -310,27 +385,19 @@ struct Boundary : public BoundaryInterface { Type frozen_value{}; bool has_warning_level{false}; - Boundary(Type lower_warning, Type upper_warning, Type lower_boundary, - Type upper_boundary) - : lower_warning(lower_warning), - upper_warning(upper_warning), - lower_boundary(lower_boundary), - upper_boundary(upper_boundary), - has_warning_level{true} - { + Boundary(Type lower_warning, Type upper_warning, Type lower_boundary, Type upper_boundary) + : lower_warning(lower_warning), upper_warning(upper_warning), + lower_boundary(lower_boundary), upper_boundary(upper_boundary), has_warning_level{true} { if (lower_warning < lower_boundary || upper_warning > upper_boundary) { ErrorHandler("Warning thresholds are outside of boundaries"); } }; Boundary(Type lower_boundary, Type upper_boundary) - : lower_warning(std::numeric_limits::min()), - upper_warning(std::numeric_limits::max()), - lower_boundary(lower_boundary), - upper_boundary(upper_boundary) - {}; + : lower_warning(std::numeric_limits::min()), + upper_warning(std::numeric_limits::max()), lower_boundary(lower_boundary), + upper_boundary(upper_boundary){}; Boundary(Type* src, Boundary boundary) - : src(src), - lower_boundary(boundary.lower_boundary), + : src(src), lower_boundary(boundary.lower_boundary), upper_boundary(boundary.upper_boundary) { boundary_type_id = Protector; format_id = BoundaryInterface::format_look_up.at(type_id); @@ -339,111 +406,167 @@ struct Boundary : public BoundaryInterface { lower_warning = boundary.lower_warning; upper_warning = boundary.upper_warning; warn_message = new HeapOrder( - uint16_t{2222}, &format_id, &boundary_type_id, &name, - &boundary.lower_boundary, &boundary.upper_boundary, - &this->frozen_value, &Global_RTC::global_RTC.counter, - &Global_RTC::global_RTC.second, &Global_RTC::global_RTC.minute, - &Global_RTC::global_RTC.hour, &Global_RTC::global_RTC.day, - &Global_RTC::global_RTC.month, &Global_RTC::global_RTC.year); + uint16_t{2222}, + &format_id, + &boundary_type_id, + &name, + &boundary.lower_boundary, + &boundary.upper_boundary, + &this->frozen_value, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); ok_message = new HeapOrder( - uint16_t{3222}, &format_id, &boundary_type_id, &name, - &boundary.lower_warning, &boundary.upper_warning, - &this->frozen_value, &Global_RTC::global_RTC.counter, - &Global_RTC::global_RTC.second, &Global_RTC::global_RTC.minute, - &Global_RTC::global_RTC.hour, &Global_RTC::global_RTC.day, - &Global_RTC::global_RTC.month, &Global_RTC::global_RTC.year); + uint16_t{3222}, + &format_id, + &boundary_type_id, + &name, + &boundary.lower_warning, + &boundary.upper_warning, + &this->frozen_value, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } else { ok_message = new HeapOrder( - uint16_t{3222}, &format_id, &boundary_type_id, &name, - &this->lower_boundary, &this->upper_boundary, - &this->frozen_value, &Global_RTC::global_RTC.counter, - &Global_RTC::global_RTC.second, &Global_RTC::global_RTC.minute, - &Global_RTC::global_RTC.hour, &Global_RTC::global_RTC.day, - &Global_RTC::global_RTC.month, &Global_RTC::global_RTC.year); + uint16_t{3222}, + &format_id, + &boundary_type_id, + &name, + &this->lower_boundary, + &this->upper_boundary, + &this->frozen_value, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } fault_message = new HeapOrder( - uint16_t{1222}, &format_id, &boundary_type_id, &name, - &this->lower_boundary, &this->upper_boundary, &this->frozen_value, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{1222}, + &format_id, + &boundary_type_id, + &name, + &this->lower_boundary, + &this->upper_boundary, + &this->frozen_value, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } Boundary(Type* src, Type lower_boundary, Type upper_boundary) - : src(src), - lower_boundary(lower_boundary), - upper_boundary(upper_boundary) {} + : src(src), lower_boundary(lower_boundary), upper_boundary(upper_boundary) {} Protections::FaultType check_bounds() override { frozen_value = *src; if (*src < lower_boundary || *src > upper_boundary) return Protections::FAULT; - if (has_warning_level && - ((*src < lower_boundary) || (*src > upper_boundary))) { + if (has_warning_level && ((*src < lower_boundary) || (*src > upper_boundary))) { return Protections::WARNING; } return Protections::OK; } }; -template <> -struct Boundary : public BoundaryInterface { +template <> struct Boundary : public BoundaryInterface { static constexpr ProtectionType Protector = ERROR_HANDLER; Boundary(void*) { boundary_type_id = Protector; error_handler_string.reserve(ERROR_HANDLER_MSG_MAX_LEN); fault_message = new HeapOrder( - uint16_t{1555}, &padding, &boundary_type_id, &name, - &error_handler_string, &Global_RTC::global_RTC.counter, - &Global_RTC::global_RTC.second, &Global_RTC::global_RTC.minute, - &Global_RTC::global_RTC.hour, &Global_RTC::global_RTC.day, - &Global_RTC::global_RTC.month, &Global_RTC::global_RTC.year); + uint16_t{1555}, + &padding, + &boundary_type_id, + &name, + &error_handler_string, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } uint8_t padding{}; Boundary(void*, Boundary) { boundary_type_id = Protector; error_handler_string.reserve(ERROR_HANDLER_MSG_MAX_LEN); fault_message = new HeapOrder( - uint16_t{1555}, &padding, &boundary_type_id, &name, - &error_handler_string, &Global_RTC::global_RTC.counter, - &Global_RTC::global_RTC.second, &Global_RTC::global_RTC.minute, - &Global_RTC::global_RTC.hour, &Global_RTC::global_RTC.day, - &Global_RTC::global_RTC.month, &Global_RTC::global_RTC.year); + uint16_t{1555}, + &padding, + &boundary_type_id, + &name, + &error_handler_string, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } Boundary() = default; Protections::FaultType check_bounds() override { - return not ErrorHandlerModel::error_triggered ? Protections::OK - : Protections::FAULT; + return not ErrorHandlerModel::error_triggered ? Protections::OK : Protections::FAULT; } void update_error_handler_message(const char* err_message) override { error_handler_string = err_message; if (strlen(err_message) > ERROR_HANDLER_MSG_MAX_LEN) { - ErrorHandler("Error Handler message is too long, max length is %d", - ERROR_HANDLER_MSG_MAX_LEN); + ErrorHandler( + "Error Handler message is too long, max length is %d", + ERROR_HANDLER_MSG_MAX_LEN + ); return; } error_handler_string_len = error_handler_string.size(); } - private: +private: string error_handler_string{}; uint16_t error_handler_string_len{}; static constexpr uint16_t ERROR_HANDLER_MSG_MAX_LEN = 255; }; -template <> -struct Boundary : public BoundaryInterface { +template <> struct Boundary : public BoundaryInterface { static constexpr ProtectionType Protector = INFO_WARNING; Boundary(void*) { boundary_type_id = Protector - 2; warning_string.reserve(WARNING_HANDLER_MSG_MAX_LEN); warn_message = new HeapOrder( - uint16_t{2555}, &padding, &boundary_type_id, &name, &warning_string, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{2555}, + &padding, + &boundary_type_id, + &name, + &warning_string, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } uint8_t padding{}; Boundary(void*, Boundary) { @@ -451,28 +574,37 @@ struct Boundary : public BoundaryInterface { boundary_type_id = Protector - 2; warning_string.reserve(WARNING_HANDLER_MSG_MAX_LEN); warn_message = new HeapOrder( - uint16_t{2555}, &padding, &boundary_type_id, &name, &warning_string, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{2555}, + &padding, + &boundary_type_id, + &name, + &warning_string, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } Boundary() = default; Protections::FaultType check_bounds() override { - return InfoWarning::warning_triggered ? Protections::WARNING - : Protections::OK; + return InfoWarning::warning_triggered ? Protections::WARNING : Protections::OK; } void update_warning_message(const char* warn_message) override { warning_string = warn_message; if (strlen(warn_message) > WARNING_HANDLER_MSG_MAX_LEN) { - ErrorHandler("Error Handler message is too long, max length is %d", - WARNING_HANDLER_MSG_MAX_LEN); + ErrorHandler( + "Error Handler message is too long, max length is %d", + WARNING_HANDLER_MSG_MAX_LEN + ); return; } warning_string_len = warning_string.size(); } - private: +private: string warning_string{}; uint16_t warning_string_len{}; static constexpr uint16_t WARNING_HANDLER_MSG_MAX_LEN = 255; @@ -482,36 +614,35 @@ template requires(std::is_floating_point_v) struct Boundary : public BoundaryInterface { static constexpr ProtectionType Protector = TIME_ACCUMULATION; - Boundary(Type bound, float time_limit, float frequency, - Boundary*& external_pointer) - : real_still_good(new Protections::FaultType{Protections::OK}), - bound(bound), - time_limit(time_limit), - frequency(frequency), - moving_order(frequency * time_limit / 100), + Boundary( + Type bound, + float time_limit, + float frequency, + Boundary*& external_pointer + ) + : real_still_good(new Protections::FaultType{Protections::OK}), bound(bound), + time_limit(time_limit), frequency(frequency), moving_order(frequency * time_limit / 100), external_pointer(&external_pointer) { external_pointer = this; }; - Boundary(Type warning_threshold, Type bound, float time_limit, - float frequency, Boundary*& external_pointer) - : real_still_good(new Protections::FaultType{Protections::OK}), - bound(bound), - time_limit(time_limit), - frequency(frequency), - moving_order(frequency * time_limit / 100), + Boundary( + Type warning_threshold, + Type bound, + float time_limit, + float frequency, + Boundary*& external_pointer + ) + : real_still_good(new Protections::FaultType{Protections::OK}), bound(bound), + time_limit(time_limit), frequency(frequency), moving_order(frequency * time_limit / 100), external_pointer(&external_pointer) { external_pointer = this; has_warning_level = true; this->warning_threshold = warning_threshold; }; Boundary(Type* src, Boundary boundary) - : real_still_good(boundary.real_still_good), - src(src), - bound(boundary.bound), - time_limit(boundary.time_limit), - frequency(boundary.frequency), - moving_order(frequency * time_limit / 100), - external_pointer(boundary.external_pointer) { + : real_still_good(boundary.real_still_good), src(src), bound(boundary.bound), + time_limit(boundary.time_limit), frequency(boundary.frequency), + moving_order(frequency * time_limit / 100), external_pointer(boundary.external_pointer) { *external_pointer = this; boundary_type_id = Protector; format_id = BoundaryInterface::format_look_up.at(type_id); @@ -519,37 +650,63 @@ struct Boundary : public BoundaryInterface { if (boundary.has_warning_level) { warning_threshold = boundary.warning_threshold; warn_message = new HeapOrder( - uint16_t{2666}, &format_id, &boundary_type_id, &name, - &this->warning_threshold, &this->bound, &this->frozen_value, - &this->time_limit, &this->frequency, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{2666}, + &format_id, + &boundary_type_id, + &name, + &this->warning_threshold, + &this->bound, + &this->frozen_value, + &this->time_limit, + &this->frequency, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); ok_message = new HeapOrder( - uint16_t{3666}, &format_id, &boundary_type_id, &name, - &this->warning_threshold, &this->bound, &this->frozen_value, - &this->time_limit, &this->frequency, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{3666}, + &format_id, + &boundary_type_id, + &name, + &this->warning_threshold, + &this->bound, + &this->frozen_value, + &this->time_limit, + &this->frequency, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } fault_message = new HeapOrder( - uint16_t{1666}, &format_id, &boundary_type_id, &name, &this->bound, - &this->frozen_value, &this->time_limit, &this->frequency, - &Global_RTC::global_RTC.counter, &Global_RTC::global_RTC.second, - &Global_RTC::global_RTC.minute, &Global_RTC::global_RTC.hour, - &Global_RTC::global_RTC.day, &Global_RTC::global_RTC.month, - &Global_RTC::global_RTC.year); + uint16_t{1666}, + &format_id, + &boundary_type_id, + &name, + &this->bound, + &this->frozen_value, + &this->time_limit, + &this->frequency, + &Global_RTC::global_RTC.counter, + &Global_RTC::global_RTC.second, + &Global_RTC::global_RTC.minute, + &Global_RTC::global_RTC.hour, + &Global_RTC::global_RTC.day, + &Global_RTC::global_RTC.month, + &Global_RTC::global_RTC.year + ); } Boundary(Type* src, Type bound, float time_limit, float frequency) - : real_still_good(new Protections::FaultType{Protections::OK}), - src(src), - bound(bound), - time_limit(time_limit), - frequency(frequency), - moving_order(frequency * time_limit / 100), + : real_still_good(new Protections::FaultType{Protections::OK}), src(src), bound(bound), + time_limit(time_limit), frequency(frequency), moving_order(frequency * time_limit / 100), external_pointer(nullptr) {} bool has_warning_level{false}; Type warning_threshold; @@ -571,7 +728,8 @@ struct Boundary : public BoundaryInterface { Type accumulator{}; Protections::FaultType check_accumulation(Type value) { - if (still_good == Protections::FAULT) return Protections::FAULT; + if (still_good == Protections::FAULT) + return Protections::FAULT; mean_calculator.input(abs(value)); mean_calculator.execute(); if (mean_calculator.output_value == 0) { diff --git a/Inc/ST-LIB_HIGH/Protections/Notification.hpp b/Inc/ST-LIB_HIGH/Protections/Notification.hpp index 164728220..996e5e6fe 100644 --- a/Inc/ST-LIB_HIGH/Protections/Notification.hpp +++ b/Inc/ST-LIB_HIGH/Protections/Notification.hpp @@ -5,97 +5,99 @@ #include "HALAL/Models/Packets/Order.hpp" #include "Protection.hpp" -class Notification : public Order{ +class Notification : public Order { private: - typedef uint16_t message_size_t; - uint16_t id; - void(*callback)() = nullptr; + typedef uint16_t message_size_t; + uint16_t id; + void (*callback)() = nullptr; string tx_message; message_size_t tx_message_size; string rx_message; uint8_t* buffer = nullptr; OrderProtocol* received_socket = nullptr; -public: - Notification(uint16_t packet_id,void(*callback)() ,string message) : id(packet_id), callback(callback),tx_message(message), tx_message_size(message.size()){ - Order::orders[id] = this; - Packet::packets[id] = this; - } - Notification(uint16_t packet_id,void(*callback)()) : id(packet_id), callback(callback){ - Order::orders[id] = this; - Packet::packets[id] = this; +public: + Notification(uint16_t packet_id, void (*callback)(), string message) + : id(packet_id), callback(callback), tx_message(message), tx_message_size(message.size()) { + Order::orders[id] = this; + Packet::packets[id] = this; } - void set_callback(void(*callback)()) override { - this->callback = callback; + Notification(uint16_t packet_id, void (*callback)()) : id(packet_id), callback(callback) { + Order::orders[id] = this; + Packet::packets[id] = this; } - void process() override{ - if(callback != nullptr) callback(); - string aux = tx_message; - tx_message = rx_message; - for(OrderProtocol* socket : OrderProtocol::sockets){ - if(socket == received_socket) continue; - socket->send_order(*this); - } - tx_message = aux; + void set_callback(void (*callback)()) override { this->callback = callback; } + + void process() override { + if (callback != nullptr) + callback(); + string aux = tx_message; + tx_message = rx_message; + for (OrderProtocol* socket : OrderProtocol::sockets) { + if (socket == received_socket) + continue; + socket->send_order(*this); + } + tx_message = aux; } uint8_t* build() { - if(buffer != nullptr) free(buffer); - buffer = (uint8_t*)malloc(get_size()); - - memcpy(buffer,&id,sizeof(id)); - memcpy(buffer + sizeof(id), &tx_message_size, sizeof(tx_message_size)); - memcpy(buffer+sizeof(id)+sizeof(tx_message_size),tx_message.c_str(), tx_message.size()); - return buffer; + if (buffer != nullptr) + free(buffer); + buffer = (uint8_t*)malloc(get_size()); + + memcpy(buffer, &id, sizeof(id)); + memcpy(buffer + sizeof(id), &tx_message_size, sizeof(tx_message_size)); + memcpy( + buffer + sizeof(id) + sizeof(tx_message_size), + tx_message.c_str(), + tx_message.size() + ); + return buffer; } - void notify(string message){ - tx_message = message; - tx_message_size = message.size(); - notify(); + void notify(string message) { + tx_message = message; + tx_message_size = message.size(); + notify(); } - void notify(){ - if(tx_message.empty()){ - ErrorHandler("Cannot notify empty notification"); - return; - } - for(OrderProtocol* socket : OrderProtocol::sockets){ - socket->send_order(*this); - } + void notify() { + if (tx_message.empty()) { + ErrorHandler("Cannot notify empty notification"); + return; + } + for (OrderProtocol* socket : OrderProtocol::sockets) { + socket->send_order(*this); + } } void parse(OrderProtocol* socket, uint8_t* data) { - received_socket = socket; - char* temp = (char*)malloc(get_string_size(data)); - memcpy(temp, data+sizeof(id)+sizeof(message_size_t), get_string_size(data)); - rx_message = string(temp); - free(temp); + received_socket = socket; + char* temp = (char*)malloc(get_string_size(data)); + memcpy(temp, data + sizeof(id) + sizeof(message_size_t), get_string_size(data)); + rx_message = string(temp); + free(temp); } size_t get_size() { - size = sizeof(id) + sizeof(tx_message_size) + tx_message_size; - return size; + size = sizeof(id) + sizeof(tx_message_size) + tx_message_size; + return size; } - uint16_t get_id() { - return id; - } + uint16_t get_id() { return id; } - void set_pointer(size_t index, void* pointer) override{ - ErrorHandler("Notification does not suport this method!"); + void set_pointer(size_t index, void* pointer) override { + ErrorHandler("Notification does not suport this method!"); } - ~Notification(){ - if(buffer != nullptr) free(buffer); + ~Notification() { + if (buffer != nullptr) + free(buffer); } - private: - - uint16_t get_string_size(uint8_t* buffer){ - return *(uint16_t*)(buffer + sizeof(id)); - } + uint16_t get_string_size(uint8_t* buffer) { return *(uint16_t*)(buffer + sizeof(id)); } }; diff --git a/Inc/ST-LIB_HIGH/Protections/Protection.hpp b/Inc/ST-LIB_HIGH/Protections/Protection.hpp index 119f2d931..089b3f4ff 100644 --- a/Inc/ST-LIB_HIGH/Protections/Protection.hpp +++ b/Inc/ST-LIB_HIGH/Protections/Protection.hpp @@ -4,9 +4,7 @@ #include "ErrorHandler/ErrorHandler.hpp" #include "Boundary.hpp" - - -class Protection{ +class Protection { private: char* name = nullptr; vector> boundaries; @@ -15,81 +13,81 @@ class Protection{ uint8_t triggered_protecions_idx[4]{}; uint8_t triggered_oks_idx[4]{}; uint64_t last_notify_tick{0}; + public: - const uint64_t get_last_notify_tick()const{ - return last_notify_tick; - } - void update_last_notify_tick(uint64_t new_tick){ - last_notify_tick = new_tick; - } + const uint64_t get_last_notify_tick() const { return last_notify_tick; } + void update_last_notify_tick(uint64_t new_tick) { last_notify_tick = new_tick; } vector> warnings_triggered; vector> oks_triggered; - template class Boundaries> - Protection(Type* src, Boundaries... protectors) { - (boundaries.push_back(shared_ptr(new Boundary(src,protectors))), ...); + template < + class Type, + ProtectionType... Protector, + template + class Boundaries> + Protection(Type* src, Boundaries... protectors) { + (boundaries.push_back( + shared_ptr(new Boundary(src, protectors)) + ), + ...); } - void set_name(char* name) { - this->name = name; - } + void set_name(char* name) { this->name = name; } - char* get_name() { - return name; - } + char* get_name() { return name; } Protections::FaultType check_state() { uint8_t warning_count = 0; uint8_t oks_count = 0; - //to save the index of the triggered warning + // to save the index of the triggered warning uint8_t idx = 0; - for(shared_ptr& bound: boundaries){ + for (shared_ptr& bound : boundaries) { auto fault_type = bound->check_bounds(); idx++; fault_protection = nullptr; - switch(fault_type){ - // in case a Protection has more than one boundary, give priority to fault messages - case Protections::FAULT: - fault_protection = bound.get(); - // adding the fault_protection to the vector is not desired, - // the fault signal should propagate as fast as possible - if(bound->warning_already_triggered){} - else{ - bound->warning_already_triggered = true; - } - return Protections::FAULT; - case Protections::WARNING: - //warnings are non fatal, but we cannot waste time, we need to check if any - //faults were triggered - if(bound->warning_already_triggered) - break; + switch (fault_type) { + // in case a Protection has more than one boundary, give priority to fault messages + case Protections::FAULT: + fault_protection = bound.get(); + // adding the fault_protection to the vector is not desired, + // the fault signal should propagate as fast as possible + if (bound->warning_already_triggered) { + } else { bound->warning_already_triggered = true; - triggered_protecions_idx[warning_count] = idx-1; - warning_count++; - break; - case Protections::OK: - if(bound->warning_already_triggered){ - bound->back_to_normal = true; - } - bound->warning_already_triggered = false; - if(bound->back_to_normal && bound->boundary_type_id != INFO_WARNING -2){ - triggered_oks_idx[oks_count] = idx-1; - oks_count++; - bound->back_to_normal = false; - } - - break; - default: - ErrorHandler("INVALID Protection::STATE type"); + } + return Protections::FAULT; + case Protections::WARNING: + // warnings are non fatal, but we cannot waste time, we need to check if any + // faults were triggered + if (bound->warning_already_triggered) break; + bound->warning_already_triggered = true; + triggered_protecions_idx[warning_count] = idx - 1; + warning_count++; + break; + case Protections::OK: + if (bound->warning_already_triggered) { + bound->back_to_normal = true; + } + bound->warning_already_triggered = false; + if (bound->back_to_normal && bound->boundary_type_id != INFO_WARNING - 2) { + triggered_oks_idx[oks_count] = idx - 1; + oks_count++; + bound->back_to_normal = false; + } + + break; + default: + ErrorHandler("INVALID Protection::STATE type"); + break; } } - if(oks_count){ - for(uint8_t i = 0; i < oks_count; i++){ + if (oks_count) { + for (uint8_t i = 0; i < oks_count; i++) { oks_triggered.push_back(boundaries[triggered_oks_idx[i]]); } } - if(warning_count){ - for(uint8_t i = 0; i < warning_count; i++){ + if (warning_count) { + for (uint8_t i = 0; i < warning_count; i++) { warnings_triggered.push_back(boundaries[triggered_protecions_idx[i]]); } return Protections::WARNING; diff --git a/Inc/ST-LIB_HIGH/Protections/ProtectionManager.hpp b/Inc/ST-LIB_HIGH/Protections/ProtectionManager.hpp index 62564998e..5a69cf235 100644 --- a/Inc/ST-LIB_HIGH/Protections/ProtectionManager.hpp +++ b/Inc/ST-LIB_HIGH/Protections/ProtectionManager.hpp @@ -10,34 +10,32 @@ #include "StateMachine/StateMachine.hpp" #define getname(var) #var -#define add_protection(src, ...) \ - { \ - Protection& ref = \ - ProtectionManager::_add_protection(src, __VA_ARGS__); \ - if (getname(src)[0] == '&') { \ - ref.set_name((char*)malloc(sizeof(getname(src)) - 1)); \ - sprintf(ref.get_name(), "%s", getname(src) + 1); \ - } else { \ - ref.set_name((char*)malloc(sizeof(getname(src)))); \ - sprintf(ref.get_name(), "%s", getname(src)); \ - } \ +#define add_protection(src, ...) \ + { \ + Protection& ref = ProtectionManager::_add_protection(src, __VA_ARGS__); \ + if (getname(src)[0] == '&') { \ + ref.set_name((char*)malloc(sizeof(getname(src)) - 1)); \ + sprintf(ref.get_name(), "%s", getname(src) + 1); \ + } else { \ + ref.set_name((char*)malloc(sizeof(getname(src)))); \ + sprintf(ref.get_name(), "%s", getname(src)); \ + } \ } -#define add_high_frequency_protection(src, ...) \ - { \ - Protection& ref = ProtectionManager::_add_high_frequency_protection( \ - src, __VA_ARGS__); \ - if (getname(src)[0] == '&') { \ - ref.set_name((char*)malloc(sizeof(getname(src)) - 1)); \ - sprintf(ref.get_name(), "%s", getname(src) + 1); \ - } else { \ - ref.set_name((char*)malloc(sizeof(getname(src)))); \ - sprintf(ref.get_name(), "%s", getname(src)); \ - } \ +#define add_high_frequency_protection(src, ...) \ + { \ + Protection& ref = ProtectionManager::_add_high_frequency_protection(src, __VA_ARGS__); \ + if (getname(src)[0] == '&') { \ + ref.set_name((char*)malloc(sizeof(getname(src)) - 1)); \ + sprintf(ref.get_name(), "%s", getname(src) + 1); \ + } else { \ + ref.set_name((char*)malloc(sizeof(getname(src)))); \ + sprintf(ref.get_name(), "%s", getname(src)); \ + } \ } class ProtectionManager { - public: +public: typedef uint8_t state_id; static bool external_trigger; @@ -46,21 +44,25 @@ class ProtectionManager { static void set_id(Boards::ID id); - static void link_state_machine(IStateMachine& general_state_machine, - state_id fault_id); + static void link_state_machine(IStateMachine& general_state_machine, state_id fault_id); - template class Boundaries> - static Protection& _add_protection( - Type* src, Boundaries... protectors) { + template < + class Type, + ProtectionType... Protector, + template + class Boundaries> + static Protection& _add_protection(Type* src, Boundaries... protectors) { low_frequency_protections.push_back(Protection(src, protectors...)); return low_frequency_protections.back(); } - template class Boundaries> - static Protection& _add_high_frequency_protection( - Type* src, Boundaries... protectors) { + template < + class Type, + ProtectionType... Protector, + template + class Boundaries> + static Protection& + _add_high_frequency_protection(Type* src, Boundaries... protectors) { high_frequency_protections.push_back(Protection(src, protectors...)); return high_frequency_protections.back(); } @@ -76,14 +78,13 @@ class ProtectionManager { static void propagate_fault(); static void notify(Protection& protection); - private: +private: static constexpr uint16_t warning_id = 2; static constexpr uint16_t fault_id = 3; static char* message; static size_t message_size; static bool test_fault; - static constexpr const char* format = - "{\"boardId\": %s, \"timestamp\":{%s}, %s}"; + static constexpr const char* format = "{\"boardId\": %s, \"timestamp\":{%s}, %s}"; static Boards::ID board_id; static vector low_frequency_protections; diff --git a/Inc/ST-LIB_HIGH/ST-LIB_HIGH.hpp b/Inc/ST-LIB_HIGH/ST-LIB_HIGH.hpp index 392bbee9c..6e1d499d9 100644 --- a/Inc/ST-LIB_HIGH/ST-LIB_HIGH.hpp +++ b/Inc/ST-LIB_HIGH/ST-LIB_HIGH.hpp @@ -24,8 +24,8 @@ #include "Control/ControlSystem.hpp" #ifdef SIM_ON #else - #include "FlashStorer/FlashStorer.hpp" +#include "FlashStorer/FlashStorer.hpp" #endif namespace STLIB_HIGH { - void start(); +void start(); } diff --git a/Inc/ST-LIB_LOW/Clocks/Counter.hpp b/Inc/ST-LIB_LOW/Clocks/Counter.hpp index 34e60e0bb..1b6c43632 100644 --- a/Inc/ST-LIB_LOW/Clocks/Counter.hpp +++ b/Inc/ST-LIB_LOW/Clocks/Counter.hpp @@ -8,19 +8,18 @@ #pragma once #include "C++Utilities/CppUtils.hpp" -class Counter{ +class Counter { private: - uint16_t update_period_ms = 125; - uint8_t time_id; - void update(); + uint16_t update_period_ms = 125; + uint8_t time_id; + void update(); public: - uint64_t counter = 0; - double freq = 0.0; - - ~Counter(); - Counter(uint64_t update_period_ms); - void count(); + uint64_t counter = 0; + double freq = 0.0; + ~Counter(); + Counter(uint64_t update_period_ms); + void count(); }; diff --git a/Inc/ST-LIB_LOW/Clocks/Stopwatch.hpp b/Inc/ST-LIB_LOW/Clocks/Stopwatch.hpp index 10768415e..18ae527aa 100644 --- a/Inc/ST-LIB_LOW/Clocks/Stopwatch.hpp +++ b/Inc/ST-LIB_LOW/Clocks/Stopwatch.hpp @@ -9,13 +9,12 @@ #include "C++Utilities/CppUtils.hpp" #include "ErrorHandler/ErrorHandler.hpp" -class Stopwatch{ +class Stopwatch { private: - map start_times; + map start_times; public: - void start(const string); - uint64_t stop(const string); + void start(const string); + uint64_t stop(const string); }; - diff --git a/Inc/ST-LIB_LOW/Communication/Server/Server.hpp b/Inc/ST-LIB_LOW/Communication/Server/Server.hpp index dc78ab022..e1cf1e7a9 100644 --- a/Inc/ST-LIB_LOW/Communication/Server/Server.hpp +++ b/Inc/ST-LIB_LOW/Communication/Server/Server.hpp @@ -17,35 +17,30 @@ #include "HALAL/Services/Communication/Ethernet/LWIP/TCP/ServerSocket.hpp" #ifndef MAX_CONNECTIONS_TCP_SERVER - #define MAX_CONNECTIONS_TCP_SERVER 10 +#define MAX_CONNECTIONS_TCP_SERVER 10 #endif -class Server{ +class Server { public: + enum ServerState { RUNNING, CLOSING, CLOSED }; - enum ServerState{ - RUNNING, - CLOSING, - CLOSED - }; - - ServerSocket *open_connection; - array running_connections; - uint16_t running_connections_count; - IPV4 local_ip; - uint32_t local_port; - ServerState status; - - static vector running_servers; - - Server(IPV4 local_ip, uint32_t local_port); - ~Server(); - void update(); - void broadcast_order(Order& order); - void close_all(); - uint32_t connections_count(); - - static void update_servers(); + ServerSocket* open_connection; + array running_connections; + uint16_t running_connections_count; + IPV4 local_ip; + uint32_t local_port; + ServerState status; + + static vector running_servers; + + Server(IPV4 local_ip, uint32_t local_port); + ~Server(); + void update(); + void broadcast_order(Order& order); + void close_all(); + uint32_t connections_count(); + + static void update_servers(); }; -#endif //STLIB_ETH +#endif // STLIB_ETH diff --git a/Inc/ST-LIB_LOW/DigitalInput2.hpp b/Inc/ST-LIB_LOW/DigitalInput2.hpp index 5b46205fd..aff1c749f 100644 --- a/Inc/ST-LIB_LOW/DigitalInput2.hpp +++ b/Inc/ST-LIB_LOW/DigitalInput2.hpp @@ -6,58 +6,59 @@ using ST_LIB::GPIODomain; namespace ST_LIB { struct DigitalInputDomain { - struct Entry { - size_t gpio_idx; - }; - struct DigitalInput { - GPIODomain::GPIO gpio; - using domain = DigitalInputDomain; - - consteval DigitalInput(const GPIODomain::Pin &pin, - GPIODomain::Pull pull = GPIODomain::Pull::None, - GPIODomain::Speed speed = GPIODomain::Speed::Low) - : gpio{pin, GPIODomain::OperationMode::INPUT, pull, speed} {} - - template consteval std::size_t inscribe(Ctx &ctx) const { - const auto gpio_idx = gpio.inscribe(ctx); - Entry e{.gpio_idx = gpio_idx}; - return ctx.template add(e, this); - } - }; + struct Entry { + size_t gpio_idx; + }; + struct DigitalInput { + GPIODomain::GPIO gpio; + using domain = DigitalInputDomain; + + consteval DigitalInput( + const GPIODomain::Pin& pin, + GPIODomain::Pull pull = GPIODomain::Pull::None, + GPIODomain::Speed speed = GPIODomain::Speed::Low + ) + : gpio{pin, GPIODomain::OperationMode::INPUT, pull, speed} {} - static constexpr std::size_t max_instances{110}; + template consteval std::size_t inscribe(Ctx& ctx) const { + const auto gpio_idx = gpio.inscribe(ctx); + Entry e{.gpio_idx = gpio_idx}; + return ctx.template add(e, this); + } + }; - struct Config { - size_t gpio_idx; - }; + static constexpr std::size_t max_instances{110}; - template - static consteval array build(span outputs) { - array cfgs{}; + struct Config { + size_t gpio_idx; + }; - for (std::size_t i = 0; i < N; ++i) { - cfgs[i].gpio_idx = outputs[i].gpio_idx; + template static consteval array build(span outputs) { + array cfgs{}; + + for (std::size_t i = 0; i < N; ++i) { + cfgs[i].gpio_idx = outputs[i].gpio_idx; + } + return cfgs; } - return cfgs; - } - struct Instance { - GPIODomain::Instance *gpio_instance; + struct Instance { + GPIODomain::Instance* gpio_instance; - GPIO_PinState read() { return gpio_instance->read(); } - }; + GPIO_PinState read() { return gpio_instance->read(); } + }; - template struct Init { - static inline std::array instances{}; + template struct Init { + static inline std::array instances{}; - static void init(std::span cfgs, - std::span gpio_instances) { - for (std::size_t i = 0; i < N; ++i) { - const auto &e = cfgs[i]; + static void + init(std::span cfgs, std::span gpio_instances) { + for (std::size_t i = 0; i < N; ++i) { + const auto& e = cfgs[i]; - instances[i].gpio_instance = &gpio_instances[e.gpio_idx]; - } - } - }; + instances[i].gpio_instance = &gpio_instances[e.gpio_idx]; + } + } + }; }; } // namespace ST_LIB diff --git a/Inc/ST-LIB_LOW/DigitalOutput/DigitalOutput.hpp b/Inc/ST-LIB_LOW/DigitalOutput/DigitalOutput.hpp index 8566d5cfb..0bef59c17 100644 --- a/Inc/ST-LIB_LOW/DigitalOutput/DigitalOutput.hpp +++ b/Inc/ST-LIB_LOW/DigitalOutput/DigitalOutput.hpp @@ -12,7 +12,7 @@ #include "HALAL/Models/PinModel/Pin.hpp" class DigitalOutput { - public: +public: DigitalOutput() = default; DigitalOutput(Pin& pin); @@ -22,7 +22,7 @@ class DigitalOutput { void set_pin_state(PinState state); bool lock_pin_state(PinState state); - private: +private: Pin pin; uint8_t id; }; diff --git a/Inc/ST-LIB_LOW/DigitalOutput2.hpp b/Inc/ST-LIB_LOW/DigitalOutput2.hpp index 0c7602120..cbc06ddbb 100644 --- a/Inc/ST-LIB_LOW/DigitalOutput2.hpp +++ b/Inc/ST-LIB_LOW/DigitalOutput2.hpp @@ -6,70 +6,68 @@ using ST_LIB::GPIODomain; namespace ST_LIB { struct DigitalOutputDomain { - enum class OutputMode : uint8_t { - PUSH_PULL = - static_cast(GPIODomain::OperationMode::OUTPUT_PUSHPULL), - OPEN_DRAIN = - static_cast(GPIODomain::OperationMode::OUTPUT_OPENDRAIN), - }; - struct Entry { - size_t gpio_idx; - }; - struct DigitalOutput { - GPIODomain::GPIO gpio; - using domain = DigitalOutputDomain; - - consteval DigitalOutput(const GPIODomain::Pin &pin, - OutputMode mode = OutputMode::PUSH_PULL, - GPIODomain::Pull pull = GPIODomain::Pull::None, - GPIODomain::Speed speed = GPIODomain::Speed::Low) - : gpio{pin, static_cast(mode), pull, speed} { + enum class OutputMode : uint8_t { + PUSH_PULL = static_cast(GPIODomain::OperationMode::OUTPUT_PUSHPULL), + OPEN_DRAIN = static_cast(GPIODomain::OperationMode::OUTPUT_OPENDRAIN), + }; + struct Entry { + size_t gpio_idx; + }; + struct DigitalOutput { + GPIODomain::GPIO gpio; + using domain = DigitalOutputDomain; + + consteval DigitalOutput( + const GPIODomain::Pin& pin, + OutputMode mode = OutputMode::PUSH_PULL, + GPIODomain::Pull pull = GPIODomain::Pull::None, + GPIODomain::Speed speed = GPIODomain::Speed::Low + ) + : gpio{pin, static_cast(mode), pull, speed} {} + + template consteval std::size_t inscribe(Ctx& ctx) const { + const auto gpio_idx = gpio.inscribe(ctx); + Entry e{.gpio_idx = gpio_idx}; + return ctx.template add(e, this); + } + }; + + static constexpr std::size_t max_instances{110}; + + struct Config { + size_t gpio_idx; + }; + + template static consteval array build(span outputs) { + array cfgs{}; + + for (std::size_t i = 0; i < N; ++i) { + cfgs[i].gpio_idx = outputs[i].gpio_idx; + } + return cfgs; } - template consteval std::size_t inscribe(Ctx &ctx) const { - const auto gpio_idx = gpio.inscribe(ctx); - Entry e{.gpio_idx = gpio_idx}; - return ctx.template add(e, this); - } - }; - - static constexpr std::size_t max_instances{110}; - - struct Config { - size_t gpio_idx; - }; - - template - static consteval array build(span outputs) { - array cfgs{}; + struct Instance { + GPIODomain::Instance* gpio_instance; - for (std::size_t i = 0; i < N; ++i) { - cfgs[i].gpio_idx = outputs[i].gpio_idx; - } - return cfgs; - } - - struct Instance { - GPIODomain::Instance *gpio_instance; + void turn_on() { gpio_instance->turn_on(); } - void turn_on() { gpio_instance->turn_on(); } + void turn_off() { gpio_instance->turn_off(); } - void turn_off() { gpio_instance->turn_off(); } + void toggle() { gpio_instance->toggle(); } + }; - void toggle() { gpio_instance->toggle(); } - }; + template struct Init { + static inline std::array instances{}; - template struct Init { - static inline std::array instances{}; + static void + init(std::span cfgs, std::span gpio_instances) { + for (std::size_t i = 0; i < N; ++i) { + const auto& e = cfgs[i]; - static void init(std::span cfgs, - std::span gpio_instances) { - for (std::size_t i = 0; i < N; ++i) { - const auto &e = cfgs[i]; - - instances[i].gpio_instance = &gpio_instances[e.gpio_idx]; - } - } - }; + instances[i].gpio_instance = &gpio_instances[e.gpio_idx]; + } + } + }; }; } // namespace ST_LIB diff --git a/Inc/ST-LIB_LOW/ErrorHandler/ErrorHandler.hpp b/Inc/ST-LIB_LOW/ErrorHandler/ErrorHandler.hpp index 7332c21b6..09ffefca4 100644 --- a/Inc/ST-LIB_LOW/ErrorHandler/ErrorHandler.hpp +++ b/Inc/ST-LIB_LOW/ErrorHandler/ErrorHandler.hpp @@ -16,45 +16,51 @@ class ErrorHandlerModel { private: - static string description; - static string line; - static string func; - static string file; + static string description; + static string line; + static string func; + static string file; public: - static double error_triggered; - static bool error_to_communicate; - - /** - * @brief Triggers ErrorHandler and format the error message. The format works - * exactly like printf format. - * - * @param format String which will be formated. - * @param args Arguments specifying data to print - * @return uint8_t Id of the service. - */ - static void ErrorHandlerTrigger(string format, ...); - - /** - * @brief Get all metadata needed for the error message, including the line function and file. - * The default parameters are not necessary but are there in case the compiler macros stop - * working because a change of the compiler. - * - * @param line Line where the error occurred - * @param func Function where the error occurred - * @param file File where the file occurred - * @return uint8_t Id of the service. - */ - static void SetMetaData( int line = __builtin_LINE(), const char * func = __builtin_FUNCTION(), const char * file = __builtin_FILE()); - - /** - * @brief Transmit the error message. - */ - static void ErrorHandlerUpdate(); - - friend class BoundaryInterface; + static double error_triggered; + static bool error_to_communicate; + /** + * @brief Triggers ErrorHandler and format the error message. The format works + * exactly like printf format. + * + * @param format String which will be formated. + * @param args Arguments specifying data to print + * @return uint8_t Id of the service. + */ + static void ErrorHandlerTrigger(string format, ...); + + /** + * @brief Get all metadata needed for the error message, including the line function and file. + * The default parameters are not necessary but are there in case the compiler macros + * stop working because a change of the compiler. + * + * @param line Line where the error occurred + * @param func Function where the error occurred + * @param file File where the file occurred + * @return uint8_t Id of the service. + */ + static void SetMetaData( + int line = __builtin_LINE(), + const char* func = __builtin_FUNCTION(), + const char* file = __builtin_FILE() + ); + + /** + * @brief Transmit the error message. + */ + static void ErrorHandlerUpdate(); + + friend class BoundaryInterface; }; -#define ErrorHandler(x, ...) do { ErrorHandlerModel::SetMetaData(__LINE__, __FUNCTION__, __FILE__); \ - ErrorHandlerModel::ErrorHandlerTrigger(x, ##__VA_ARGS__);}while(0) +#define ErrorHandler(x, ...) \ + do { \ + ErrorHandlerModel::SetMetaData(__LINE__, __FUNCTION__, __FILE__); \ + ErrorHandlerModel::ErrorHandlerTrigger(x, ##__VA_ARGS__); \ + } while (0) diff --git a/Inc/ST-LIB_LOW/HalfBridge/HalfBridge.hpp b/Inc/ST-LIB_LOW/HalfBridge/HalfBridge.hpp index 3ddd6dfac..37f34d75b 100644 --- a/Inc/ST-LIB_LOW/HalfBridge/HalfBridge.hpp +++ b/Inc/ST-LIB_LOW/HalfBridge/HalfBridge.hpp @@ -15,24 +15,28 @@ class HalfBridge { public: - HalfBridge() = default; - HalfBridge(Pin& positive_pwm_pin, Pin& positive_pwm_negated_pin, Pin& negative_pwm_pin, Pin& negative_pwm_negated_pin, - Pin& enable_pin); - - void turn_on(); - void turn_off(); - void set_duty_cycle(float duty_cycle); - void set_frequency(int32_t frequency); - void set_phase(float phase); - void set_positive_pwm_phase(float phase); - void set_negative_pwm_phase(float phase); - float get_phase(); - + HalfBridge() = default; + HalfBridge( + Pin& positive_pwm_pin, + Pin& positive_pwm_negated_pin, + Pin& negative_pwm_pin, + Pin& negative_pwm_negated_pin, + Pin& enable_pin + ); + + void turn_on(); + void turn_off(); + void set_duty_cycle(float duty_cycle); + void set_frequency(int32_t frequency); + void set_phase(float phase); + void set_positive_pwm_phase(float phase); + void set_negative_pwm_phase(float phase); + float get_phase(); private: - bool is_dual; - DualPhasedPWM positive_pwm; - DualPhasedPWM negative_pwm; + bool is_dual; + DualPhasedPWM positive_pwm; + DualPhasedPWM negative_pwm; - uint8_t enable; + uint8_t enable; }; diff --git a/Inc/ST-LIB_LOW/Math/Math.hpp b/Inc/ST-LIB_LOW/Math/Math.hpp index a895d0886..be106931d 100644 --- a/Inc/ST-LIB_LOW/Math/Math.hpp +++ b/Inc/ST-LIB_LOW/Math/Math.hpp @@ -11,13 +11,12 @@ #define TG_DECIMAL_BITS 16 #define SQ_DECIMAL_BITS 16 #define MAX_INT 2147483647 -#define MIN_COS_MARGIN_TG (MAX_INT >> (TG_DECIMAL_BITS-1)) +#define MIN_COS_MARGIN_TG (MAX_INT >> (TG_DECIMAL_BITS - 1)) #define MAX_MOD_MARGIN 2147483392 #define MAX_MARGIN 2147483392 #define Q31_MASK 0x7FFFFFFF - -class Math{ +class Math { public: static int32_t sin(int32_t angle); static int32_t cos(int32_t angle); @@ -31,6 +30,7 @@ class Math{ static inline int32_t unitary_to_sq(int32_t in); static inline int32_t tg_to_unitary(int32_t tg_in); static inline int32_t unitary_to_tg(int32_t in); + private: - static std::array pointers; + static std::array pointers; }; diff --git a/Inc/ST-LIB_LOW/ST-LIB_LOW.hpp b/Inc/ST-LIB_LOW/ST-LIB_LOW.hpp index 7fc564eb9..148b19318 100644 --- a/Inc/ST-LIB_LOW/ST-LIB_LOW.hpp +++ b/Inc/ST-LIB_LOW/ST-LIB_LOW.hpp @@ -19,7 +19,7 @@ #include "Sensors/SensorInterrupt/SensorInterrupt.hpp" #include "Sensors/LinearSensor/LinearSensor.hpp" #include "Sensors/LookupSensor/LookupSensor.hpp" -//#include "Sensors/EncoderSensor/EncoderSensor.hpp" +// #include "Sensors/EncoderSensor/EncoderSensor.hpp" #include "Sensors/EncoderSensor/NewEncoderSensor.hpp" #include "Sensors/PWMSensor/PWMSensor.hpp" #include "Sensors/NTC/NTC.hpp" @@ -32,5 +32,5 @@ class STLIB_LOW { public: - static void start(); + static void start(); }; diff --git a/Inc/ST-LIB_LOW/Sd/Sd.hpp b/Inc/ST-LIB_LOW/Sd/Sd.hpp index 08f834bb5..b2eb5f00d 100644 --- a/Inc/ST-LIB_LOW/Sd/Sd.hpp +++ b/Inc/ST-LIB_LOW/Sd/Sd.hpp @@ -25,10 +25,10 @@ using ST_LIB::DigitalInputDomain; using ST_LIB::GPIODomain; -extern SD_HandleTypeDef *g_sdmmc1_handle; -extern SD_HandleTypeDef *g_sdmmc2_handle; -extern void *g_sdmmc1_instance_ptr; -extern void *g_sdmmc2_instance_ptr; +extern SD_HandleTypeDef* g_sdmmc1_handle; +extern SD_HandleTypeDef* g_sdmmc2_handle; +extern void* g_sdmmc1_instance_ptr; +extern void* g_sdmmc2_instance_ptr; namespace ST_LIB { struct SdDomain { @@ -38,13 +38,14 @@ struct SdDomain { sdmmc2 = SDMMC2_BASE, }; - struct Entry { Peripheral peripheral; std::size_t mpu_buffer0_idx; std::size_t mpu_buffer1_idx; - std::optional> cd_pin_idx; // Card Detect pin index in GPIO domain, if any - std::optional> wp_pin_idx; // Write Protect pin index in GPIO domain, if any + std::optional> + cd_pin_idx; // Card Detect pin index in GPIO domain, if any + std::optional> + wp_pin_idx; // Write Protect pin index in GPIO domain, if any std::size_t cmd_pin_idx; std::size_t ck_pin_idx; std::size_t d0_pin_idx; // Fixed for SDMMC2, configurable for SDMMC1 @@ -53,19 +54,21 @@ struct SdDomain { std::size_t d3_pin_idx; }; - - template - struct SdCard { + template struct SdCard { using domain = SdDomain; Entry e; Peripheral peripheral; - MPUDomain::Buffer> buffer0; // Alignment of 32-bit for SDMMC DMA - MPUDomain::Buffer> buffer1; // Alignment of 32-bit for SDMMC DMA + MPUDomain::Buffer> + buffer0; // Alignment of 32-bit for SDMMC DMA + MPUDomain::Buffer> + buffer1; // Alignment of 32-bit for SDMMC DMA - std::optional> cd; // Card Detect, if any, and its active state - std::optional> wp; // Write Protect, if any, and its active state + std::optional> + cd; // Card Detect, if any, and its active state + std::optional> + wp; // Write Protect, if any, and its active state GPIODomain::GPIO cmd; GPIODomain::GPIO ck; @@ -77,46 +80,124 @@ struct SdDomain { /** * @brief Construct a new SdCard * @tparam buffer_blocks Number of 512-byte blocks for the MPU buffer - * @param sdmmc_peripheral The SDMMC peripheral to use (Peripheral::sdmmc1 or Peripheral::sdmmc2) - * @param card_detect_config Optional Card Detect pin (DigitalInputDomain::DigitalInput) and its active state, or nullopt for none - * @param write_protect_config Optional Write Protect pin (DigitalInputDomain::DigitalInput) and its active state, or nullopt for none + * @param sdmmc_peripheral The SDMMC peripheral to use (Peripheral::sdmmc1 or + * Peripheral::sdmmc2) + * @param card_detect_config Optional Card Detect pin (DigitalInputDomain::DigitalInput) and + * its active state, or nullopt for none + * @param write_protect_config Optional Write Protect pin (DigitalInputDomain::DigitalInput) + * and its active state, or nullopt for none * @param d0_pin_for_sdmmc1 D0 pin to use if using SDMMC1 (default PC8, can also be PB13). * @note The other pins (CMD, CK, D1, D2, D3) are fixed for each peripheral. */ - consteval SdCard(Peripheral sdmmc_peripheral, - std::optional> card_detect_config, std::optional> write_protect_config, - GPIODomain::Pin d0_pin_for_sdmmc1 = ST_LIB::PC8) : - e{.peripheral = sdmmc_peripheral}, - peripheral(sdmmc_peripheral), - buffer0(MPUDomain::Buffer>(MPUDomain::MemoryType::NonCached, MPUDomain::MemoryDomain::D1)), - buffer1(MPUDomain::Buffer>(MPUDomain::MemoryType::NonCached, MPUDomain::MemoryDomain::D1)), - cd(card_detect_config), - wp(write_protect_config), - cmd((sdmmc_peripheral == Peripheral::sdmmc1) ? - GPIODomain::GPIO(ST_LIB::PD2, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, GPIODomain::AlternateFunction::AF12) : - GPIODomain::GPIO(ST_LIB::PD7, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, GPIODomain::AlternateFunction::AF11)), - ck((sdmmc_peripheral == Peripheral::sdmmc1) ? - GPIODomain::GPIO(ST_LIB::PC12, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, GPIODomain::AlternateFunction::AF12) : - GPIODomain::GPIO(ST_LIB::PD6, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, GPIODomain::AlternateFunction::AF11)), - d0((sdmmc_peripheral == Peripheral::sdmmc1) ? - GPIODomain::GPIO(d0_pin_for_sdmmc1, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, GPIODomain::AlternateFunction::AF12) : - GPIODomain::GPIO(ST_LIB::PB14, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, GPIODomain::AlternateFunction::AF9)), - d1((sdmmc_peripheral == Peripheral::sdmmc1) ? - GPIODomain::GPIO(ST_LIB::PC9, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, GPIODomain::AlternateFunction::AF12) : - GPIODomain::GPIO(ST_LIB::PB15, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, GPIODomain::AlternateFunction::AF9)), - d2((sdmmc_peripheral == Peripheral::sdmmc1) ? - GPIODomain::GPIO(ST_LIB::PC10, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, GPIODomain::AlternateFunction::AF12) : - GPIODomain::GPIO(ST_LIB::PG11, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, GPIODomain::AlternateFunction::AF10)), - d3((sdmmc_peripheral == Peripheral::sdmmc1) ? - GPIODomain::GPIO(ST_LIB::PC11, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, GPIODomain::AlternateFunction::AF12) : - GPIODomain::GPIO(ST_LIB::PG12, GPIODomain::OperationMode::ALT_PP, GPIODomain::Pull::None, GPIODomain::Speed::VeryHigh, GPIODomain::AlternateFunction::AF10)) - { + consteval SdCard( + Peripheral sdmmc_peripheral, + std::optional> + card_detect_config, + std::optional> + write_protect_config, + GPIODomain::Pin d0_pin_for_sdmmc1 = ST_LIB::PC8 + ) + : e{.peripheral = sdmmc_peripheral}, peripheral(sdmmc_peripheral), + buffer0(MPUDomain::Buffer>( + MPUDomain::MemoryType::NonCached, + MPUDomain::MemoryDomain::D1 + )), + buffer1(MPUDomain::Buffer>( + MPUDomain::MemoryType::NonCached, + MPUDomain::MemoryDomain::D1 + )), + cd(card_detect_config), wp(write_protect_config), + cmd((sdmmc_peripheral == Peripheral::sdmmc1) ? GPIODomain::GPIO( + ST_LIB::PD2, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF12 + ) + : GPIODomain::GPIO( + ST_LIB::PD7, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF11 + )), + ck((sdmmc_peripheral == Peripheral::sdmmc1) ? GPIODomain::GPIO( + ST_LIB::PC12, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF12 + ) + : GPIODomain::GPIO( + ST_LIB::PD6, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF11 + )), + d0((sdmmc_peripheral == Peripheral::sdmmc1) ? GPIODomain::GPIO( + d0_pin_for_sdmmc1, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF12 + ) + : GPIODomain::GPIO( + ST_LIB::PB14, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF9 + )), + d1((sdmmc_peripheral == Peripheral::sdmmc1) ? GPIODomain::GPIO( + ST_LIB::PC9, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF12 + ) + : GPIODomain::GPIO( + ST_LIB::PB15, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF9 + )), + d2((sdmmc_peripheral == Peripheral::sdmmc1) ? GPIODomain::GPIO( + ST_LIB::PC10, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF12 + ) + : GPIODomain::GPIO( + ST_LIB::PG11, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF10 + )), + d3((sdmmc_peripheral == Peripheral::sdmmc1) ? GPIODomain::GPIO( + ST_LIB::PC11, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF12 + ) + : GPIODomain::GPIO( + ST_LIB::PG12, + GPIODomain::OperationMode::ALT_PP, + GPIODomain::Pull::None, + GPIODomain::Speed::VeryHigh, + GPIODomain::AlternateFunction::AF10 + )) { if (sdmmc_peripheral != Peripheral::sdmmc1 && sdmmc_peripheral != Peripheral::sdmmc2) { ST_LIB::compile_error("Invalid SDMMC peripheral"); } - if ((d0_pin_for_sdmmc1.pin != ST_LIB::PC8.pin || d0_pin_for_sdmmc1.port != ST_LIB::PC8.port) - && - (d0_pin_for_sdmmc1.pin != ST_LIB::PB13.pin || d0_pin_for_sdmmc1.port != ST_LIB::PB13.port)) { + if ((d0_pin_for_sdmmc1.pin != ST_LIB::PC8.pin || + d0_pin_for_sdmmc1.port != ST_LIB::PC8.port) && + (d0_pin_for_sdmmc1.pin != ST_LIB::PB13.pin || + d0_pin_for_sdmmc1.port != ST_LIB::PB13.port)) { ST_LIB::compile_error("D0 pin can only be PC8 or PB13 for SDMMC1"); } if (buffer_blocks == 0) { @@ -125,10 +206,8 @@ struct SdDomain { ST_LIB::compile_error("Buffer blocks must be less than or equal to 15"); } } - - template - consteval std::size_t inscribe(Ctx &ctx) const { + template consteval std::size_t inscribe(Ctx& ctx) const { Entry local_e = e; local_e.mpu_buffer0_idx = buffer0.inscribe(ctx); @@ -152,10 +231,8 @@ struct SdDomain { } }; - static constexpr std::size_t max_instances = 2; - struct Config { Peripheral peripheral; std::size_t mpu_buffer0_idx; @@ -170,21 +247,21 @@ struct SdDomain { std::size_t d3_pin_idx; }; - template static consteval std::array build(std::span entries) { std::array cfgs{}; - if (N == 0 ) { + if (N == 0) { return cfgs; } bool peripheral_used[2] = {false, false}; // SDMMC1, SDMMC2 for (std::size_t i = 0; i < N; i++) { - const Entry &e = entries[i]; + const Entry& e = entries[i]; // Verify uniqueness of peripheral usage std::size_t peripheral_index = (e.peripheral == Peripheral::sdmmc1) ? 0 : 1; - if (peripheral_used[peripheral_index]) ST_LIB::compile_error("SDMMC peripheral used multiple times in SdDomain"); + if (peripheral_used[peripheral_index]) + ST_LIB::compile_error("SDMMC peripheral used multiple times in SdDomain"); peripheral_used[peripheral_index] = true; cfgs[i].peripheral = e.peripheral; @@ -203,38 +280,39 @@ struct SdDomain { return cfgs; } - enum class BufferSelect : bool { - Buffer0 = false, - Buffer1 = true - }; + enum class BufferSelect : bool { Buffer0 = false, Buffer1 = true }; template struct SdCardWrapper; template struct Init; // State holder, logic is in SdCardWrapper struct Instance { - template friend struct SdCardWrapper; + template friend struct SdCardWrapper; template friend struct Init; - bool *operation_flag = nullptr; // External flag to indicate that an operation has finished. Only public so that it can be set in the public handlers below. + bool* operation_flag = + nullptr; // External flag to indicate that an operation has finished. Only public so + // that it can be set in the public handlers below. - // Public handlers called from C HAL callbacks. Don't use them, don't even think about using them. - void on_dma_read_complete(); - void on_dma_write_complete(); - void on_abort(); - void on_error(); + // Public handlers called from C HAL callbacks. Don't use them, don't even think about using + // them. + void on_dma_read_complete(); + void on_dma_write_complete(); + void on_abort(); + void on_error(); - private: + private: SD_HandleTypeDef hsd; - MPUDomain::Instance *mpu_buffer0_instance; - MPUDomain::Instance *mpu_buffer1_instance; + MPUDomain::Instance* mpu_buffer0_instance; + MPUDomain::Instance* mpu_buffer1_instance; std::optional> cd_instance; std::optional> wp_instance; bool card_initialized; - BufferSelect current_buffer; // The one that is currently available for CPU access and not being used by IDMA + BufferSelect current_buffer; // The one that is currently available for CPU access and not + // being used by IDMA HAL_SD_CardInfoTypeDef card_info; @@ -247,18 +325,17 @@ struct SdDomain { void switch_buffer(); bool configure_idma(); // Variation of HAL_SDEx_ReadBlocksDMAMultiBuffer to fit our needs - HAL_StatusTypeDef Not_HAL_SDEx_ReadBlocksDMAMultiBuffer(uint32_t BlockAdd, uint32_t NumberOfBlocks); - HAL_StatusTypeDef Not_HAL_SDEx_WriteBlocksDMAMultiBuffer(uint32_t BlockAdd, uint32_t NumberOfBlocks); + HAL_StatusTypeDef + Not_HAL_SDEx_ReadBlocksDMAMultiBuffer(uint32_t BlockAdd, uint32_t NumberOfBlocks); + HAL_StatusTypeDef + Not_HAL_SDEx_WriteBlocksDMAMultiBuffer(uint32_t BlockAdd, uint32_t NumberOfBlocks); }; - template - struct SdCardWrapper{ + template struct SdCardWrapper { static constexpr bool has_cd = card_request.cd.has_value(); static constexpr bool has_wp = card_request.wp.has_value(); - - SdCardWrapper(Instance& instance) : instance(instance) { - check_cd_wp(); - }; + + SdCardWrapper(Instance& instance) : instance(instance) { check_cd_wp(); }; void init_card() { check_cd_wp(); @@ -276,11 +353,9 @@ struct SdDomain { } } - bool is_card_initialized() { - return instance.card_initialized; - } + bool is_card_initialized() { return instance.card_initialized; } - bool read_blocks(uint32_t start_block, uint32_t num_blocks, bool *operation_complete_flag) { + bool read_blocks(uint32_t start_block, uint32_t num_blocks, bool* operation_complete_flag) { check_cd_wp(); if (!instance.card_initialized) { ErrorHandler("SD Card not initialized"); @@ -288,13 +363,15 @@ struct SdDomain { if (num_blocks > instance.mpu_buffer0_instance->size / 512) { ErrorHandler("Too many blocks requested to read from SD"); } - + if (HAL_SD_GetCardState(&instance.hsd) != HAL_SD_CARD_TRANSFER) { return false; // Card not ready for data transfer } - // Won't use HAL_SDEx_ReadBlocksDMAMultiBuffer because it doesn't support double buffering the way we want - HAL_StatusTypeDef status = instance.Not_HAL_SDEx_ReadBlocksDMAMultiBuffer(start_block, num_blocks); + // Won't use HAL_SDEx_ReadBlocksDMAMultiBuffer because it doesn't support double + // buffering the way we want + HAL_StatusTypeDef status = + instance.Not_HAL_SDEx_ReadBlocksDMAMultiBuffer(start_block, num_blocks); if (status != HAL_OK) { ErrorHandler("SD Card read operation failed"); @@ -306,7 +383,8 @@ struct SdDomain { return true; } - bool write_blocks(uint32_t start_block, uint32_t num_blocks, bool *operation_complete_flag) { + bool + write_blocks(uint32_t start_block, uint32_t num_blocks, bool* operation_complete_flag) { check_cd_wp(); if (!instance.card_initialized) { ErrorHandler("SD Card not initialized"); @@ -314,12 +392,14 @@ struct SdDomain { if (num_blocks > instance.mpu_buffer0_instance->size / 512) { ErrorHandler("Too many blocks requested to write in SD"); } - + if (HAL_SD_GetCardState(&instance.hsd) != HAL_SD_CARD_TRANSFER) { return false; // Card not ready for data transfer } - // Won't use HAL_SDEx_WriteBlocksDMAMultiBuffer because it doesn't support double buffering the way we want - HAL_StatusTypeDef status = instance.Not_HAL_SDEx_WriteBlocksDMAMultiBuffer(start_block, num_blocks); + // Won't use HAL_SDEx_WriteBlocksDMAMultiBuffer because it doesn't support double + // buffering the way we want + HAL_StatusTypeDef status = + instance.Not_HAL_SDEx_WriteBlocksDMAMultiBuffer(start_block, num_blocks); if (status != HAL_OK) { ErrorHandler("SD Card write operation failed"); @@ -333,54 +413,68 @@ struct SdDomain { std::pair get_current_buffer() { if (instance.current_buffer == BufferSelect::Buffer0) { - return std::make_pair(reinterpret_cast(instance.mpu_buffer0_instance->ptr), instance.mpu_buffer0_instance->size); + return std::make_pair( + reinterpret_cast(instance.mpu_buffer0_instance->ptr), + instance.mpu_buffer0_instance->size + ); } else { - return std::make_pair(reinterpret_cast(instance.mpu_buffer1_instance->ptr), instance.mpu_buffer1_instance->size); + return std::make_pair( + reinterpret_cast(instance.mpu_buffer1_instance->ptr), + instance.mpu_buffer1_instance->size + ); } } - bool is_busy() { - return instance.is_busy(); - } + bool is_busy() { return instance.is_busy(); } - private: + private: Instance& instance; // Actual State void check_cd_wp() { if constexpr (has_cd) { - if (!instance.is_card_present()) { ErrorHandler("SD Card not present"); } + if (!instance.is_card_present()) { + ErrorHandler("SD Card not present"); + } } if constexpr (has_wp) { - if (instance.is_write_protected()) { ErrorHandler("SD Card is write-protected"); } + if (instance.is_write_protected()) { + ErrorHandler("SD Card is write-protected"); + } } } }; - - template - struct Init { + template struct Init { static inline std::array instances{}; - static void init(std::span cfgs, - std::span mpu_buffer_instances, - std::span digital_input_instances) { - + static void init( + std::span cfgs, + std::span mpu_buffer_instances, + std::span digital_input_instances + ) { + if (N == 0) { return; } for (std::size_t i = 0; i < N; i++) { - const auto &cfg = cfgs[i]; - auto &inst = instances[i]; + const auto& cfg = cfgs[i]; + auto& inst = instances[i]; inst.mpu_buffer0_instance = &mpu_buffer_instances[cfg.mpu_buffer0_idx]; inst.mpu_buffer1_instance = &mpu_buffer_instances[cfg.mpu_buffer1_idx]; if (cfg.cd_pin_idx.has_value()) { - inst.cd_instance = {&digital_input_instances[cfg.cd_pin_idx.value().first], cfg.cd_pin_idx.value().second}; + inst.cd_instance = { + &digital_input_instances[cfg.cd_pin_idx.value().first], + cfg.cd_pin_idx.value().second + }; } if (cfg.wp_pin_idx.has_value()) { - inst.wp_instance = {&digital_input_instances[cfg.wp_pin_idx.value().first], cfg.wp_pin_idx.value().second}; + inst.wp_instance = { + &digital_input_instances[cfg.wp_pin_idx.value().first], + cfg.wp_pin_idx.value().second + }; } if (cfg.peripheral == Peripheral::sdmmc1) { @@ -394,21 +488,28 @@ struct SdDomain { inst.hsd.Init.HardwareFlowControl = SDMMC_HARDWARE_FLOW_CONTROL_ENABLE; uint32_t target_freq = 50000000; // Target frequency 50 MHz - #ifdef SD_DEBUG_ENABLE +#ifdef SD_DEBUG_ENABLE inst.hsd.Init.BusWide = SDMMC_BUS_WIDE_1B; // For debugging, use 1-bit bus - target_freq = 400000; // For debugging, use 400 kHz - #endif // SD_DEBUG_ENABLE + target_freq = 400000; // For debugging, use 400 kHz +#endif // SD_DEBUG_ENABLE PLL1_ClocksTypeDef pll1_clock; HAL_RCCEx_GetPLL1ClockFreq(&pll1_clock); uint32_t sdmmc_clk = pll1_clock.PLL1_Q_Frequency; - uint32_t target_div = (sdmmc_clk + target_freq - 1) / target_freq; // Target divider rounded up (target_freq is the maximum frequency) + uint32_t target_div = + (sdmmc_clk + target_freq - 1) / + target_freq; // Target divider rounded up (target_freq is the maximum frequency) uint32_t translated_clock_div; // sdmmc_ker_ck / [2 * CLKDIV] - if (target_div == 0) translated_clock_div = 0; // Special case for 0 (no division) - else translated_clock_div = (target_div+1) / 2; // Round up to ensure frequency is not higher than target + if (target_div == 0) + translated_clock_div = 0; // Special case for 0 (no division) + else + translated_clock_div = + (target_div + 1) / + 2; // Round up to ensure frequency is not higher than target if (translated_clock_div > 1023) { - ErrorHandler("SDMMC clock divider too high, cannot achieve target frequency with current PLL1 Q clock"); + ErrorHandler("SDMMC clock divider too high, cannot achieve target frequency " + "with current PLL1 Q clock"); } inst.hsd.Init.ClockDiv = translated_clock_div; @@ -430,7 +531,8 @@ struct SdDomain { RCC_PeriphCLKInitStruct.PeriphClockSelection = RCC_PERIPHCLK_SDMMC; RCC_PeriphCLKInitStruct.SdmmcClockSelection = RCC_SDMMCCLKSOURCE_PLL; if (HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphCLKInitStruct) != HAL_OK) { - ErrorHandler("SDMMC clock configuration failed, maybe try with a slower clock or higher divider?"); + ErrorHandler("SDMMC clock configuration failed, maybe try with a slower clock or " + "higher divider?"); } // Ensure PLL1Q output is enabled @@ -443,9 +545,11 @@ struct SdDomain { }; }; -static inline SdDomain::Instance* get_sd_instance(SD_HandleTypeDef *hsd) { - if (hsd == g_sdmmc1_handle) return static_cast(g_sdmmc1_instance_ptr); - if (hsd == g_sdmmc2_handle) return static_cast(g_sdmmc2_instance_ptr); +static inline SdDomain::Instance* get_sd_instance(SD_HandleTypeDef* hsd) { + if (hsd == g_sdmmc1_handle) + return static_cast(g_sdmmc1_instance_ptr); + if (hsd == g_sdmmc2_handle) + return static_cast(g_sdmmc2_instance_ptr); return nullptr; } diff --git a/Inc/ST-LIB_LOW/Sensors/Common/PT100.hpp b/Inc/ST-LIB_LOW/Sensors/Common/PT100.hpp index 3790c6e8d..252629dc5 100644 --- a/Inc/ST-LIB_LOW/Sensors/Common/PT100.hpp +++ b/Inc/ST-LIB_LOW/Sensors/Common/PT100.hpp @@ -5,52 +5,49 @@ #include "Control/Blocks/MovingAverage.hpp" #include "HALAL/Services/ADC/NewADC.hpp" - -template -class PT100{ +template class PT100 { public: - static constexpr float k = 841.836735; - static constexpr float offset = -492.204082; - MovingAverage* filter = nullptr; + static constexpr float k = 841.836735; + static constexpr float offset = -492.204082; + MovingAverage* filter = nullptr; + + PT100(ST_LIB::ADCDomain::Instance& adc, float* value, MovingAverage& filter); + PT100(ST_LIB::ADCDomain::Instance& adc, float* value); - PT100(ST_LIB::ADCDomain::Instance& adc, float* value, MovingAverage& filter); - PT100(ST_LIB::ADCDomain::Instance& adc, float* value); + PT100(ST_LIB::ADCDomain::Instance& adc, float& value, MovingAverage& filter); + PT100(ST_LIB::ADCDomain::Instance& adc, float& value); - PT100(ST_LIB::ADCDomain::Instance& adc, float& value, MovingAverage& filter); - PT100(ST_LIB::ADCDomain::Instance& adc, float& value); + void read(); - void read(); protected: - ST_LIB::ADCDomain::Instance* adc = nullptr; - float* value = nullptr; + ST_LIB::ADCDomain::Instance* adc = nullptr; + float* value = nullptr; }; -template +template PT100::PT100(ST_LIB::ADCDomain::Instance& adc, float* value, MovingAverage& filter) - : adc(&adc), value(value), filter(&filter){} + : adc(&adc), value(value), filter(&filter) {} -template +template PT100::PT100(ST_LIB::ADCDomain::Instance& adc, float& value, MovingAverage& filter) - : adc(&adc), value(&value), filter(&filter){} - -template -PT100::PT100(ST_LIB::ADCDomain::Instance& adc, float* value) - : adc(&adc), value(value){} - -template -PT100::PT100(ST_LIB::ADCDomain::Instance& adc, float& value) - : adc(&adc), value(&value){} - -template -void PT100::read(){ - if(adc == nullptr || value == nullptr){ - return; - } - const float raw = adc->get_raw(); - const float val = adc->get_value_from_raw(raw, 3.3f); - if(filter != nullptr){ - filter->input(k/val + offset); - filter->execute(); - *value = filter->output_value; - }else *value = k/val + offset; + : adc(&adc), value(&value), filter(&filter) {} + +template +PT100::PT100(ST_LIB::ADCDomain::Instance& adc, float* value) : adc(&adc), value(value) {} + +template +PT100::PT100(ST_LIB::ADCDomain::Instance& adc, float& value) : adc(&adc), value(&value) {} + +template void PT100::read() { + if (adc == nullptr || value == nullptr) { + return; + } + const float raw = adc->get_raw(); + const float val = adc->get_value_from_raw(raw, 3.3f); + if (filter != nullptr) { + filter->input(k / val + offset); + filter->execute(); + *value = filter->output_value; + } else + *value = k / val + offset; } diff --git a/Inc/ST-LIB_LOW/Sensors/DigitalSensor/DigitalSensor.hpp b/Inc/ST-LIB_LOW/Sensors/DigitalSensor/DigitalSensor.hpp index 55ec634d5..9119d6c6e 100644 --- a/Inc/ST-LIB_LOW/Sensors/DigitalSensor/DigitalSensor.hpp +++ b/Inc/ST-LIB_LOW/Sensors/DigitalSensor/DigitalSensor.hpp @@ -11,17 +11,15 @@ #include "HALAL/Models/PinModel/Pin.hpp" - -class DigitalSensor{ +class DigitalSensor { public: - DigitalSensor() = default; - DigitalSensor(Pin &pin, PinState *value); - DigitalSensor(Pin &pin, PinState &value); - void read(); - uint8_t get_id(); + DigitalSensor() = default; + DigitalSensor(Pin& pin, PinState* value); + DigitalSensor(Pin& pin, PinState& value); + void read(); + uint8_t get_id(); protected: - uint8_t id; - PinState *value; + uint8_t id; + PinState* value; }; - diff --git a/Inc/ST-LIB_LOW/Sensors/EncoderSensor/EncoderSensor.hpp b/Inc/ST-LIB_LOW/Sensors/EncoderSensor/EncoderSensor.hpp index 53cdd84b9..f3f6b479a 100644 --- a/Inc/ST-LIB_LOW/Sensors/EncoderSensor/EncoderSensor.hpp +++ b/Inc/ST-LIB_LOW/Sensors/EncoderSensor/EncoderSensor.hpp @@ -15,12 +15,11 @@ #include "HALAL/Models/PinModel/Pin.hpp" #include "HALAL/Services/Encoder/Encoder.hpp" -template -class EncoderSensor { - public: +template class EncoderSensor { +public: enum Direction : uint8_t { FORWARD = 0, BACKWARDS = 1 }; - private: +private: constexpr static int64_t START_COUNTER{UINT32_MAX / 2}; const double counter_distance_m; @@ -33,23 +32,27 @@ class EncoderSensor { // across time. The SAMPLES computation rounds it down to make it even RingBuffer past_delta_counters{}; - Direction *direction; - double *position; - double *speed; - double *acceleration; - - public: - EncoderSensor(Pin &pin1, Pin &pin2, const double counter_distance_m, - const double sample_time_s, Direction *direction, - double *position, double *speed, double *acceleration) - : counter_distance_m(counter_distance_m), - sample_time_s(sample_time_s), - encoder_id(Encoder::inscribe(pin1, pin2)), - direction(direction), - position(position), - speed(speed), - acceleration(acceleration) { - for (size_t i{0}; i < SAMPLES; ++i) past_delta_counters.push(0); + Direction* direction; + double* position; + double* speed; + double* acceleration; + +public: + EncoderSensor( + Pin& pin1, + Pin& pin2, + const double counter_distance_m, + const double sample_time_s, + Direction* direction, + double* position, + double* speed, + double* acceleration + ) + : counter_distance_m(counter_distance_m), sample_time_s(sample_time_s), + encoder_id(Encoder::inscribe(pin1, pin2)), direction(direction), position(position), + speed(speed), acceleration(acceleration) { + for (size_t i{0}; i < SAMPLES; ++i) + past_delta_counters.push(0); } void turn_on() { Encoder::turn_on(encoder_id); } @@ -57,7 +60,8 @@ class EncoderSensor { void reset() { Encoder::reset(encoder_id); - for (size_t i{0}; i < SAMPLES; ++i) past_delta_counters.push_pop(0); + for (size_t i{0}; i < SAMPLES; ++i) + past_delta_counters.push_pop(0); } // must be called on equally spaced time periods @@ -65,24 +69,25 @@ class EncoderSensor { uint32_t counter{Encoder::get_counter(encoder_id)}; int64_t delta_counter{(int64_t)counter - START_COUNTER}; - const int64_t &previous_delta_counter{ - past_delta_counters[past_delta_counters.size() / 2 - 1]}; - const int64_t &previous_previous_delta_counter{ - past_delta_counters[past_delta_counters.size() - 1]}; + const int64_t& previous_delta_counter{ + past_delta_counters[past_delta_counters.size() / 2 - 1] + }; + const int64_t& previous_previous_delta_counter{ + past_delta_counters[past_delta_counters.size() - 1] + }; *position = delta_counter * counter_distance_m; // https://en.wikipedia.org/wiki/Finite_difference_coefficient#Backward_finite_difference *speed = ((3.0 * delta_counter / 2.0) - (2.0 * previous_delta_counter) + (previous_previous_delta_counter / 2.0)) * - counter_distance_m / - (sample_time_s * past_delta_counters.size() / 2); - - *acceleration = (delta_counter - (2.0 * previous_delta_counter) + - previous_previous_delta_counter) * - counter_distance_m / - ((sample_time_s * past_delta_counters.size() / 2) * - (sample_time_s * past_delta_counters.size() / 2)); + counter_distance_m / (sample_time_s * past_delta_counters.size() / 2); + + *acceleration = + (delta_counter - (2.0 * previous_delta_counter) + previous_previous_delta_counter) * + counter_distance_m / + ((sample_time_s * past_delta_counters.size() / 2) * + (sample_time_s * past_delta_counters.size() / 2)); *direction = Encoder::get_direction(encoder_id) ? FORWARD : BACKWARDS; diff --git a/Inc/ST-LIB_LOW/Sensors/EncoderSensor/NewEncoderSensor.hpp b/Inc/ST-LIB_LOW/Sensors/EncoderSensor/NewEncoderSensor.hpp index bb2d6c626..9c72527f4 100644 --- a/Inc/ST-LIB_LOW/Sensors/EncoderSensor/NewEncoderSensor.hpp +++ b/Inc/ST-LIB_LOW/Sensors/EncoderSensor/NewEncoderSensor.hpp @@ -7,12 +7,11 @@ #pragma once #include "HALAL/HALAL.hpp" -//#include "HALAL/Services/Encoder/NewEncoder.hpp" +// #include "HALAL/Services/Encoder/NewEncoder.hpp" namespace ST_LIB { -template -struct EncoderSensor { +template struct EncoderSensor { enum Direction : uint8_t { FORWARD = 0, BACKWARDS = 1 }; private: @@ -25,32 +24,34 @@ struct EncoderSensor { RingBuffer past_delta_counters{}; - Direction *direction; - double *position; - double *speed; - double *acceleration; + Direction* direction; + double* position; + double* speed; + double* acceleration; public: - EncoderSensor(EncoderType& enc, const double counter_distance_m, - const double sample_time_s, Direction *direction, - double *position, double *speed, double *acceleration) : - counter_distance_m(counter_distance_m), - sample_time_s(sample_time_s), - encoder(enc), - direction(direction), - position(position), - speed(speed), - acceleration(acceleration) - { - for(size_t i{0}; i < SAMPLES; i++) past_delta_counters.push(0); + EncoderSensor( + EncoderType& enc, + const double counter_distance_m, + const double sample_time_s, + Direction* direction, + double* position, + double* speed, + double* acceleration + ) + : counter_distance_m(counter_distance_m), sample_time_s(sample_time_s), encoder(enc), + direction(direction), position(position), speed(speed), acceleration(acceleration) { + for (size_t i{0}; i < SAMPLES; i++) + past_delta_counters.push(0); } void turn_on() { encoder.turn_on(); } void turn_off() { encoder.turn_off(); } - + void reset() { encoder.reset(); - for (size_t i{0}; i < SAMPLES; ++i) past_delta_counters.push_pop(0); + for (size_t i{0}; i < SAMPLES; ++i) + past_delta_counters.push_pop(0); } // must be called on equally spaced time periods @@ -58,24 +59,25 @@ struct EncoderSensor { uint32_t counter{encoder.get_counter()}; int64_t delta_counter{(int64_t)counter - START_COUNTER}; - const int64_t &previous_delta_counter{ - past_delta_counters[past_delta_counters.size() / 2 - 1]}; - const int64_t &previous_previous_delta_counter{ - past_delta_counters[past_delta_counters.size() - 1]}; + const int64_t& previous_delta_counter{ + past_delta_counters[past_delta_counters.size() / 2 - 1] + }; + const int64_t& previous_previous_delta_counter{ + past_delta_counters[past_delta_counters.size() - 1] + }; *position = delta_counter * counter_distance_m; // https://en.wikipedia.org/wiki/Finite_difference_coefficient#Backward_finite_difference *speed = ((3.0 * delta_counter / 2.0) - (2.0 * previous_delta_counter) + (previous_previous_delta_counter / 2.0)) * - counter_distance_m / - (sample_time_s * past_delta_counters.size() / 2); + counter_distance_m / (sample_time_s * past_delta_counters.size() / 2); - *acceleration = (delta_counter - (2.0 * previous_delta_counter) + - previous_previous_delta_counter) * - counter_distance_m / - ((sample_time_s * past_delta_counters.size() / 2) * - (sample_time_s * past_delta_counters.size() / 2)); + *acceleration = + (delta_counter - (2.0 * previous_delta_counter) + previous_previous_delta_counter) * + counter_distance_m / + ((sample_time_s * past_delta_counters.size() / 2) * + (sample_time_s * past_delta_counters.size() / 2)); *direction = encoder.get_direction() ? FORWARD : BACKWARDS; @@ -83,4 +85,4 @@ struct EncoderSensor { } }; -} \ No newline at end of file +} // namespace ST_LIB \ No newline at end of file diff --git a/Inc/ST-LIB_LOW/Sensors/LinearSensor/FilteredLinearSensor.hpp b/Inc/ST-LIB_LOW/Sensors/LinearSensor/FilteredLinearSensor.hpp index 63ce27309..4227cf2f7 100644 --- a/Inc/ST-LIB_LOW/Sensors/LinearSensor/FilteredLinearSensor.hpp +++ b/Inc/ST-LIB_LOW/Sensors/LinearSensor/FilteredLinearSensor.hpp @@ -3,38 +3,54 @@ #include "Control/Blocks/MovingAverage.hpp" #include "LinearSensor.hpp" -template -class FilteredLinearSensor : public LinearSensor { - MovingAverage &filter; +template class FilteredLinearSensor : public LinearSensor { + MovingAverage& filter; public: - FilteredLinearSensor(ST_LIB::ADCDomain::Instance &adc, Type slope, - Type offset, Type *value, MovingAverage &filter) - : LinearSensor(adc, slope, offset, value), filter(filter) {} + FilteredLinearSensor( + ST_LIB::ADCDomain::Instance& adc, + Type slope, + Type offset, + Type* value, + MovingAverage& filter + ) + : LinearSensor(adc, slope, offset, value), filter(filter) {} - FilteredLinearSensor(ST_LIB::ADCDomain::Instance &adc, Type slope, - Type offset, Type &value, MovingAverage &filter) - : LinearSensor(adc, slope, offset, value), filter(filter) {} + FilteredLinearSensor( + ST_LIB::ADCDomain::Instance& adc, + Type slope, + Type offset, + Type& value, + MovingAverage& filter + ) + : LinearSensor(adc, slope, offset, value), filter(filter) {} - void read() { - if (this->adc == nullptr || this->value == nullptr) { - return; + void read() { + if (this->adc == nullptr || this->value == nullptr) { + return; + } + const float raw = this->adc->get_raw(); + const float val = this->adc->get_value_from_raw(raw, this->vref); + *this->value = filter.compute(this->slope * static_cast(val) + this->offset); } - const float raw = this->adc->get_raw(); - const float val = this->adc->get_value_from_raw(raw, this->vref); - *this->value = - filter.compute(this->slope * static_cast(val) + this->offset); - } }; // CTAD #if __cpp_deduction_guides >= 201606 template -FilteredLinearSensor(ST_LIB::ADCDomain::Instance &adc, Type slope, Type offset, - Type *value, MovingAverage &filter) - -> FilteredLinearSensor; +FilteredLinearSensor( + ST_LIB::ADCDomain::Instance& adc, + Type slope, + Type offset, + Type* value, + MovingAverage& filter +) -> FilteredLinearSensor; template -FilteredLinearSensor(ST_LIB::ADCDomain::Instance &adc, Type slope, Type offset, - Type &value, MovingAverage &filter) - -> FilteredLinearSensor; +FilteredLinearSensor( + ST_LIB::ADCDomain::Instance& adc, + Type slope, + Type offset, + Type& value, + MovingAverage& filter +) -> FilteredLinearSensor; #endif diff --git a/Inc/ST-LIB_LOW/Sensors/LinearSensor/LinearSensor.hpp b/Inc/ST-LIB_LOW/Sensors/LinearSensor/LinearSensor.hpp index b5ebfb71c..c0fbe8a4a 100644 --- a/Inc/ST-LIB_LOW/Sensors/LinearSensor/LinearSensor.hpp +++ b/Inc/ST-LIB_LOW/Sensors/LinearSensor/LinearSensor.hpp @@ -14,12 +14,22 @@ template requires std::is_integral_v || std::is_floating_point_v class LinearSensor { - public: +public: LinearSensor() = default; - LinearSensor(ST_LIB::ADCDomain::Instance &adc, Type slope, Type offset, - Type *value, float vref = 3.3f); - LinearSensor(ST_LIB::ADCDomain::Instance &adc, Type slope, Type offset, - Type &value, float vref = 3.3f); + LinearSensor( + ST_LIB::ADCDomain::Instance& adc, + Type slope, + Type offset, + Type* value, + float vref = 3.3f + ); + LinearSensor( + ST_LIB::ADCDomain::Instance& adc, + Type slope, + Type offset, + Type& value, + float vref = 3.3f + ); void read(); @@ -29,26 +39,36 @@ class LinearSensor { void set_gain(Type new_gain); Type get_gain(); - Type *get_value_pointer() const; + Type* get_value_pointer() const; - protected: - ST_LIB::ADCDomain::Instance *adc = nullptr; +protected: + ST_LIB::ADCDomain::Instance* adc = nullptr; Type slope; Type offset; - Type *value = nullptr; + Type* value = nullptr; float vref = 3.3f; }; template requires std::is_integral_v || std::is_floating_point_v -LinearSensor::LinearSensor(ST_LIB::ADCDomain::Instance &adc, Type slope, - Type offset, Type *value, float vref) +LinearSensor::LinearSensor( + ST_LIB::ADCDomain::Instance& adc, + Type slope, + Type offset, + Type* value, + float vref +) : adc(&adc), slope(slope), offset(offset), value(value), vref(vref) {} template requires std::is_integral_v || std::is_floating_point_v -LinearSensor::LinearSensor(ST_LIB::ADCDomain::Instance &adc, Type slope, - Type offset, Type &value, float vref) +LinearSensor::LinearSensor( + ST_LIB::ADCDomain::Instance& adc, + Type slope, + Type offset, + Type& value, + float vref +) : LinearSensor::LinearSensor(adc, slope, offset, &value, vref) {} template @@ -89,6 +109,6 @@ void LinearSensor::set_gain(Type new_gain) { template requires std::is_integral_v || std::is_floating_point_v -Type *LinearSensor::get_value_pointer() const { +Type* LinearSensor::get_value_pointer() const { return value; } diff --git a/Inc/ST-LIB_LOW/Sensors/LookupSensor/LookupSensor.hpp b/Inc/ST-LIB_LOW/Sensors/LookupSensor/LookupSensor.hpp index 7cf6c727a..7422c462a 100644 --- a/Inc/ST-LIB_LOW/Sensors/LookupSensor/LookupSensor.hpp +++ b/Inc/ST-LIB_LOW/Sensors/LookupSensor/LookupSensor.hpp @@ -13,17 +13,16 @@ #define REFERENCE_VOLTAGE 3.3 - -class LookupSensor{ +class LookupSensor { public: - LookupSensor() = default; - LookupSensor(ST_LIB::ADCDomain::Instance& adc, double *table, int table_size, double *value); - LookupSensor(ST_LIB::ADCDomain::Instance& adc, double *table, int table_size, double &value); - void read(); + LookupSensor() = default; + LookupSensor(ST_LIB::ADCDomain::Instance& adc, double* table, int table_size, double* value); + LookupSensor(ST_LIB::ADCDomain::Instance& adc, double* table, int table_size, double& value); + void read(); protected: - ST_LIB::ADCDomain::Instance* adc = nullptr; - double *table = nullptr; - int table_size = 0; - double *value = nullptr; + ST_LIB::ADCDomain::Instance* adc = nullptr; + double* table = nullptr; + int table_size = 0; + double* value = nullptr; }; diff --git a/Inc/ST-LIB_LOW/Sensors/NTC/NTC.hpp b/Inc/ST-LIB_LOW/Sensors/NTC/NTC.hpp index c5e7ca90f..8de9475c5 100644 --- a/Inc/ST-LIB_LOW/Sensors/NTC/NTC.hpp +++ b/Inc/ST-LIB_LOW/Sensors/NTC/NTC.hpp @@ -5,7 +5,6 @@ * Author: ricardo */ - /* This NTC class is not generic. It is only for 10k Ohm, 1976Beta value NTCs. */ @@ -14,489 +13,290 @@ This NTC class is not generic. It is only for 10k Ohm, 1976Beta value NTCs. #include "HALAL/Services/ADC/NewADC.hpp" -class NTC{ +class NTC { public: - NTC() = default; - NTC(ST_LIB::ADCDomain::Instance& adc, float* src); - NTC(ST_LIB::ADCDomain::Instance& adc, float& src); - void read(); + NTC() = default; + NTC(ST_LIB::ADCDomain::Instance& adc, float* src); + NTC(ST_LIB::ADCDomain::Instance& adc, float& src); + void read(); private: - static constexpr int NTC_table[4096] = { - 6154, 5192, 4230, 3768, 3476, 3267, 3106, - 2977, 2869, 2778, 2698, 2628, 2566, 2510, - 2459, 2412, 2370, 2330, 2293, 2259, 2227, - 2197, 2169, 2142, 2116, 2092, 2069, 2047, - 2026, 2006, 1987, 1969, 1951, 1934, 1917, - 1902, 1886, 1872, 1857, 1843, 1830, 1817, - 1804, 1792, 1780, 1769, 1757, 1746, 1736, - 1725, 1715, 1705, 1695, 1686, 1676, 1667, - 1659, 1650, 1641, 1633, 1625, 1617, 1609, - 1601, 1594, 1586, 1579, 1572, 1565, 1558, - 1551, 1545, 1538, 1532, 1525, 1519, 1513, - 1507, 1501, 1495, 1490, 1484, 1478, 1473, - 1467, 1462, 1457, 1451, 1446, 1441, 1436, - 1431, 1426, 1422, 1417, 1412, 1408, 1403, - 1399, 1394, 1390, 1385, 1381, 1377, 1373, - 1368, 1364, 1360, 1356, 1352, 1348, 1344, - 1341, 1337, 1333, 1329, 1326, 1322, 1318, - 1315, 1311, 1308, 1304, 1301, 1297, 1294, - 1291, 1287, 1284, 1281, 1278, 1274, 1271, - 1268, 1265, 1262, 1259, 1256, 1253, 1250, - 1247, 1244, 1241, 1238, 1235, 1232, 1230, - 1227, 1224, 1221, 1219, 1216, 1213, 1210, - 1208, 1205, 1203, 1200, 1197, 1195, 1192, - 1190, 1187, 1185, 1182, 1180, 1178, 1175, - 1173, 1170, 1168, 1166, 1163, 1161, 1159, - 1157, 1154, 1152, 1150, 1148, 1145, 1143, - 1141, 1139, 1137, 1135, 1132, 1130, 1128, - 1126, 1124, 1122, 1120, 1118, 1116, 1114, - 1112, 1110, 1108, 1106, 1104, 1102, 1100, - 1098, 1096, 1094, 1093, 1091, 1089, 1087, - 1085, 1083, 1081, 1080, 1078, 1076, 1074, - 1073, 1071, 1069, 1067, 1065, 1064, 1062, - 1060, 1059, 1057, 1055, 1054, 1052, 1050, - 1049, 1047, 1045, 1044, 1042, 1040, 1039, - 1037, 1036, 1034, 1032, 1031, 1029, 1028, - 1026, 1025, 1023, 1022, 1020, 1019, 1017, - 1016, 1014, 1013, 1011, 1010, 1008, 1007, - 1005, 1004, 1002, 1001, 1000, 998, 997, 995, - 994, 993, 991, 990, 988, 987, 986, 984, 983, - 982, 980, 979, 978, 976, 975, 974, 972, 971, - 970, 968, 967, 966, 965, 963, 962, 961, 959, - 958, 957, 956, 954, 953, 952, 951, 950, 948, - 947, 946, 945, 944, 942, 941, 940, 939, 938, - 936, 935, 934, 933, 932, 931, 929, 928, 927, - 926, 925, 924, 923, 921, 920, 919, 918, 917, - 916, 915, 914, 913, 912, 910, 909, 908, 907, - 906, 905, 904, 903, 902, 901, 900, 899, 898, - 897, 896, 895, 894, 893, 891, 890, 889, 888, - 887, 886, 885, 884, 883, 882, 881, 880, 879, - 878, 877, 876, 876, 875, 874, 873, 872, 871, - 870, 869, 868, 867, 866, 865, 864, 863, 862, - 861, 860, 859, 858, 857, 857, 856, 855, 854, - 853, 852, 851, 850, 849, 848, 847, 847, 846, - 845, 844, 843, 842, 841, 840, 840, 839, 838, - 837, 836, 835, 834, 833, 833, 832, 831, 830, - 829, 828, 828, 827, 826, 825, 824, 823, 823, - 822, 821, 820, 819, 818, 818, 817, 816, 815, - 814, 814, 813, 812, 811, 810, 810, 809, 808, - 807, 806, 806, 805, 804, 803, 802, 802, 801, - 800, 799, 799, 798, 797, 796, 795, 795, 794, - 793, 792, 792, 791, 790, 789, 789, 788, 787, - 786, 786, 785, 784, 784, 783, 782, 781, 781, - 780, 779, 778, 778, 777, 776, 776, 775, 774, - 773, 773, 772, 771, 771, 770, 769, 768, 768, - 767, 766, 766, 765, 764, 764, 763, 762, 761, - 761, 760, 759, 759, 758, 757, 757, 756, 755, - 755, 754, 753, 753, 752, 751, 751, 750, 749, - 749, 748, 747, 747, 746, 745, 745, 744, 743, - 743, 742, 741, 741, 740, 739, 739, 738, 738, - 737, 736, 736, 735, 734, 734, 733, 732, 732, - 731, 731, 730, 729, 729, 728, 727, 727, 726, - 726, 725, 724, 724, 723, 723, 722, 721, 721, - 720, 719, 719, 718, 718, 717, 716, 716, 715, - 715, 714, 713, 713, 712, 712, 711, 710, 710, - 709, 709, 708, 708, 707, 706, 706, 705, 705, - 704, 703, 703, 702, 702, 701, 701, 700, 699, - 699, 698, 698, 697, 697, 696, 695, 695, 694, - 694, 693, 693, 692, 692, 691, 690, 690, 689, - 689, 688, 688, 687, 687, 686, 685, 685, 684, - 684, 683, 683, 682, 682, 681, 681, 680, 679, - 679, 678, 678, 677, 677, 676, 676, 675, 675, - 674, 674, 673, 673, 672, 671, 671, 670, 670, - 669, 669, 668, 668, 667, 667, 666, 666, 665, - 665, 664, 664, 663, 663, 662, 662, 661, 661, - 660, 660, 659, 659, 658, 658, 657, 657, 656, - 656, 655, 655, 654, 654, 653, 653, 652, 652, - 651, 651, 650, 650, 649, 649, 648, 648, 647, - 647, 646, 646, 645, 645, 644, 644, 643, 643, - 642, 642, 641, 641, 640, 640, 639, 639, 638, - 638, 637, 637, 636, 636, 635, 635, 635, 634, - 634, 633, 633, 632, 632, 631, 631, 630, 630, - 629, 629, 628, 628, 628, 627, 627, 626, 626, - 625, 625, 624, 624, 623, 623, 622, 622, 622, - 621, 621, 620, 620, 619, 619, 618, 618, 617, - 617, 617, 616, 616, 615, 615, 614, 614, 613, - 613, 613, 612, 612, 611, 611, 610, 610, 609, - 609, 609, 608, 608, 607, 607, 606, 606, 605, - 605, 605, 604, 604, 603, 603, 602, 602, 602, - 601, 601, 600, 600, 599, 599, 599, 598, 598, - 597, 597, 596, 596, 596, 595, 595, 594, 594, - 593, 593, 593, 592, 592, 591, 591, 591, 590, - 590, 589, 589, 588, 588, 588, 587, 587, 586, - 586, 586, 585, 585, 584, 584, 583, 583, 583, - 582, 582, 581, 581, 581, 580, 580, 579, 579, - 579, 578, 578, 577, 577, 577, 576, 576, 575, - 575, 575, 574, 574, 573, 573, 573, 572, 572, - 571, 571, 571, 570, 570, 569, 569, 569, 568, - 568, 567, 567, 567, 566, 566, 565, 565, 565, - 564, 564, 563, 563, 563, 562, 562, 562, 561, - 561, 560, 560, 560, 559, 559, 558, 558, 558, - 557, 557, 557, 556, 556, 555, 555, 555, 554, - 554, 553, 553, 553, 552, 552, 552, 551, 551, - 550, 550, 550, 549, 549, 549, 548, 548, 547, - 547, 547, 546, 546, 546, 545, 545, 544, 544, - 544, 543, 543, 543, 542, 542, 542, 541, 541, - 540, 540, 540, 539, 539, 539, 538, 538, 538, - 537, 537, 536, 536, 536, 535, 535, 535, 534, - 534, 534, 533, 533, 532, 532, 532, 531, 531, - 531, 530, 530, 530, 529, 529, 529, 528, 528, - 527, 527, 527, 526, 526, 526, 525, 525, 525, - 524, 524, 524, 523, 523, 523, 522, 522, 521, - 521, 521, 520, 520, 520, 519, 519, 519, 518, - 518, 518, 517, 517, 517, 516, 516, 516, 515, - 515, 515, 514, 514, 514, 513, 513, 513, 512, - 512, 511, 511, 511, 510, 510, 510, 509, 509, - 509, 508, 508, 508, 507, 507, 507, 506, 506, - 506, 505, 505, 505, 504, 504, 504, 503, 503, - 503, 502, 502, 502, 501, 501, 501, 500, 500, - 500, 499, 499, 499, 498, 498, 498, 497, 497, - 497, 496, 496, 496, 495, 495, 495, 494, 494, - 494, 494, 493, 493, 493, 492, 492, 492, 491, - 491, 491, 490, 490, 490, 489, 489, 489, 488, - 488, 488, 487, 487, 487, 486, 486, 486, 485, - 485, 485, 484, 484, 484, 484, 483, 483, 483, - 482, 482, 482, 481, 481, 481, 480, 480, 480, - 479, 479, 479, 478, 478, 478, 478, 477, 477, - 477, 476, 476, 476, 475, 475, 475, 474, 474, - 474, 473, 473, 473, 473, 472, 472, 472, 471, - 471, 471, 470, 470, 470, 469, 469, 469, 469, - 468, 468, 468, 467, 467, 467, 466, 466, 466, - 465, 465, 465, 465, 464, 464, 464, 463, 463, - 463, 462, 462, 462, 462, 461, 461, 461, 460, - 460, 460, 459, 459, 459, 458, 458, 458, 458, - 457, 457, 457, 456, 456, 456, 455, 455, 455, - 455, 454, 454, 454, 453, 453, 453, 453, 452, - 452, 452, 451, 451, 451, 450, 450, 450, 450, - 449, 449, 449, 448, 448, 448, 448, 447, 447, - 447, 446, 446, 446, 445, 445, 445, 445, 444, - 444, 444, 443, 443, 443, 443, 442, 442, 442, - 441, 441, 441, 441, 440, 440, 440, 439, 439, - 439, 439, 438, 438, 438, 437, 437, 437, 436, - 436, 436, 436, 435, 435, 435, 434, 434, 434, - 434, 433, 433, 433, 433, 432, 432, 432, 431, - 431, 431, 431, 430, 430, 430, 429, 429, 429, - 429, 428, 428, 428, 427, 427, 427, 427, 426, - 426, 426, 425, 425, 425, 425, 424, 424, 424, - 423, 423, 423, 423, 422, 422, 422, 422, 421, - 421, 421, 420, 420, 420, 420, 419, 419, 419, - 419, 418, 418, 418, 417, 417, 417, 417, 416, - 416, 416, 415, 415, 415, 415, 414, 414, 414, - 414, 413, 413, 413, 412, 412, 412, 412, 411, - 411, 411, 411, 410, 410, 410, 409, 409, 409, - 409, 408, 408, 408, 408, 407, 407, 407, 407, - 406, 406, 406, 405, 405, 405, 405, 404, 404, - 404, 404, 403, 403, 403, 402, 402, 402, 402, - 401, 401, 401, 401, 400, 400, 400, 400, 399, - 399, 399, 398, 398, 398, 398, 397, 397, 397, - 397, 396, 396, 396, 396, 395, 395, 395, 395, - 394, 394, 394, 393, 393, 393, 393, 392, 392, - 392, 392, 391, 391, 391, 391, 390, 390, 390, - 390, 389, 389, 389, 388, 388, 388, 388, 387, - 387, 387, 387, 386, 386, 386, 386, 385, 385, - 385, 385, 384, 384, 384, 384, 383, 383, 383, - 383, 382, 382, 382, 381, 381, 381, 381, 380, - 380, 380, 380, 379, 379, 379, 379, 378, 378, - 378, 378, 377, 377, 377, 377, 376, 376, 376, - 376, 375, 375, 375, 375, 374, 374, 374, 374, - 373, 373, 373, 373, 372, 372, 372, 372, 371, - 371, 371, 371, 370, 370, 370, 370, 369, 369, - 369, 369, 368, 368, 368, 367, 367, 367, 367, - 366, 366, 366, 366, 365, 365, 365, 365, 364, - 364, 364, 364, 363, 363, 363, 363, 362, 362, - 362, 362, 361, 361, 361, 361, 361, 360, 360, - 360, 360, 359, 359, 359, 359, 358, 358, 358, - 358, 357, 357, 357, 357, 356, 356, 356, 356, - 355, 355, 355, 355, 354, 354, 354, 354, 353, - 353, 353, 353, 352, 352, 352, 352, 351, 351, - 351, 351, 350, 350, 350, 350, 349, 349, 349, - 349, 348, 348, 348, 348, 347, 347, 347, 347, - 347, 346, 346, 346, 346, 345, 345, 345, 345, - 344, 344, 344, 344, 343, 343, 343, 343, 342, - 342, 342, 342, 341, 341, 341, 341, 340, 340, - 340, 340, 339, 339, 339, 339, 339, 338, 338, - 338, 338, 337, 337, 337, 337, 336, 336, 336, - 336, 335, 335, 335, 335, 334, 334, 334, 334, - 334, 333, 333, 333, 333, 332, 332, 332, 332, - 331, 331, 331, 331, 330, 330, 330, 330, 329, - 329, 329, 329, 329, 328, 328, 328, 328, 327, - 327, 327, 327, 326, 326, 326, 326, 325, 325, - 325, 325, 325, 324, 324, 324, 324, 323, 323, - 323, 323, 322, 322, 322, 322, 321, 321, 321, - 321, 321, 320, 320, 320, 320, 319, 319, 319, - 319, 318, 318, 318, 318, 317, 317, 317, 317, - 317, 316, 316, 316, 316, 315, 315, 315, 315, - 314, 314, 314, 314, 314, 313, 313, 313, 313, - 312, 312, 312, 312, 311, 311, 311, 311, 310, - 310, 310, 310, 310, 309, 309, 309, 309, 308, - 308, 308, 308, 307, 307, 307, 307, 307, 306, - 306, 306, 306, 305, 305, 305, 305, 305, 304, - 304, 304, 304, 303, 303, 303, 303, 302, 302, - 302, 302, 302, 301, 301, 301, 301, 300, 300, - 300, 300, 299, 299, 299, 299, 299, 298, 298, - 298, 298, 297, 297, 297, 297, 297, 296, 296, - 296, 296, 295, 295, 295, 295, 294, 294, 294, - 294, 294, 293, 293, 293, 293, 292, 292, 292, - 292, 292, 291, 291, 291, 291, 290, 290, 290, - 290, 289, 289, 289, 289, 289, 288, 288, 288, - 288, 287, 287, 287, 287, 287, 286, 286, 286, - 286, 285, 285, 285, 285, 285, 284, 284, 284, - 284, 283, 283, 283, 283, 283, 282, 282, 282, - 282, 281, 281, 281, 281, 280, 280, 280, 280, - 280, 279, 279, 279, 279, 278, 278, 278, 278, - 278, 277, 277, 277, 277, 276, 276, 276, 276, - 276, 275, 275, 275, 275, 274, 274, 274, 274, - 274, 273, 273, 273, 273, 272, 272, 272, 272, - 272, 271, 271, 271, 271, 270, 270, 270, 270, - 270, 269, 269, 269, 269, 268, 268, 268, 268, - 268, 267, 267, 267, 267, 266, 266, 266, 266, - 266, 265, 265, 265, 265, 264, 264, 264, 264, - 264, 263, 263, 263, 263, 263, 262, 262, 262, - 262, 261, 261, 261, 261, 261, 260, 260, 260, - 260, 259, 259, 259, 259, 259, 258, 258, 258, - 258, 257, 257, 257, 257, 257, 256, 256, 256, - 256, 255, 255, 255, 255, 255, 254, 254, 254, - 254, 253, 253, 253, 253, 253, 252, 252, 252, - 252, 252, 251, 251, 251, 251, 250, 250, 250, - 250, 250, 249, 249, 249, 249, 248, 248, 248, - 248, 248, 247, 247, 247, 247, 247, 246, 246, - 246, 246, 245, 245, 245, 245, 245, 244, 244, - 244, 244, 243, 243, 243, 243, 243, 242, 242, - 242, 242, 242, 241, 241, 241, 241, 240, 240, - 240, 240, 240, 239, 239, 239, 239, 238, 238, - 238, 238, 238, 237, 237, 237, 237, 237, 236, - 236, 236, 236, 235, 235, 235, 235, 235, 234, - 234, 234, 234, 233, 233, 233, 233, 233, 232, - 232, 232, 232, 232, 231, 231, 231, 231, 230, - 230, 230, 230, 230, 229, 229, 229, 229, 229, - 228, 228, 228, 228, 227, 227, 227, 227, 227, - 226, 226, 226, 226, 226, 225, 225, 225, 225, - 224, 224, 224, 224, 224, 223, 223, 223, 223, - 222, 222, 222, 222, 222, 221, 221, 221, 221, - 221, 220, 220, 220, 220, 219, 219, 219, 219, - 219, 218, 218, 218, 218, 218, 217, 217, 217, - 217, 216, 216, 216, 216, 216, 215, 215, 215, - 215, 215, 214, 214, 214, 214, 213, 213, 213, - 213, 213, 212, 212, 212, 212, 212, 211, 211, - 211, 211, 210, 210, 210, 210, 210, 209, 209, - 209, 209, 209, 208, 208, 208, 208, 207, 207, - 207, 207, 207, 206, 206, 206, 206, 206, 205, - 205, 205, 205, 204, 204, 204, 204, 204, 203, - 203, 203, 203, 203, 202, 202, 202, 202, 201, - 201, 201, 201, 201, 200, 200, 200, 200, 200, - 199, 199, 199, 199, 198, 198, 198, 198, 198, - 197, 197, 197, 197, 197, 196, 196, 196, 196, - 195, 195, 195, 195, 195, 194, 194, 194, 194, - 194, 193, 193, 193, 193, 193, 192, 192, 192, - 192, 191, 191, 191, 191, 191, 190, 190, 190, - 190, 190, 189, 189, 189, 189, 188, 188, 188, - 188, 188, 187, 187, 187, 187, 187, 186, 186, - 186, 186, 185, 185, 185, 185, 185, 184, 184, - 184, 184, 184, 183, 183, 183, 183, 182, 182, - 182, 182, 182, 181, 181, 181, 181, 181, 180, - 180, 180, 180, 179, 179, 179, 179, 179, 178, - 178, 178, 178, 178, 177, 177, 177, 177, 176, - 176, 176, 176, 176, 175, 175, 175, 175, 175, - 174, 174, 174, 174, 173, 173, 173, 173, 173, - 172, 172, 172, 172, 172, 171, 171, 171, 171, - 170, 170, 170, 170, 170, 169, 169, 169, 169, - 169, 168, 168, 168, 168, 168, 167, 167, 167, - 167, 166, 166, 166, 166, 166, 165, 165, 165, - 165, 165, 164, 164, 164, 164, 163, 163, 163, - 163, 163, 162, 162, 162, 162, 162, 161, 161, - 161, 161, 160, 160, 160, 160, 160, 159, 159, - 159, 159, 159, 158, 158, 158, 158, 157, 157, - 157, 157, 157, 156, 156, 156, 156, 156, 155, - 155, 155, 155, 154, 154, 154, 154, 154, 153, - 153, 153, 153, 152, 152, 152, 152, 152, 151, - 151, 151, 151, 151, 150, 150, 150, 150, 149, - 149, 149, 149, 149, 148, 148, 148, 148, 148, - 147, 147, 147, 147, 146, 146, 146, 146, 146, - 145, 145, 145, 145, 145, 144, 144, 144, 144, - 143, 143, 143, 143, 143, 142, 142, 142, 142, - 142, 141, 141, 141, 141, 140, 140, 140, 140, - 140, 139, 139, 139, 139, 138, 138, 138, 138, - 138, 137, 137, 137, 137, 137, 136, 136, 136, - 136, 135, 135, 135, 135, 135, 134, 134, 134, - 134, 134, 133, 133, 133, 133, 132, 132, 132, - 132, 132, 131, 131, 131, 131, 130, 130, 130, - 130, 130, 129, 129, 129, 129, 129, 128, 128, - 128, 128, 127, 127, 127, 127, 127, 126, 126, - 126, 126, 125, 125, 125, 125, 125, 124, 124, - 124, 124, 124, 123, 123, 123, 123, 122, 122, - 122, 122, 122, 121, 121, 121, 121, 120, 120, - 120, 120, 120, 119, 119, 119, 119, 118, 118, - 118, 118, 118, 117, 117, 117, 117, 117, 116, - 116, 116, 116, 115, 115, 115, 115, 115, 114, - 114, 114, 114, 113, 113, 113, 113, 113, 112, - 112, 112, 112, 111, 111, 111, 111, 111, 110, - 110, 110, 110, 109, 109, 109, 109, 109, 108, - 108, 108, 108, 107, 107, 107, 107, 107, 106, - 106, 106, 106, 105, 105, 105, 105, 105, 104, - 104, 104, 104, 104, 103, 103, 103, 103, 102, - 102, 102, 102, 102, 101, 101, 101, 101, 100, - 100, 100, 100, 100, 99, 99, 99, 99, 98, 98, - 98, 98, 97, 97, 97, 97, 97, 96, 96, 96, 96, - 95, 95, 95, 95, 95, 94, 94, 94, 94, 93, 93, - 93, 93, 93, 92, 92, 92, 92, 91, 91, 91, 91, - 91, 90, 90, 90, 90, 89, 89, 89, 89, 89, 88, - 88, 88, 88, 87, 87, 87, 87, 87, 86, 86, 86, - 86, 85, 85, 85, 85, 84, 84, 84, 84, 84, 83, - 83, 83, 83, 82, 82, 82, 82, 82, 81, 81, 81, - 81, 80, 80, 80, 80, 79, 79, 79, 79, 79, 78, - 78, 78, 78, 77, 77, 77, 77, 77, 76, 76, 76, - 76, 75, 75, 75, 75, 74, 74, 74, 74, 74, 73, - 73, 73, 73, 72, 72, 72, 72, 71, 71, 71, 71, - 71, 70, 70, 70, 70, 69, 69, 69, 69, 68, 68, - 68, 68, 68, 67, 67, 67, 67, 66, 66, 66, 66, - 65, 65, 65, 65, 65, 64, 64, 64, 64, 63, 63, - 63, 63, 62, 62, 62, 62, 62, 61, 61, 61, 61, - 60, 60, 60, 60, 59, 59, 59, 59, 58, 58, 58, - 58, 58, 57, 57, 57, 57, 56, 56, 56, 56, 55, - 55, 55, 55, 54, 54, 54, 54, 54, 53, 53, 53, - 53, 52, 52, 52, 52, 51, 51, 51, 51, 50, 50, - 50, 50, 50, 49, 49, 49, 49, 48, 48, 48, 48, - 47, 47, 47, 47, 46, 46, 46, 46, 45, 45, 45, - 45, 45, 44, 44, 44, 44, 43, 43, 43, 43, 42, - 42, 42, 42, 41, 41, 41, 41, 40, 40, 40, 40, - 39, 39, 39, 39, 39, 38, 38, 38, 38, 37, 37, - 37, 37, 36, 36, 36, 36, 35, 35, 35, 35, 34, - 34, 34, 34, 33, 33, 33, 33, 32, 32, 32, 32, - 31, 31, 31, 31, 30, 30, 30, 30, 29, 29, 29, - 29, 29, 28, 28, 28, 28, 27, 27, 27, 27, 26, - 26, 26, 26, 25, 25, 25, 25, 24, 24, 24, 24, - 23, 23, 23, 23, 22, 22, 22, 22, 21, 21, 21, - 21, 20, 20, 20, 20, 19, 19, 19, 19, 18, 18, - 18, 18, 17, 17, 17, 17, 16, 16, 16, 16, 15, - 15, 15, 15, 14, 14, 14, 14, 13, 13, 13, 13, - 12, 12, 12, 12, 11, 11, 11, 10, 10, 10, 10, - 9, 9, 9, 9, 8, 8, 8, 8, 7, 7, 7, 7, 6, 6, - 6, 6, 5, 5, 5, 5, 4, 4, 4, 4, 3, 3, 3, 3, - 2, 2, 2, 1, 1, 1, 1, 0, 0, 0, 0, -1, -1, - -1, -1, -2, -2, -2, -2, -3, -3, -3, -3, -4, - -4, -4, -5, -5, -5, -5, -6, -6, -6, -6, -7, - -7, -7, -7, -8, -8, -8, -8, -9, -9, -9, -10, - -10, -10, -10, -11, -11, -11, -11, -12, -12, - -12, -12, -13, -13, -13, -14, -14, -14, -14, - -15, -15, -15, -15, -16, -16, -16, -17, -17, - -17, -17, -18, -18, -18, -18, -19, -19, -19, - -19, -20, -20, -20, -21, -21, -21, -21, -22, - -22, -22, -22, -23, -23, -23, -24, -24, -24, - -24, -25, -25, -25, -26, -26, -26, -26, -27, - -27, -27, -27, -28, -28, -28, -29, -29, -29, - -29, -30, -30, -30, -31, -31, -31, -31, -32, - -32, -32, -32, -33, -33, -33, -34, -34, -34, - -34, -35, -35, -35, -36, -36, -36, -36, -37, - -37, -37, -38, -38, -38, -38, -39, -39, -39, - -40, -40, -40, -40, -41, -41, -41, -42, -42, - -42, -42, -43, -43, -43, -44, -44, -44, -44, - -45, -45, -45, -46, -46, -46, -47, -47, -47, - -47, -48, -48, -48, -49, -49, -49, -49, -50, - -50, -50, -51, -51, -51, -52, -52, -52, -52, - -53, -53, -53, -54, -54, -54, -54, -55, -55, - -55, -56, -56, -56, -57, -57, -57, -57, -58, - -58, -58, -59, -59, -59, -60, -60, -60, -61, - -61, -61, -61, -62, -62, -62, -63, -63, -63, - -64, -64, -64, -64, -65, -65, -65, -66, -66, - -66, -67, -67, -67, -68, -68, -68, -68, -69, - -69, -69, -70, -70, -70, -71, -71, -71, -72, - -72, -72, -73, -73, -73, -73, -74, -74, -74, - -75, -75, -75, -76, -76, -76, -77, -77, -77, - -78, -78, -78, -79, -79, -79, -80, -80, -80, - -80, -81, -81, -81, -82, -82, -82, -83, -83, - -83, -84, -84, -84, -85, -85, -85, -86, -86, - -86, -87, -87, -87, -88, -88, -88, -89, -89, - -89, -90, -90, -90, -91, -91, -91, -92, -92, - -92, -93, -93, -93, -94, -94, -94, -95, -95, - -95, -96, -96, -96, -97, -97, -97, -98, -98, - -98, -99, -99, -99, -100, -100, -100, -101, - -101, -101, -102, -102, -103, -103, -103, - -104, -104, -104, -105, -105, -105, -106, - -106, -106, -107, -107, -107, -108, -108, - -108, -109, -109, -109, -110, -110, -111, - -111, -111, -112, -112, -112, -113, -113, - -113, -114, -114, -114, -115, -115, -116, - -116, -116, -117, -117, -117, -118, -118, - -118, -119, -119, -120, -120, -120, -121, - -121, -121, -122, -122, -123, -123, -123, - -124, -124, -124, -125, -125, -126, -126, - -126, -127, -127, -127, -128, -128, -129, - -129, -129, -130, -130, -130, -131, -131, - -132, -132, -132, -133, -133, -133, -134, - -134, -135, -135, -135, -136, -136, -137, - -137, -137, -138, -138, -139, -139, -139, - -140, -140, -141, -141, -141, -142, -142, - -142, -143, -143, -144, -144, -144, -145, - -145, -146, -146, -147, -147, -147, -148, - -148, -149, -149, -149, -150, -150, -151, - -151, -151, -152, -152, -153, -153, -153, - -154, -154, -155, -155, -156, -156, -156, - -157, -157, -158, -158, -159, -159, -159, - -160, -160, -161, -161, -162, -162, -162, - -163, -163, -164, -164, -165, -165, -165, - -166, -166, -167, -167, -168, -168, -168, - -169, -169, -170, -170, -171, -171, -172, - -172, -172, -173, -173, -174, -174, -175, - -175, -176, -176, -177, -177, -177, -178, - -178, -179, -179, -180, -180, -181, -181, - -182, -182, -183, -183, -183, -184, -184, - -185, -185, -186, -186, -187, -187, -188, - -188, -189, -189, -190, -190, -191, -191, - -192, -192, -193, -193, -194, -194, -195, - -195, -196, -196, -196, -197, -197, -198, - -198, -199, -199, -200, -201, -201, -202, - -202, -203, -203, -204, -204, -205, -205, - -206, -206, -207, -207, -208, -208, -209, - -209, -210, -210, -211, -211, -212, -212, - -213, -213, -214, -215, -215, -216, -216, - -217, -217, -218, -218, -219, -219, -220, - -221, -221, -222, -222, -223, -223, -224, - -224, -225, -226, -226, -227, -227, -228, - -228, -229, -230, -230, -231, -231, -232, - -232, -233, -234, -234, -235, -235, -236, - -237, -237, -238, -238, -239, -240, -240, - -241, -241, -242, -243, -243, -244, -244, - -245, -246, -246, -247, -248, -248, -249, - -249, -250, -251, -251, -252, -253, -253, - -254, -255, -255, -256, -256, -257, -258, - -258, -259, -260, -260, -261, -262, -262, - -263, -264, -265, -265, -266, -267, -267, - -268, -269, -269, -270, -271, -271, -272, - -273, -274, -274, -275, -276, -276, -277, - -278, -279, -279, -280, -281, -282, -282, - -283, -284, -285, -285, -286, -287, -288, - -288, -289, -290, -291, -291, -292, -293, - -294, -295, -295, -296, -297, -298, -299, - -299, -300, -301, -302, -303, -303, -304, - -305, -306, -307, -308, -309, -309, -310, - -311, -312, -313, -314, -315, -315, -316, - -317, -318, -319, -320, -321, -322, -323, - -324, -325, -326, -326, -327, -328, -329, - -330, -331, -332, -333, -334, -335, -336, - -337, -338, -339, -340, -341, -342, -343, - -344, -345, -346, -347, -348, -349, -351, - -352, -353, -354, -355, -356, -357, -358, - -359, -361, -362, -363, -364, -365, -366, - -368, -369, -370, -371, -372, -374, -375, - -376, -377, -379, -380, -381, -382, -384, - -385, -386, -388, -389, -390, -392, -393, - -394, -396, -397, -399, -400, -402, -403, - -405, -406, -408, -409, -411, -412, -414, - -415, -417, -418, -420, -422, -423, -425, - -427, -428, -430, -432, -434, -436, -437, - -439, -441, -443, -445, -447, -449, -451, - -453, -455, -457, -459, -461, -463, -465, - -467, -470, -472, -474, -477, -479, -481, - -484, -486, -489, -491, -494, -497, -499, - -502, -505, -508, -511, -514, -517, -520, - -523, -527, -530, -534, -537, -541, -544, - -548, -552, -556, -561, -565, -569, -574, - -579, -584, -589, -594, -600, -606, -612, - -618, -625, -632, -640, -648, -657, -666, - -677, -688, -700, -714, -729, -748, -770, - -797, -835, -873 -}; - float* value = nullptr; - ST_LIB::ADCDomain::Instance* adc = nullptr; + static constexpr int NTC_table[4096] = { + 6154, 5192, 4230, 3768, 3476, 3267, 3106, 2977, 2869, 2778, 2698, 2628, 2566, 2510, 2459, + 2412, 2370, 2330, 2293, 2259, 2227, 2197, 2169, 2142, 2116, 2092, 2069, 2047, 2026, 2006, + 1987, 1969, 1951, 1934, 1917, 1902, 1886, 1872, 1857, 1843, 1830, 1817, 1804, 1792, 1780, + 1769, 1757, 1746, 1736, 1725, 1715, 1705, 1695, 1686, 1676, 1667, 1659, 1650, 1641, 1633, + 1625, 1617, 1609, 1601, 1594, 1586, 1579, 1572, 1565, 1558, 1551, 1545, 1538, 1532, 1525, + 1519, 1513, 1507, 1501, 1495, 1490, 1484, 1478, 1473, 1467, 1462, 1457, 1451, 1446, 1441, + 1436, 1431, 1426, 1422, 1417, 1412, 1408, 1403, 1399, 1394, 1390, 1385, 1381, 1377, 1373, + 1368, 1364, 1360, 1356, 1352, 1348, 1344, 1341, 1337, 1333, 1329, 1326, 1322, 1318, 1315, + 1311, 1308, 1304, 1301, 1297, 1294, 1291, 1287, 1284, 1281, 1278, 1274, 1271, 1268, 1265, + 1262, 1259, 1256, 1253, 1250, 1247, 1244, 1241, 1238, 1235, 1232, 1230, 1227, 1224, 1221, + 1219, 1216, 1213, 1210, 1208, 1205, 1203, 1200, 1197, 1195, 1192, 1190, 1187, 1185, 1182, + 1180, 1178, 1175, 1173, 1170, 1168, 1166, 1163, 1161, 1159, 1157, 1154, 1152, 1150, 1148, + 1145, 1143, 1141, 1139, 1137, 1135, 1132, 1130, 1128, 1126, 1124, 1122, 1120, 1118, 1116, + 1114, 1112, 1110, 1108, 1106, 1104, 1102, 1100, 1098, 1096, 1094, 1093, 1091, 1089, 1087, + 1085, 1083, 1081, 1080, 1078, 1076, 1074, 1073, 1071, 1069, 1067, 1065, 1064, 1062, 1060, + 1059, 1057, 1055, 1054, 1052, 1050, 1049, 1047, 1045, 1044, 1042, 1040, 1039, 1037, 1036, + 1034, 1032, 1031, 1029, 1028, 1026, 1025, 1023, 1022, 1020, 1019, 1017, 1016, 1014, 1013, + 1011, 1010, 1008, 1007, 1005, 1004, 1002, 1001, 1000, 998, 997, 995, 994, 993, 991, + 990, 988, 987, 986, 984, 983, 982, 980, 979, 978, 976, 975, 974, 972, 971, + 970, 968, 967, 966, 965, 963, 962, 961, 959, 958, 957, 956, 954, 953, 952, + 951, 950, 948, 947, 946, 945, 944, 942, 941, 940, 939, 938, 936, 935, 934, + 933, 932, 931, 929, 928, 927, 926, 925, 924, 923, 921, 920, 919, 918, 917, + 916, 915, 914, 913, 912, 910, 909, 908, 907, 906, 905, 904, 903, 902, 901, + 900, 899, 898, 897, 896, 895, 894, 893, 891, 890, 889, 888, 887, 886, 885, + 884, 883, 882, 881, 880, 879, 878, 877, 876, 876, 875, 874, 873, 872, 871, + 870, 869, 868, 867, 866, 865, 864, 863, 862, 861, 860, 859, 858, 857, 857, + 856, 855, 854, 853, 852, 851, 850, 849, 848, 847, 847, 846, 845, 844, 843, + 842, 841, 840, 840, 839, 838, 837, 836, 835, 834, 833, 833, 832, 831, 830, + 829, 828, 828, 827, 826, 825, 824, 823, 823, 822, 821, 820, 819, 818, 818, + 817, 816, 815, 814, 814, 813, 812, 811, 810, 810, 809, 808, 807, 806, 806, + 805, 804, 803, 802, 802, 801, 800, 799, 799, 798, 797, 796, 795, 795, 794, + 793, 792, 792, 791, 790, 789, 789, 788, 787, 786, 786, 785, 784, 784, 783, + 782, 781, 781, 780, 779, 778, 778, 777, 776, 776, 775, 774, 773, 773, 772, + 771, 771, 770, 769, 768, 768, 767, 766, 766, 765, 764, 764, 763, 762, 761, + 761, 760, 759, 759, 758, 757, 757, 756, 755, 755, 754, 753, 753, 752, 751, + 751, 750, 749, 749, 748, 747, 747, 746, 745, 745, 744, 743, 743, 742, 741, + 741, 740, 739, 739, 738, 738, 737, 736, 736, 735, 734, 734, 733, 732, 732, + 731, 731, 730, 729, 729, 728, 727, 727, 726, 726, 725, 724, 724, 723, 723, + 722, 721, 721, 720, 719, 719, 718, 718, 717, 716, 716, 715, 715, 714, 713, + 713, 712, 712, 711, 710, 710, 709, 709, 708, 708, 707, 706, 706, 705, 705, + 704, 703, 703, 702, 702, 701, 701, 700, 699, 699, 698, 698, 697, 697, 696, + 695, 695, 694, 694, 693, 693, 692, 692, 691, 690, 690, 689, 689, 688, 688, + 687, 687, 686, 685, 685, 684, 684, 683, 683, 682, 682, 681, 681, 680, 679, + 679, 678, 678, 677, 677, 676, 676, 675, 675, 674, 674, 673, 673, 672, 671, + 671, 670, 670, 669, 669, 668, 668, 667, 667, 666, 666, 665, 665, 664, 664, + 663, 663, 662, 662, 661, 661, 660, 660, 659, 659, 658, 658, 657, 657, 656, + 656, 655, 655, 654, 654, 653, 653, 652, 652, 651, 651, 650, 650, 649, 649, + 648, 648, 647, 647, 646, 646, 645, 645, 644, 644, 643, 643, 642, 642, 641, + 641, 640, 640, 639, 639, 638, 638, 637, 637, 636, 636, 635, 635, 635, 634, + 634, 633, 633, 632, 632, 631, 631, 630, 630, 629, 629, 628, 628, 628, 627, + 627, 626, 626, 625, 625, 624, 624, 623, 623, 622, 622, 622, 621, 621, 620, + 620, 619, 619, 618, 618, 617, 617, 617, 616, 616, 615, 615, 614, 614, 613, + 613, 613, 612, 612, 611, 611, 610, 610, 609, 609, 609, 608, 608, 607, 607, + 606, 606, 605, 605, 605, 604, 604, 603, 603, 602, 602, 602, 601, 601, 600, + 600, 599, 599, 599, 598, 598, 597, 597, 596, 596, 596, 595, 595, 594, 594, + 593, 593, 593, 592, 592, 591, 591, 591, 590, 590, 589, 589, 588, 588, 588, + 587, 587, 586, 586, 586, 585, 585, 584, 584, 583, 583, 583, 582, 582, 581, + 581, 581, 580, 580, 579, 579, 579, 578, 578, 577, 577, 577, 576, 576, 575, + 575, 575, 574, 574, 573, 573, 573, 572, 572, 571, 571, 571, 570, 570, 569, + 569, 569, 568, 568, 567, 567, 567, 566, 566, 565, 565, 565, 564, 564, 563, + 563, 563, 562, 562, 562, 561, 561, 560, 560, 560, 559, 559, 558, 558, 558, + 557, 557, 557, 556, 556, 555, 555, 555, 554, 554, 553, 553, 553, 552, 552, + 552, 551, 551, 550, 550, 550, 549, 549, 549, 548, 548, 547, 547, 547, 546, + 546, 546, 545, 545, 544, 544, 544, 543, 543, 543, 542, 542, 542, 541, 541, + 540, 540, 540, 539, 539, 539, 538, 538, 538, 537, 537, 536, 536, 536, 535, + 535, 535, 534, 534, 534, 533, 533, 532, 532, 532, 531, 531, 531, 530, 530, + 530, 529, 529, 529, 528, 528, 527, 527, 527, 526, 526, 526, 525, 525, 525, + 524, 524, 524, 523, 523, 523, 522, 522, 521, 521, 521, 520, 520, 520, 519, + 519, 519, 518, 518, 518, 517, 517, 517, 516, 516, 516, 515, 515, 515, 514, + 514, 514, 513, 513, 513, 512, 512, 511, 511, 511, 510, 510, 510, 509, 509, + 509, 508, 508, 508, 507, 507, 507, 506, 506, 506, 505, 505, 505, 504, 504, + 504, 503, 503, 503, 502, 502, 502, 501, 501, 501, 500, 500, 500, 499, 499, + 499, 498, 498, 498, 497, 497, 497, 496, 496, 496, 495, 495, 495, 494, 494, + 494, 494, 493, 493, 493, 492, 492, 492, 491, 491, 491, 490, 490, 490, 489, + 489, 489, 488, 488, 488, 487, 487, 487, 486, 486, 486, 485, 485, 485, 484, + 484, 484, 484, 483, 483, 483, 482, 482, 482, 481, 481, 481, 480, 480, 480, + 479, 479, 479, 478, 478, 478, 478, 477, 477, 477, 476, 476, 476, 475, 475, + 475, 474, 474, 474, 473, 473, 473, 473, 472, 472, 472, 471, 471, 471, 470, + 470, 470, 469, 469, 469, 469, 468, 468, 468, 467, 467, 467, 466, 466, 466, + 465, 465, 465, 465, 464, 464, 464, 463, 463, 463, 462, 462, 462, 462, 461, + 461, 461, 460, 460, 460, 459, 459, 459, 458, 458, 458, 458, 457, 457, 457, + 456, 456, 456, 455, 455, 455, 455, 454, 454, 454, 453, 453, 453, 453, 452, + 452, 452, 451, 451, 451, 450, 450, 450, 450, 449, 449, 449, 448, 448, 448, + 448, 447, 447, 447, 446, 446, 446, 445, 445, 445, 445, 444, 444, 444, 443, + 443, 443, 443, 442, 442, 442, 441, 441, 441, 441, 440, 440, 440, 439, 439, + 439, 439, 438, 438, 438, 437, 437, 437, 436, 436, 436, 436, 435, 435, 435, + 434, 434, 434, 434, 433, 433, 433, 433, 432, 432, 432, 431, 431, 431, 431, + 430, 430, 430, 429, 429, 429, 429, 428, 428, 428, 427, 427, 427, 427, 426, + 426, 426, 425, 425, 425, 425, 424, 424, 424, 423, 423, 423, 423, 422, 422, + 422, 422, 421, 421, 421, 420, 420, 420, 420, 419, 419, 419, 419, 418, 418, + 418, 417, 417, 417, 417, 416, 416, 416, 415, 415, 415, 415, 414, 414, 414, + 414, 413, 413, 413, 412, 412, 412, 412, 411, 411, 411, 411, 410, 410, 410, + 409, 409, 409, 409, 408, 408, 408, 408, 407, 407, 407, 407, 406, 406, 406, + 405, 405, 405, 405, 404, 404, 404, 404, 403, 403, 403, 402, 402, 402, 402, + 401, 401, 401, 401, 400, 400, 400, 400, 399, 399, 399, 398, 398, 398, 398, + 397, 397, 397, 397, 396, 396, 396, 396, 395, 395, 395, 395, 394, 394, 394, + 393, 393, 393, 393, 392, 392, 392, 392, 391, 391, 391, 391, 390, 390, 390, + 390, 389, 389, 389, 388, 388, 388, 388, 387, 387, 387, 387, 386, 386, 386, + 386, 385, 385, 385, 385, 384, 384, 384, 384, 383, 383, 383, 383, 382, 382, + 382, 381, 381, 381, 381, 380, 380, 380, 380, 379, 379, 379, 379, 378, 378, + 378, 378, 377, 377, 377, 377, 376, 376, 376, 376, 375, 375, 375, 375, 374, + 374, 374, 374, 373, 373, 373, 373, 372, 372, 372, 372, 371, 371, 371, 371, + 370, 370, 370, 370, 369, 369, 369, 369, 368, 368, 368, 367, 367, 367, 367, + 366, 366, 366, 366, 365, 365, 365, 365, 364, 364, 364, 364, 363, 363, 363, + 363, 362, 362, 362, 362, 361, 361, 361, 361, 361, 360, 360, 360, 360, 359, + 359, 359, 359, 358, 358, 358, 358, 357, 357, 357, 357, 356, 356, 356, 356, + 355, 355, 355, 355, 354, 354, 354, 354, 353, 353, 353, 353, 352, 352, 352, + 352, 351, 351, 351, 351, 350, 350, 350, 350, 349, 349, 349, 349, 348, 348, + 348, 348, 347, 347, 347, 347, 347, 346, 346, 346, 346, 345, 345, 345, 345, + 344, 344, 344, 344, 343, 343, 343, 343, 342, 342, 342, 342, 341, 341, 341, + 341, 340, 340, 340, 340, 339, 339, 339, 339, 339, 338, 338, 338, 338, 337, + 337, 337, 337, 336, 336, 336, 336, 335, 335, 335, 335, 334, 334, 334, 334, + 334, 333, 333, 333, 333, 332, 332, 332, 332, 331, 331, 331, 331, 330, 330, + 330, 330, 329, 329, 329, 329, 329, 328, 328, 328, 328, 327, 327, 327, 327, + 326, 326, 326, 326, 325, 325, 325, 325, 325, 324, 324, 324, 324, 323, 323, + 323, 323, 322, 322, 322, 322, 321, 321, 321, 321, 321, 320, 320, 320, 320, + 319, 319, 319, 319, 318, 318, 318, 318, 317, 317, 317, 317, 317, 316, 316, + 316, 316, 315, 315, 315, 315, 314, 314, 314, 314, 314, 313, 313, 313, 313, + 312, 312, 312, 312, 311, 311, 311, 311, 310, 310, 310, 310, 310, 309, 309, + 309, 309, 308, 308, 308, 308, 307, 307, 307, 307, 307, 306, 306, 306, 306, + 305, 305, 305, 305, 305, 304, 304, 304, 304, 303, 303, 303, 303, 302, 302, + 302, 302, 302, 301, 301, 301, 301, 300, 300, 300, 300, 299, 299, 299, 299, + 299, 298, 298, 298, 298, 297, 297, 297, 297, 297, 296, 296, 296, 296, 295, + 295, 295, 295, 294, 294, 294, 294, 294, 293, 293, 293, 293, 292, 292, 292, + 292, 292, 291, 291, 291, 291, 290, 290, 290, 290, 289, 289, 289, 289, 289, + 288, 288, 288, 288, 287, 287, 287, 287, 287, 286, 286, 286, 286, 285, 285, + 285, 285, 285, 284, 284, 284, 284, 283, 283, 283, 283, 283, 282, 282, 282, + 282, 281, 281, 281, 281, 280, 280, 280, 280, 280, 279, 279, 279, 279, 278, + 278, 278, 278, 278, 277, 277, 277, 277, 276, 276, 276, 276, 276, 275, 275, + 275, 275, 274, 274, 274, 274, 274, 273, 273, 273, 273, 272, 272, 272, 272, + 272, 271, 271, 271, 271, 270, 270, 270, 270, 270, 269, 269, 269, 269, 268, + 268, 268, 268, 268, 267, 267, 267, 267, 266, 266, 266, 266, 266, 265, 265, + 265, 265, 264, 264, 264, 264, 264, 263, 263, 263, 263, 263, 262, 262, 262, + 262, 261, 261, 261, 261, 261, 260, 260, 260, 260, 259, 259, 259, 259, 259, + 258, 258, 258, 258, 257, 257, 257, 257, 257, 256, 256, 256, 256, 255, 255, + 255, 255, 255, 254, 254, 254, 254, 253, 253, 253, 253, 253, 252, 252, 252, + 252, 252, 251, 251, 251, 251, 250, 250, 250, 250, 250, 249, 249, 249, 249, + 248, 248, 248, 248, 248, 247, 247, 247, 247, 247, 246, 246, 246, 246, 245, + 245, 245, 245, 245, 244, 244, 244, 244, 243, 243, 243, 243, 243, 242, 242, + 242, 242, 242, 241, 241, 241, 241, 240, 240, 240, 240, 240, 239, 239, 239, + 239, 238, 238, 238, 238, 238, 237, 237, 237, 237, 237, 236, 236, 236, 236, + 235, 235, 235, 235, 235, 234, 234, 234, 234, 233, 233, 233, 233, 233, 232, + 232, 232, 232, 232, 231, 231, 231, 231, 230, 230, 230, 230, 230, 229, 229, + 229, 229, 229, 228, 228, 228, 228, 227, 227, 227, 227, 227, 226, 226, 226, + 226, 226, 225, 225, 225, 225, 224, 224, 224, 224, 224, 223, 223, 223, 223, + 222, 222, 222, 222, 222, 221, 221, 221, 221, 221, 220, 220, 220, 220, 219, + 219, 219, 219, 219, 218, 218, 218, 218, 218, 217, 217, 217, 217, 216, 216, + 216, 216, 216, 215, 215, 215, 215, 215, 214, 214, 214, 214, 213, 213, 213, + 213, 213, 212, 212, 212, 212, 212, 211, 211, 211, 211, 210, 210, 210, 210, + 210, 209, 209, 209, 209, 209, 208, 208, 208, 208, 207, 207, 207, 207, 207, + 206, 206, 206, 206, 206, 205, 205, 205, 205, 204, 204, 204, 204, 204, 203, + 203, 203, 203, 203, 202, 202, 202, 202, 201, 201, 201, 201, 201, 200, 200, + 200, 200, 200, 199, 199, 199, 199, 198, 198, 198, 198, 198, 197, 197, 197, + 197, 197, 196, 196, 196, 196, 195, 195, 195, 195, 195, 194, 194, 194, 194, + 194, 193, 193, 193, 193, 193, 192, 192, 192, 192, 191, 191, 191, 191, 191, + 190, 190, 190, 190, 190, 189, 189, 189, 189, 188, 188, 188, 188, 188, 187, + 187, 187, 187, 187, 186, 186, 186, 186, 185, 185, 185, 185, 185, 184, 184, + 184, 184, 184, 183, 183, 183, 183, 182, 182, 182, 182, 182, 181, 181, 181, + 181, 181, 180, 180, 180, 180, 179, 179, 179, 179, 179, 178, 178, 178, 178, + 178, 177, 177, 177, 177, 176, 176, 176, 176, 176, 175, 175, 175, 175, 175, + 174, 174, 174, 174, 173, 173, 173, 173, 173, 172, 172, 172, 172, 172, 171, + 171, 171, 171, 170, 170, 170, 170, 170, 169, 169, 169, 169, 169, 168, 168, + 168, 168, 168, 167, 167, 167, 167, 166, 166, 166, 166, 166, 165, 165, 165, + 165, 165, 164, 164, 164, 164, 163, 163, 163, 163, 163, 162, 162, 162, 162, + 162, 161, 161, 161, 161, 160, 160, 160, 160, 160, 159, 159, 159, 159, 159, + 158, 158, 158, 158, 157, 157, 157, 157, 157, 156, 156, 156, 156, 156, 155, + 155, 155, 155, 154, 154, 154, 154, 154, 153, 153, 153, 153, 152, 152, 152, + 152, 152, 151, 151, 151, 151, 151, 150, 150, 150, 150, 149, 149, 149, 149, + 149, 148, 148, 148, 148, 148, 147, 147, 147, 147, 146, 146, 146, 146, 146, + 145, 145, 145, 145, 145, 144, 144, 144, 144, 143, 143, 143, 143, 143, 142, + 142, 142, 142, 142, 141, 141, 141, 141, 140, 140, 140, 140, 140, 139, 139, + 139, 139, 138, 138, 138, 138, 138, 137, 137, 137, 137, 137, 136, 136, 136, + 136, 135, 135, 135, 135, 135, 134, 134, 134, 134, 134, 133, 133, 133, 133, + 132, 132, 132, 132, 132, 131, 131, 131, 131, 130, 130, 130, 130, 130, 129, + 129, 129, 129, 129, 128, 128, 128, 128, 127, 127, 127, 127, 127, 126, 126, + 126, 126, 125, 125, 125, 125, 125, 124, 124, 124, 124, 124, 123, 123, 123, + 123, 122, 122, 122, 122, 122, 121, 121, 121, 121, 120, 120, 120, 120, 120, + 119, 119, 119, 119, 118, 118, 118, 118, 118, 117, 117, 117, 117, 117, 116, + 116, 116, 116, 115, 115, 115, 115, 115, 114, 114, 114, 114, 113, 113, 113, + 113, 113, 112, 112, 112, 112, 111, 111, 111, 111, 111, 110, 110, 110, 110, + 109, 109, 109, 109, 109, 108, 108, 108, 108, 107, 107, 107, 107, 107, 106, + 106, 106, 106, 105, 105, 105, 105, 105, 104, 104, 104, 104, 104, 103, 103, + 103, 103, 102, 102, 102, 102, 102, 101, 101, 101, 101, 100, 100, 100, 100, + 100, 99, 99, 99, 99, 98, 98, 98, 98, 97, 97, 97, 97, 97, 96, + 96, 96, 96, 95, 95, 95, 95, 95, 94, 94, 94, 94, 93, 93, 93, + 93, 93, 92, 92, 92, 92, 91, 91, 91, 91, 91, 90, 90, 90, 90, + 89, 89, 89, 89, 89, 88, 88, 88, 88, 87, 87, 87, 87, 87, 86, + 86, 86, 86, 85, 85, 85, 85, 84, 84, 84, 84, 84, 83, 83, 83, + 83, 82, 82, 82, 82, 82, 81, 81, 81, 81, 80, 80, 80, 80, 79, + 79, 79, 79, 79, 78, 78, 78, 78, 77, 77, 77, 77, 77, 76, 76, + 76, 76, 75, 75, 75, 75, 74, 74, 74, 74, 74, 73, 73, 73, 73, + 72, 72, 72, 72, 71, 71, 71, 71, 71, 70, 70, 70, 70, 69, 69, + 69, 69, 68, 68, 68, 68, 68, 67, 67, 67, 67, 66, 66, 66, 66, + 65, 65, 65, 65, 65, 64, 64, 64, 64, 63, 63, 63, 63, 62, 62, + 62, 62, 62, 61, 61, 61, 61, 60, 60, 60, 60, 59, 59, 59, 59, + 58, 58, 58, 58, 58, 57, 57, 57, 57, 56, 56, 56, 56, 55, 55, + 55, 55, 54, 54, 54, 54, 54, 53, 53, 53, 53, 52, 52, 52, 52, + 51, 51, 51, 51, 50, 50, 50, 50, 50, 49, 49, 49, 49, 48, 48, + 48, 48, 47, 47, 47, 47, 46, 46, 46, 46, 45, 45, 45, 45, 45, + 44, 44, 44, 44, 43, 43, 43, 43, 42, 42, 42, 42, 41, 41, 41, + 41, 40, 40, 40, 40, 39, 39, 39, 39, 39, 38, 38, 38, 38, 37, + 37, 37, 37, 36, 36, 36, 36, 35, 35, 35, 35, 34, 34, 34, 34, + 33, 33, 33, 33, 32, 32, 32, 32, 31, 31, 31, 31, 30, 30, 30, + 30, 29, 29, 29, 29, 29, 28, 28, 28, 28, 27, 27, 27, 27, 26, + 26, 26, 26, 25, 25, 25, 25, 24, 24, 24, 24, 23, 23, 23, 23, + 22, 22, 22, 22, 21, 21, 21, 21, 20, 20, 20, 20, 19, 19, 19, + 19, 18, 18, 18, 18, 17, 17, 17, 17, 16, 16, 16, 16, 15, 15, + 15, 15, 14, 14, 14, 14, 13, 13, 13, 13, 12, 12, 12, 12, 11, + 11, 11, 10, 10, 10, 10, 9, 9, 9, 9, 8, 8, 8, 8, 7, + 7, 7, 7, 6, 6, 6, 6, 5, 5, 5, 5, 4, 4, 4, 4, + 3, 3, 3, 3, 2, 2, 2, 1, 1, 1, 1, 0, 0, 0, 0, + -1, -1, -1, -1, -2, -2, -2, -2, -3, -3, -3, -3, -4, -4, -4, + -5, -5, -5, -5, -6, -6, -6, -6, -7, -7, -7, -7, -8, -8, -8, + -8, -9, -9, -9, -10, -10, -10, -10, -11, -11, -11, -11, -12, -12, -12, + -12, -13, -13, -13, -14, -14, -14, -14, -15, -15, -15, -15, -16, -16, -16, + -17, -17, -17, -17, -18, -18, -18, -18, -19, -19, -19, -19, -20, -20, -20, + -21, -21, -21, -21, -22, -22, -22, -22, -23, -23, -23, -24, -24, -24, -24, + -25, -25, -25, -26, -26, -26, -26, -27, -27, -27, -27, -28, -28, -28, -29, + -29, -29, -29, -30, -30, -30, -31, -31, -31, -31, -32, -32, -32, -32, -33, + -33, -33, -34, -34, -34, -34, -35, -35, -35, -36, -36, -36, -36, -37, -37, + -37, -38, -38, -38, -38, -39, -39, -39, -40, -40, -40, -40, -41, -41, -41, + -42, -42, -42, -42, -43, -43, -43, -44, -44, -44, -44, -45, -45, -45, -46, + -46, -46, -47, -47, -47, -47, -48, -48, -48, -49, -49, -49, -49, -50, -50, + -50, -51, -51, -51, -52, -52, -52, -52, -53, -53, -53, -54, -54, -54, -54, + -55, -55, -55, -56, -56, -56, -57, -57, -57, -57, -58, -58, -58, -59, -59, + -59, -60, -60, -60, -61, -61, -61, -61, -62, -62, -62, -63, -63, -63, -64, + -64, -64, -64, -65, -65, -65, -66, -66, -66, -67, -67, -67, -68, -68, -68, + -68, -69, -69, -69, -70, -70, -70, -71, -71, -71, -72, -72, -72, -73, -73, + -73, -73, -74, -74, -74, -75, -75, -75, -76, -76, -76, -77, -77, -77, -78, + -78, -78, -79, -79, -79, -80, -80, -80, -80, -81, -81, -81, -82, -82, -82, + -83, -83, -83, -84, -84, -84, -85, -85, -85, -86, -86, -86, -87, -87, -87, + -88, -88, -88, -89, -89, -89, -90, -90, -90, -91, -91, -91, -92, -92, -92, + -93, -93, -93, -94, -94, -94, -95, -95, -95, -96, -96, -96, -97, -97, -97, + -98, -98, -98, -99, -99, -99, -100, -100, -100, -101, -101, -101, -102, -102, -103, + -103, -103, -104, -104, -104, -105, -105, -105, -106, -106, -106, -107, -107, -107, -108, + -108, -108, -109, -109, -109, -110, -110, -111, -111, -111, -112, -112, -112, -113, -113, + -113, -114, -114, -114, -115, -115, -116, -116, -116, -117, -117, -117, -118, -118, -118, + -119, -119, -120, -120, -120, -121, -121, -121, -122, -122, -123, -123, -123, -124, -124, + -124, -125, -125, -126, -126, -126, -127, -127, -127, -128, -128, -129, -129, -129, -130, + -130, -130, -131, -131, -132, -132, -132, -133, -133, -133, -134, -134, -135, -135, -135, + -136, -136, -137, -137, -137, -138, -138, -139, -139, -139, -140, -140, -141, -141, -141, + -142, -142, -142, -143, -143, -144, -144, -144, -145, -145, -146, -146, -147, -147, -147, + -148, -148, -149, -149, -149, -150, -150, -151, -151, -151, -152, -152, -153, -153, -153, + -154, -154, -155, -155, -156, -156, -156, -157, -157, -158, -158, -159, -159, -159, -160, + -160, -161, -161, -162, -162, -162, -163, -163, -164, -164, -165, -165, -165, -166, -166, + -167, -167, -168, -168, -168, -169, -169, -170, -170, -171, -171, -172, -172, -172, -173, + -173, -174, -174, -175, -175, -176, -176, -177, -177, -177, -178, -178, -179, -179, -180, + -180, -181, -181, -182, -182, -183, -183, -183, -184, -184, -185, -185, -186, -186, -187, + -187, -188, -188, -189, -189, -190, -190, -191, -191, -192, -192, -193, -193, -194, -194, + -195, -195, -196, -196, -196, -197, -197, -198, -198, -199, -199, -200, -201, -201, -202, + -202, -203, -203, -204, -204, -205, -205, -206, -206, -207, -207, -208, -208, -209, -209, + -210, -210, -211, -211, -212, -212, -213, -213, -214, -215, -215, -216, -216, -217, -217, + -218, -218, -219, -219, -220, -221, -221, -222, -222, -223, -223, -224, -224, -225, -226, + -226, -227, -227, -228, -228, -229, -230, -230, -231, -231, -232, -232, -233, -234, -234, + -235, -235, -236, -237, -237, -238, -238, -239, -240, -240, -241, -241, -242, -243, -243, + -244, -244, -245, -246, -246, -247, -248, -248, -249, -249, -250, -251, -251, -252, -253, + -253, -254, -255, -255, -256, -256, -257, -258, -258, -259, -260, -260, -261, -262, -262, + -263, -264, -265, -265, -266, -267, -267, -268, -269, -269, -270, -271, -271, -272, -273, + -274, -274, -275, -276, -276, -277, -278, -279, -279, -280, -281, -282, -282, -283, -284, + -285, -285, -286, -287, -288, -288, -289, -290, -291, -291, -292, -293, -294, -295, -295, + -296, -297, -298, -299, -299, -300, -301, -302, -303, -303, -304, -305, -306, -307, -308, + -309, -309, -310, -311, -312, -313, -314, -315, -315, -316, -317, -318, -319, -320, -321, + -322, -323, -324, -325, -326, -326, -327, -328, -329, -330, -331, -332, -333, -334, -335, + -336, -337, -338, -339, -340, -341, -342, -343, -344, -345, -346, -347, -348, -349, -351, + -352, -353, -354, -355, -356, -357, -358, -359, -361, -362, -363, -364, -365, -366, -368, + -369, -370, -371, -372, -374, -375, -376, -377, -379, -380, -381, -382, -384, -385, -386, + -388, -389, -390, -392, -393, -394, -396, -397, -399, -400, -402, -403, -405, -406, -408, + -409, -411, -412, -414, -415, -417, -418, -420, -422, -423, -425, -427, -428, -430, -432, + -434, -436, -437, -439, -441, -443, -445, -447, -449, -451, -453, -455, -457, -459, -461, + -463, -465, -467, -470, -472, -474, -477, -479, -481, -484, -486, -489, -491, -494, -497, + -499, -502, -505, -508, -511, -514, -517, -520, -523, -527, -530, -534, -537, -541, -544, + -548, -552, -556, -561, -565, -569, -574, -579, -584, -589, -594, -600, -606, -612, -618, + -625, -632, -640, -648, -657, -666, -677, -688, -700, -714, -729, -748, -770, -797, -835, + -873 + }; + float* value = nullptr; + ST_LIB::ADCDomain::Instance* adc = nullptr; }; diff --git a/Inc/ST-LIB_LOW/Sensors/PWMSensor/PWMSensor.hpp b/Inc/ST-LIB_LOW/Sensors/PWMSensor/PWMSensor.hpp index 4b929973a..4e6c00ce0 100644 --- a/Inc/ST-LIB_LOW/Sensors/PWMSensor/PWMSensor.hpp +++ b/Inc/ST-LIB_LOW/Sensors/PWMSensor/PWMSensor.hpp @@ -12,41 +12,36 @@ #include "HALAL/Models/PinModel/Pin.hpp" #include "HALAL/Services/InputCapture/InputCapture.hpp" #include "Sensors/Sensor/Sensor.hpp" -template -class PWMSensor { - protected: +template class PWMSensor { +protected: uint8_t id; - Type *frequency; - Type *duty_cycle; + Type* frequency; + Type* duty_cycle; - public: +public: PWMSensor() = default; - PWMSensor(Pin &pin, Type &frequency, Type &duty_cycle); - PWMSensor(Pin &pin, Type *frequency, Type *duty_cycle); + PWMSensor(Pin& pin, Type& frequency, Type& duty_cycle); + PWMSensor(Pin& pin, Type* frequency, Type* duty_cycle); void read(); uint8_t get_id(); }; template -PWMSensor::PWMSensor(Pin &pin, Type &frequency, Type &duty_cycle) +PWMSensor::PWMSensor(Pin& pin, Type& frequency, Type& duty_cycle) : frequency(&frequency), duty_cycle(&duty_cycle) { id = InputCapture::inscribe(pin); Sensor::inputcapture_id_list.push_back(id); } template -PWMSensor::PWMSensor(Pin &pin, Type *frequency, Type *duty_cycle) +PWMSensor::PWMSensor(Pin& pin, Type* frequency, Type* duty_cycle) : frequency(frequency), duty_cycle(duty_cycle) { id = InputCapture::inscribe(pin); Sensor::inputcapture_id_list.push_back(id); } -template -void PWMSensor::read() { +template void PWMSensor::read() { *duty_cycle = InputCapture::read_duty_cycle(id); *frequency = InputCapture::read_frequency(id); } -template -uint8_t PWMSensor::get_id() { - return id; -} +template uint8_t PWMSensor::get_id() { return id; } diff --git a/Inc/ST-LIB_LOW/Sensors/Sensor/Sensor.hpp b/Inc/ST-LIB_LOW/Sensors/Sensor/Sensor.hpp index 1eade3607..3e1071c59 100644 --- a/Inc/ST-LIB_LOW/Sensors/Sensor/Sensor.hpp +++ b/Inc/ST-LIB_LOW/Sensors/Sensor/Sensor.hpp @@ -9,9 +9,9 @@ #include #include -class Sensor{ +class Sensor { public: - static void start(); - static std::vector EXTI_id_list; - static std::vector inputcapture_id_list; + static void start(); + static std::vector EXTI_id_list; + static std::vector inputcapture_id_list; }; diff --git a/Inc/ST-LIB_LOW/Sensors/SensorInterrupt/SensorInterrupt.hpp b/Inc/ST-LIB_LOW/Sensors/SensorInterrupt/SensorInterrupt.hpp index 20cbc70e1..ef3d629e1 100644 --- a/Inc/ST-LIB_LOW/Sensors/SensorInterrupt/SensorInterrupt.hpp +++ b/Inc/ST-LIB_LOW/Sensors/SensorInterrupt/SensorInterrupt.hpp @@ -12,15 +12,25 @@ #include "HALAL/Services/EXTI/EXTI.hpp" -class SensorInterrupt{ +class SensorInterrupt { public: - SensorInterrupt() = default; - SensorInterrupt(Pin &pin, std::function &&action, PinState *value, TRIGGER trigger = TRIGGER::RISING_EDGE); - SensorInterrupt(Pin &pin, std::function &&action, PinState &value, TRIGGER trigger = TRIGGER::RISING_EDGE); - void read(); - uint8_t get_id(); + SensorInterrupt() = default; + SensorInterrupt( + Pin& pin, + std::function&& action, + PinState* value, + TRIGGER trigger = TRIGGER::RISING_EDGE + ); + SensorInterrupt( + Pin& pin, + std::function&& action, + PinState& value, + TRIGGER trigger = TRIGGER::RISING_EDGE + ); + void read(); + uint8_t get_id(); protected: - uint8_t id; - PinState *value; + uint8_t id; + PinState* value; }; diff --git a/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp b/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp index aa6170dfb..6d71b8d7a 100644 --- a/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp @@ -1,31 +1,45 @@ #pragma once #include "StateMachine/StateOrder.hpp" -template -class HeapStateOrder : public HeapOrder{ +template class HeapStateOrder : public HeapOrder { public: - StateMachineType& state_machine; - typename StateMachineType::state_id state; + StateMachineType& state_machine; + typename StateMachineType::state_id state; - template - HeapStateOrder(uint16_t id,void(*callback)(void), StateMachineType& state_machine, typename StateMachineType::state_id state,Types*... values) : HeapOrder(id,callback,values...), - state_machine(state_machine), state(state) { - if(not state_machine.get_states().contains(state)){ - ErrorHandler("State Machine does not contain state, cannot add StateOrder"); - return; - } - else state_machine.get_states()[static_cast(state)].add_state_order(id); - orders[id] = this; + template + HeapStateOrder( + uint16_t id, + void (*callback)(void), + StateMachineType& state_machine, + typename StateMachineType::state_id state, + Types*... values + ) + : HeapOrder(id, callback, values...), state_machine(state_machine), state(state) { + if (not state_machine.get_states().contains(state)) { + ErrorHandler("State Machine does not contain state, cannot add StateOrder"); + return; + } else + state_machine.get_states()[static_cast(state)].add_state_order(id); + orders[id] = this; } void process() override { - if (callback != nullptr && state_machine.is_on && state_machine.get_current_state_id() == state) callback(); + if (callback != nullptr && state_machine.is_on && + state_machine.get_current_state_id() == state) + callback(); } - void parse(OrderProtocol* socket, uint8_t* data)override{ - if(state_machine.is_on && state_machine.get_current_state_id() == state) HeapOrder::parse(data); + void parse(OrderProtocol* socket, uint8_t* data) override { + if (state_machine.is_on && state_machine.get_current_state_id() == state) + HeapOrder::parse(data); } }; -template -HeapStateOrder(uint16_t, void(*)(void), StateMachineType&, typename StateMachineType::state_id, Types*...) -> HeapStateOrder; +template +HeapStateOrder( + uint16_t, + void (*)(void), + StateMachineType&, + typename StateMachineType::state_id, + Types*... +) -> HeapStateOrder; diff --git a/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp b/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp index 01cb15f31..83cffb9c4 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp @@ -1,31 +1,53 @@ #pragma once #include "StateMachine/StateOrder.hpp" -template requires NotCallablePack -class StackStateOrder : public StackOrder{ +template + requires NotCallablePack +class StackStateOrder : public StackOrder { public: - StateMachineType& state_machine; - typename StateMachineType::state_id state; - StackStateOrder(uint16_t id,void(*callback)(void), StateMachineType& state_machine, typename StateMachineType::state_id state,Types*... values) : StackOrder(id,callback,values...), - state_machine(state_machine), state(state) { - if(not state_machine.get_states().contains(state)){ - ErrorHandler("State Machine does not contain state, cannot add StateOrder"); - return; - } - else state_machine.get_states()[static_cast(state)].add_state_order(id); - Order::orders[id] = this; + StateMachineType& state_machine; + typename StateMachineType::state_id state; + StackStateOrder( + uint16_t id, + void (*callback)(void), + StateMachineType& state_machine, + typename StateMachineType::state_id state, + Types*... values + ) + : StackOrder(id, callback, values...), state_machine(state_machine), + state(state) { + if (not state_machine.get_states().contains(state)) { + ErrorHandler("State Machine does not contain state, cannot add StateOrder"); + return; + } else + state_machine.get_states()[static_cast(state)].add_state_order(id); + Order::orders[id] = this; } void process() override { - if (this->callback != nullptr && state_machine.is_on && state_machine.get_current_state_id() == state) this->callback(); + if (this->callback != nullptr && state_machine.is_on && + state_machine.get_current_state_id() == state) + this->callback(); } - void parse(OrderProtocol* socket, uint8_t* data)override{ - if(state_machine.is_on && state_machine.get_current_state_id() == state) StackOrder::parse(data); + void parse(OrderProtocol* socket, uint8_t* data) override { + if (state_machine.is_on && state_machine.get_current_state_id() == state) + StackOrder::parse(data); } }; #if __cpp_deduction_guides >= 201606 -template requires NotCallablePack -StackStateOrder(uint16_t id,void(*callback)(void), StateMachineType& state_machine, typename StateMachineType::state_id state,Types*... values)->StackStateOrder::value)*total_sizeof::value, Types...>; +template + requires NotCallablePack +StackStateOrder( + uint16_t id, + void (*callback)(void), + StateMachineType& state_machine, + typename StateMachineType::state_id state, + Types*... values +) + -> StackStateOrder< + StateMachineType, + (!has_container::value) * total_sizeof::value, + Types...>; #endif diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index 7ae89590b..0e038f573 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -19,37 +19,35 @@ using ms = std::chrono::milliseconds; using us = std::chrono::microseconds; using s = std::chrono::seconds; -template -using FixedVector = StaticVector; +template using FixedVector = StaticVector; -template +template concept IsEnum = std::is_enum_v; -template -concept ValidTime = std::same_as || std::same_as; +template +concept ValidTime = + std::same_as || std::same_as; using Callback = void (*)(); using Guard = bool (*)(); - static constexpr size_t NUMBER_OF_ACTIONS = 20; enum AlarmType { Milliseconds = 0, Microseconds = 1 }; class TimedAction { public: - Callback action = nullptr; - uint32_t period = 0; - AlarmType alarm_precision = Milliseconds; - uint8_t id = 255; - bool is_on = false; - - TimedAction() = default; - constexpr bool operator==(const TimedAction&) const = default; + Callback action = nullptr; + uint32_t period = 0; + AlarmType alarm_precision = Milliseconds; + uint8_t id = 255; + bool is_on = false; + + TimedAction() = default; + constexpr bool operator==(const TimedAction&) const = default; }; -template -struct Transition { +template struct Transition { StateEnum target; Guard predicate; constexpr bool operator==(const Transition&) const = default; @@ -58,191 +56,174 @@ struct Transition { template concept are_transitions = (std::same_as> && ...); - -template -class State { +template class State { private: - FixedVector cyclic_actions = {}; - FixedVector on_enter_actions = {}; - FixedVector on_exit_actions = {}; - StateEnum state = {}; - FixedVector, NTransitions> transitions = {}; + FixedVector cyclic_actions = {}; + FixedVector on_enter_actions = {}; + FixedVector on_exit_actions = {}; + StateEnum state = {}; + FixedVector, NTransitions> transitions = {}; - public: - [[no_unique_address]]FixedVector state_orders_ids = {}; - static constexpr size_t transition_count = NTransitions; +public: + [[no_unique_address]] FixedVector state_orders_ids = {}; + static constexpr size_t transition_count = NTransitions; - template + template requires are_transitions - consteval State(StateEnum state, T... transitions) - : state(state) - { - (this->transitions.push_back(transitions), ...); - } + consteval State(StateEnum state, T... transitions) : state(state) { + (this->transitions.push_back(transitions), ...); + } + + consteval State() = default; + + template consteval State(const State& other) { + state = other.get_state(); + for (const auto& t : other.get_transitions()) + transitions.push_back(t); + for (const auto& a : other.get_cyclic_actions()) + cyclic_actions.push_back(a); + for (const auto& a : other.get_enter_actions()) + on_enter_actions.push_back(a); + for (const auto& a : other.get_exit_actions()) + on_exit_actions.push_back(a); +#ifdef STLIB_ETH + for (const auto& id : other.state_orders_ids) + state_orders_ids.push_back(id); +#endif + } + + constexpr bool operator==(const State&) const = default; - consteval State() = default; - - template - consteval State(const State& other){ - state = other.get_state(); - for(const auto& t : other.get_transitions()) transitions.push_back(t); - for(const auto& a : other.get_cyclic_actions()) cyclic_actions.push_back(a); - for(const auto& a : other.get_enter_actions()) on_enter_actions.push_back(a); - for(const auto& a : other.get_exit_actions()) on_exit_actions.push_back(a); - #ifdef STLIB_ETH - for(const auto& id : other.state_orders_ids) state_orders_ids.push_back(id); - #endif - } - - constexpr bool operator==(const State&) const = default; - - - - constexpr const StateEnum& get_state() const { return state; }; - constexpr const auto& get_transitions() const { return transitions; }; - constexpr const auto& get_cyclic_actions() const {return cyclic_actions;}; - constexpr const auto& get_enter_actions() const {return on_enter_actions;}; - constexpr const auto& get_exit_actions() const {return on_exit_actions;}; - - - consteval void add_enter_action(Callback action) - { - on_enter_actions.push_back(action); - } - - consteval void add_exit_action(Callback action) - { - on_exit_actions.push_back(action); - } - - void enter() - { - for (const auto& action : on_enter_actions) - { - if(action) action(); - } - register_all_timed_actions(); - } - - void exit() - { - unregister_all_timed_actions(); - for (const auto& action : on_exit_actions) - { - if(action) action(); - } - } - - void unregister_all_timed_actions() - { - for(size_t i = 0; i < cyclic_actions.size(); i++) - { - TimedAction& timed_action = cyclic_actions[i]; - if(timed_action.action == nullptr){continue;} - if(!timed_action.is_on){ continue;} - - Scheduler::unregister_task(timed_action.id); - timed_action.is_on = false; + constexpr const StateEnum& get_state() const { return state; }; + constexpr const auto& get_transitions() const { return transitions; }; + constexpr const auto& get_cyclic_actions() const { return cyclic_actions; }; + constexpr const auto& get_enter_actions() const { return on_enter_actions; }; + constexpr const auto& get_exit_actions() const { return on_exit_actions; }; + + consteval void add_enter_action(Callback action) { on_enter_actions.push_back(action); } + + consteval void add_exit_action(Callback action) { on_exit_actions.push_back(action); } + + void enter() { + for (const auto& action : on_enter_actions) { + if (action) + action(); + } + register_all_timed_actions(); } - } - - void register_all_timed_actions() - { - for(size_t i = 0; i < cyclic_actions.size(); i++) - { - TimedAction& timed_action = cyclic_actions[i]; - if(timed_action.action == nullptr){ continue;} - - switch(timed_action.alarm_precision) - { - case Milliseconds: - timed_action.id = Scheduler::register_task((timed_action.period * 1000), timed_action.action); - break; - case Microseconds: - timed_action.id = Scheduler::register_task(timed_action.period, timed_action.action); - break; - - default: - ErrorHandler("Invalid Alarm Precision"); - return; - break; - } - if(timed_action.id == Scheduler::INVALID_ID) - { - for(size_t j = 0; j < i; ++j) - { - TimedAction& prev_action = cyclic_actions[j]; - if(prev_action.action == nullptr){ continue; } - if(!prev_action.is_on){ continue; } - Scheduler::unregister_task(prev_action.id); - prev_action.is_on = false; + + void exit() { + unregister_all_timed_actions(); + for (const auto& action : on_exit_actions) { + if (action) + action(); } - ErrorHandler("Failed to register timed action"); - return; - } - timed_action.is_on = true; } - } - - void remove_cyclic_action(TimedAction *timed_action) - { - if(timed_action->is_on){ unregister_timed_action(timed_action);} - - for(size_t i = 0; i < cyclic_actions.size(); i++) - { - TimedAction& slot = cyclic_actions[i]; - if(&slot == timed_action) - { - slot = TimedAction{}; - return; - } + + void unregister_all_timed_actions() { + for (size_t i = 0; i < cyclic_actions.size(); i++) { + TimedAction& timed_action = cyclic_actions[i]; + if (timed_action.action == nullptr) { + continue; + } + if (!timed_action.is_on) { + continue; + } + + Scheduler::unregister_task(timed_action.id); + timed_action.is_on = false; + } } - } - - void unregister_timed_action(TimedAction* timed_action) - { - Scheduler::unregister_task(timed_action->id); - timed_action->is_on = false; - } - - constexpr void add_state_order(uint16_t id) - { - state_orders_ids.push_back(id); - } - - template - consteval TimedAction * - add_cyclic_action(Callback action, - TimeUnit period){ - TimedAction timed_action = {}; - if constexpr (std::is_same_v){ - timed_action.alarm_precision = Milliseconds; - timed_action.period = period.count(); + + void register_all_timed_actions() { + for (size_t i = 0; i < cyclic_actions.size(); i++) { + TimedAction& timed_action = cyclic_actions[i]; + if (timed_action.action == nullptr) { + continue; + } + + switch (timed_action.alarm_precision) { + case Milliseconds: + timed_action.id = + Scheduler::register_task((timed_action.period * 1000), timed_action.action); + break; + case Microseconds: + timed_action.id = + Scheduler::register_task(timed_action.period, timed_action.action); + break; + + default: + ErrorHandler("Invalid Alarm Precision"); + return; + break; + } + if (timed_action.id == Scheduler::INVALID_ID) { + for (size_t j = 0; j < i; ++j) { + TimedAction& prev_action = cyclic_actions[j]; + if (prev_action.action == nullptr) { + continue; + } + if (!prev_action.is_on) { + continue; + } + Scheduler::unregister_task(prev_action.id); + prev_action.is_on = false; + } + ErrorHandler("Failed to register timed action"); + return; + } + timed_action.is_on = true; + } } - else if constexpr (std::is_same_v){ - timed_action.alarm_precision = Microseconds; - timed_action.period = period.count(); + void remove_cyclic_action(TimedAction* timed_action) { + if (timed_action->is_on) { + unregister_timed_action(timed_action); + } + + for (size_t i = 0; i < cyclic_actions.size(); i++) { + TimedAction& slot = cyclic_actions[i]; + if (&slot == timed_action) { + slot = TimedAction{}; + return; + } + } } - else { - ErrorHandler("Invalid Time Unit"); + void unregister_timed_action(TimedAction* timed_action) { + Scheduler::unregister_task(timed_action->id); + timed_action->is_on = false; } - - timed_action.action = action; - cyclic_actions.push_back(timed_action); - return &cyclic_actions[cyclic_actions.size() - 1]; - } + constexpr void add_state_order(uint16_t id) { state_orders_ids.push_back(id); } -}; + template + consteval TimedAction* add_cyclic_action(Callback action, TimeUnit period) { + TimedAction timed_action = {}; + if constexpr (std::is_same_v) { + timed_action.alarm_precision = Milliseconds; + timed_action.period = period.count(); + } + + else if constexpr (std::is_same_v) { + timed_action.alarm_precision = Microseconds; + timed_action.period = period.count(); + } + else { + ErrorHandler("Invalid Time Unit"); + } -template -struct is_state : std::false_type {}; + timed_action.action = action; + cyclic_actions.push_back(timed_action); + return &cyclic_actions[cyclic_actions.size() - 1]; + } +}; + +template struct is_state : std::false_type {}; template -struct is_state, StateEnum> : std::true_type {}; +struct is_state, StateEnum> : std::true_type {}; template concept IsState = is_state::value; @@ -250,345 +231,291 @@ concept IsState = is_state::value; template concept are_states = (IsState && ...); -/// Interface for State Machines to allow other classes to interact with the state machine without knowing its implementation +/// Interface for State Machines to allow other classes to interact with the state machine without +/// knowing its implementation class IStateMachine { - public: +public: virtual constexpr ~IStateMachine() = default; virtual void check_transitions() = 0; virtual void set_on(bool is_on) = 0; virtual void force_change_state(size_t state) = 0; virtual size_t get_current_state_id() const = 0; constexpr bool operator==(const IStateMachine&) const = default; - protected: + +protected: virtual void enter() = 0; virtual void exit() = 0; virtual void start() = 0; - template - friend class StateMachine; + template friend class StateMachine; }; template class StateMachine : public IStateMachine { -private: - struct NestedPair - { - StateEnum state; - IStateMachine* machine; - constexpr bool operator==(const NestedPair&) const = default; - }; +private: + struct NestedPair { + StateEnum state; + IStateMachine* machine; + constexpr bool operator==(const NestedPair&) const = default; + }; - StateEnum current_state; + StateEnum current_state; public: - constexpr ~StateMachine() override = default; - - void force_change_state(size_t state) override - { - StateEnum new_state = static_cast(state); - if(current_state == new_state) - { - return; - } - #ifdef STLIB_ETH - remove_state_orders(); - #endif - exit(); - current_state = new_state; - enter(); - #ifdef STLIB_ETH - refresh_state_orders(); - #endif - } - - size_t get_current_state_id() const override - { - return static_cast(current_state); - } - - bool is_on = true; - void set_on(bool is_on) override - { - this->is_on = is_on; - } + constexpr ~StateMachine() override = default; + + void force_change_state(size_t state) override { + StateEnum new_state = static_cast(state); + if (current_state == new_state) { + return; + } +#ifdef STLIB_ETH + remove_state_orders(); +#endif + exit(); + current_state = new_state; + enter(); +#ifdef STLIB_ETH + refresh_state_orders(); +#endif + } + + size_t get_current_state_id() const override { return static_cast(current_state); } + + bool is_on = true; + void set_on(bool is_on) override { this->is_on = is_on; } private: - FixedVector, NStates> states; - FixedVector, NTransitions> transitions = {}; - std::array, NStates> transitions_assoc ={}; - FixedVector nested_state_machine = {}; - - constexpr bool operator==(const StateMachine&) const = default; - - template - consteval void process_state(const State& state, size_t offset) - { - for (const auto& t : state.get_transitions()) - { + FixedVector, NStates> states; + FixedVector, NTransitions> transitions = {}; + std::array, NStates> transitions_assoc = {}; + FixedVector nested_state_machine = {}; + + constexpr bool operator==(const StateMachine&) const = default; + + template consteval void process_state(const State& state, size_t offset) { + for (const auto& t : state.get_transitions()) { transitions.push_back(t); offset++; } transitions_assoc[static_cast(state.get_state())] = { offset - state.get_transitions().size(), - state.get_transitions().size()}; - } - - inline void enter() override - { - auto& state = states[static_cast(current_state)]; - state.enter(); - - } - - inline void exit() override - { - auto& state = states[static_cast(current_state)]; - state.exit(); - } + state.get_transitions().size() + }; + } -public: + inline void enter() override { + auto& state = states[static_cast(current_state)]; + state.enter(); + } + + inline void exit() override { + auto& state = states[static_cast(current_state)]; + state.exit(); + } - template ... S> - consteval StateMachine(StateEnum initial_state, S... states) - : current_state(initial_state) - { - //Sort states by their enum value +public: + template ... S> + consteval StateMachine(StateEnum initial_state, S... states) : current_state(initial_state) { + // Sort states by their enum value using StateType = State; std::array sorted_states; size_t index = 0; ((sorted_states[index++] = StateType(states)), ...); - - for(size_t i = 0; i < sorted_states.size(); i++){ - for(size_t j = 0; j < sorted_states.size() - 1; j++){ - if(sorted_states[j].get_state() > sorted_states[j+1].get_state()){ + + for (size_t i = 0; i < sorted_states.size(); i++) { + for (size_t j = 0; j < sorted_states.size() - 1; j++) { + if (sorted_states[j].get_state() > sorted_states[j + 1].get_state()) { auto temp = sorted_states[j]; - sorted_states[j] = sorted_states[j+1]; - sorted_states[j+1] = temp; + sorted_states[j] = sorted_states[j + 1]; + sorted_states[j + 1] = temp; } } } - - //Check that states are contiguous and start from 0 - for(size_t i = 0; i < sorted_states.size(); i++) { - if(static_cast(sorted_states[i].get_state()) != i){ + + // Check that states are contiguous and start from 0 + for (size_t i = 0; i < sorted_states.size(); i++) { + if (static_cast(sorted_states[i].get_state()) != i) { ErrorHandler("States Enum must be contiguous and start from 0"); } } - for(size_t i = 0; i < sorted_states.size(); i++) { + for (size_t i = 0; i < sorted_states.size(); i++) { this->states.push_back(sorted_states[i]); } size_t offset = 0; - for(const auto& s : sorted_states) { - process_state(s, offset); - offset += s.get_transitions().size(); + for (const auto& s : sorted_states) { + process_state(s, offset); + offset += s.get_transitions().size(); } } - - - void check_transitions() override - { + void check_transitions() override { auto& [i, n] = transitions_assoc[static_cast(current_state)]; - for (auto index = i; index < i + n; ++index) - { + for (auto index = i; index < i + n; ++index) { const auto& t = transitions[index]; - if (t.predicate()) - { + if (t.predicate()) { exit(); - for(auto& nested : nested_state_machine) - { - if(nested.state == current_state){ - nested.machine->exit(); - break; - } + for (auto& nested : nested_state_machine) { + if (nested.state == current_state) { + nested.machine->exit(); + break; + } } - #ifdef STLIB_ETH +#ifdef STLIB_ETH remove_state_orders(); - #endif +#endif current_state = t.target; enter(); - for(auto& nested : nested_state_machine) - { - if(nested.state == current_state){ - nested.machine->enter(); - break; - } + for (auto& nested : nested_state_machine) { + if (nested.state == current_state) { + nested.machine->enter(); + break; + } } - #ifdef STLIB_ETH +#ifdef STLIB_ETH refresh_state_orders(); - #endif +#endif break; } } - for(auto& nested : nested_state_machine) - { - if(nested.state == current_state){ - nested.machine->check_transitions(); - break; - } + for (auto& nested : nested_state_machine) { + if (nested.state == current_state) { + nested.machine->check_transitions(); + break; + } } } - void start() override - { - enter(); - for(auto& nested : nested_state_machine) - { - if(nested.state == current_state){ - nested.machine->start(); - break; - } + void start() override { + enter(); + for (auto& nested : nested_state_machine) { + if (nested.state == current_state) { + nested.machine->start(); + break; + } } } + template void force_change_state(const State& state) { + StateEnum new_state = state.get_state(); + if (current_state == new_state) { + return; + } - template - void force_change_state(const State& state) - { - StateEnum new_state = state.get_state(); - if(current_state == new_state) - { - return; - } - exit(); - for(auto& nested : nested_state_machine) - { - if(nested.state == current_state){ - nested.machine->exit(); - break; + for (auto& nested : nested_state_machine) { + if (nested.state == current_state) { + nested.machine->exit(); + break; } - } - #ifdef STLIB_ETH + } +#ifdef STLIB_ETH remove_state_orders(); - #endif +#endif current_state = new_state; enter(); - for(auto& nested : nested_state_machine) - { - if(nested.state == current_state){ - nested.machine->enter(); - break; + for (auto& nested : nested_state_machine) { + if (nested.state == current_state) { + nested.machine->enter(); + break; } - } - #ifdef STLIB_ETH + } +#ifdef STLIB_ETH refresh_state_orders(); - #endif +#endif } - - - - template - consteval TimedAction * - add_cyclic_action(Callback action, - TimeUnit period,const State& state) - { - for(size_t i = 0; i < states.size(); ++i) - { - if(states[i].get_state() == state.get_state()) - { - return states[i].add_cyclic_action(action, period); + + template + consteval TimedAction* + add_cyclic_action(Callback action, TimeUnit period, const State& state) { + for (size_t i = 0; i < states.size(); ++i) { + if (states[i].get_state() == state.get_state()) { + return states[i].add_cyclic_action(action, period); + } } + ErrorHandler("Error: The state is not added to the state machine"); + return nullptr; } - ErrorHandler("Error: The state is not added to the state machine"); - return nullptr; - } - - template - void remove_cyclic_action(TimedAction *timed_action, const State& state) - { - for(size_t i = 0; i < states.size(); ++i) - { - if(states[i].get_state() == state.get_state()) - { - states[i].remove_cyclic_action(timed_action); - return; - } - } - ErrorHandler("Error: The state is not added to the state machine"); - } - - template - consteval void add_enter_action(Callback action, const State& state) - { - for(size_t i = 0; i < states.size(); ++i) - { - if(states[i].get_state() == state.get_state()) - { - states[i].add_enter_action(action); - return; + + template + void remove_cyclic_action(TimedAction* timed_action, const State& state) { + for (size_t i = 0; i < states.size(); ++i) { + if (states[i].get_state() == state.get_state()) { + states[i].remove_cyclic_action(timed_action); + return; + } } + ErrorHandler("Error: The state is not added to the state machine"); } - ErrorHandler("Error: The state is not added to the state machine"); - } - - template - consteval void add_exit_action(Callback action, const State& state) - { - for(size_t i = 0; i < states.size(); ++i) - { - if(states[i].get_state() == state.get_state()) - { - states[i].add_exit_action(action); - return; + + template + consteval void add_enter_action(Callback action, const State& state) { + for (size_t i = 0; i < states.size(); ++i) { + if (states[i].get_state() == state.get_state()) { + states[i].add_enter_action(action); + return; + } } + ErrorHandler("Error: The state is not added to the state machine"); } - ErrorHandler("Error: The state is not added to the state machine"); - } - - template - constexpr void add_state_machine(IStateMachine& state_machine, const State& state) - { - for(auto& nested : nested_state_machine) - { - if(nested.state == state.get_state()) - { - ErrorHandler("Only one Nested State Machine can be added per state, tried to add to state: %d", static_cast(state.get_state())); - return; - } - } - nested_state_machine.push_back({state.get_state(), &state_machine}); - } - StateEnum get_current_state() const - { - return current_state; - } + template + consteval void add_exit_action(Callback action, const State& state) { + for (size_t i = 0; i < states.size(); ++i) { + if (states[i].get_state() == state.get_state()) { + states[i].add_exit_action(action); + return; + } + } + ErrorHandler("Error: The state is not added to the state machine"); + } + template + constexpr void + add_state_machine(IStateMachine& state_machine, const State& state) { + for (auto& nested : nested_state_machine) { + if (nested.state == state.get_state()) { + ErrorHandler( + "Only one Nested State Machine can be added per state, tried to add to state: " + "%d", + static_cast(state.get_state()) + ); + return; + } + } + nested_state_machine.push_back({state.get_state(), &state_machine}); + } + StateEnum get_current_state() const { return current_state; } - inline void refresh_state_orders() - { - #ifdef STLIB_ETH - if(states[static_cast(current_state)].state_orders_ids.size() != 0) - { - std::span ids(states[static_cast(current_state)].state_orders_ids.get_data(), - states[static_cast(current_state)].state_orders_ids.size()); - StateOrder::add_state_orders(ids); - } - #endif - } - - inline void remove_state_orders() - { - #ifdef STLIB_ETH - if(states[static_cast(current_state)].state_orders_ids.size() != 0) - { - std::span ids(states[static_cast(current_state)].state_orders_ids.get_data(), - states[static_cast(current_state)].state_orders_ids.size()); - StateOrder::remove_state_orders(ids); + inline void refresh_state_orders() { +#ifdef STLIB_ETH + if (states[static_cast(current_state)].state_orders_ids.size() != 0) { + std::span ids( + states[static_cast(current_state)].state_orders_ids.get_data(), + states[static_cast(current_state)].state_orders_ids.size() + ); + StateOrder::add_state_orders(ids); + } +#endif } - #endif - } - constexpr auto& get_states() - { - return states; - } + inline void remove_state_orders() { +#ifdef STLIB_ETH + if (states[static_cast(current_state)].state_orders_ids.size() != 0) { + std::span ids( + states[static_cast(current_state)].state_orders_ids.get_data(), + states[static_cast(current_state)].state_orders_ids.size() + ); + StateOrder::remove_state_orders(ids); + } +#endif + } + constexpr auto& get_states() { return states; } }; /* @brief Helper function to create a State instance @@ -598,29 +525,33 @@ class StateMachine : public IStateMachine { * @param state The state enum value * @param transitions The transitions associated with the state * @return A State instance initialized with the provided state and transitions -* -*/ + * + */ template - requires are_transitions - consteval auto make_state(StateEnum state, Transitions... transitions) { + requires are_transitions +consteval auto make_state(StateEnum state, Transitions... transitions) { constexpr size_t number_of_transitions = sizeof...(transitions); - return State(state,transitions...); - } - - /* @brief Helper function to create a StateMachine instance - * - * @tparam States Variadic template parameter pack representing the states - * @param initial_state The initial state enum value - * @param states The states to be included in the state machine - * @return A StateMachine instance initialized with the provided initial state and states - */ - - template + return State(state, transitions...); +} + +/* @brief Helper function to create a StateMachine instance + * + * @tparam States Variadic template parameter pack representing the states + * @param initial_state The initial state enum value + * @param states The states to be included in the state machine + * @return A StateMachine instance initialized with the provided initial state and states + */ + +template requires are_states - consteval auto make_state_machine(StateEnum initial_state, States... states) { +consteval auto make_state_machine(StateEnum initial_state, States... states) { constexpr size_t number_of_states = sizeof...(states); - constexpr size_t number_of_transitions = (std::remove_reference_t::transition_count + ... + 0); - - return StateMachine(initial_state, states...); - } + constexpr size_t number_of_transitions = + (std::remove_reference_t::transition_count + ... + 0); + + return StateMachine( + initial_state, + states... + ); +} diff --git a/Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp b/Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp index f7ad0a20c..3b8468d9b 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp @@ -5,42 +5,34 @@ #include "ErrorHandler/ErrorHandler.hpp" #include -class StateOrder : public Order{ +class StateOrder : public Order { public: - enum StateOrdersID{ - ADD_STATE_ORDERS = 5, - REMOVE_STATE_ORDERS = 6 - }; - static OrderProtocol* informer_socket; - static uint16_t state_orders_ids_size; - static std::span state_orders_ids; - static StackOrder<0,uint16_t, std::span> add_state_orders_order; - static StackOrder<0,uint16_t, std::span> remove_state_orders_order; + enum StateOrdersID { ADD_STATE_ORDERS = 5, REMOVE_STATE_ORDERS = 6 }; + static OrderProtocol* informer_socket; + static uint16_t state_orders_ids_size; + static std::span state_orders_ids; + static StackOrder<0, uint16_t, std::span> add_state_orders_order; + static StackOrder<0, uint16_t, std::span> remove_state_orders_order; - static void set_socket(OrderProtocol& socket){ - informer_socket = &socket; - } + static void set_socket(OrderProtocol& socket) { informer_socket = &socket; } - static void add_state_orders(std::span new_ids){ - if(informer_socket == nullptr) { - ErrorHandler("Informer Socket has not been set"); - return; - }; - state_orders_ids = new_ids; - state_orders_ids_size = new_ids.size(); - informer_socket->send_order(add_state_orders_order); - } + static void add_state_orders(std::span new_ids) { + if (informer_socket == nullptr) { + ErrorHandler("Informer Socket has not been set"); + return; + }; + state_orders_ids = new_ids; + state_orders_ids_size = new_ids.size(); + informer_socket->send_order(add_state_orders_order); + } - static void remove_state_orders(std::span old_ids){ - if(informer_socket == nullptr){ - ErrorHandler("Informer Socket has not been set"); - return; - } - state_orders_ids = old_ids; - state_orders_ids_size = old_ids.size(); - informer_socket->send_order(remove_state_orders_order); - } + static void remove_state_orders(std::span old_ids) { + if (informer_socket == nullptr) { + ErrorHandler("Informer Socket has not been set"); + return; + } + state_orders_ids = old_ids; + state_orders_ids_size = old_ids.size(); + informer_socket->send_order(remove_state_orders_order); + } }; - - - diff --git a/Inc/STM32CubeH7Wrapper/hal_wrapper.h b/Inc/STM32CubeH7Wrapper/hal_wrapper.h index b944aebf9..d8f66260e 100644 --- a/Inc/STM32CubeH7Wrapper/hal_wrapper.h +++ b/Inc/STM32CubeH7Wrapper/hal_wrapper.h @@ -55,8 +55,8 @@ #endif #endif -extern "C" void HAL_SYSCFG_AnalogSwitchConfig(uint32_t SYSCFG_AnalogSwitch, - uint32_t SYSCFG_SwitchState); +extern "C" void +HAL_SYSCFG_AnalogSwitchConfig(uint32_t SYSCFG_AnalogSwitch, uint32_t SYSCFG_SwitchState); #endif #ifndef STLIB_HAS_ADC3 diff --git a/Inc/main.h b/Inc/main.h index bb6586924..27c6c6ad2 100644 --- a/Inc/main.h +++ b/Inc/main.h @@ -1,21 +1,21 @@ /* USER CODE BEGIN Header */ /** - ****************************************************************************** - * @file : main.h - * @brief : Header for main.c file. - * This file contains the common defines of the application. - ****************************************************************************** - * @attention - * - * Copyright (c) 2022 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - */ + ****************************************************************************** + * @file : main.h + * @brief : Header for main.c file. + * This file contains the common defines of the application. + ****************************************************************************** + * @attention + * + * Copyright (c) 2022 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ /* USER CODE END Header */ /* Define to prevent recursive inclusion -------------------------------------*/ @@ -49,7 +49,7 @@ extern "C" { /* USER CODE END EM */ -void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim); +void HAL_TIM_MspPostInit(TIM_HandleTypeDef* htim); /* Exported functions prototypes ---------------------------------------------*/ void Error_Handler(void); diff --git a/Inc/stm32h7xx_hal_conf_template.h b/Inc/stm32h7xx_hal_conf_template.h index 4f41c7d25..7c0d513f5 100644 --- a/Inc/stm32h7xx_hal_conf_template.h +++ b/Inc/stm32h7xx_hal_conf_template.h @@ -1,29 +1,29 @@ /** - ****************************************************************************** - * @file stm32h7xx_hal_conf_template.h - * @author MCD Application Team - * @brief HAL configuration template file. - * This file should be copied to the application folder and renamed - * to stm32h7xx_hal_conf.h. - ****************************************************************************** - * @attention - * - * Copyright (c) 2017 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - */ + ****************************************************************************** + * @file stm32h7xx_hal_conf_template.h + * @author MCD Application Team + * @brief HAL configuration template file. + * This file should be copied to the application folder and renamed + * to stm32h7xx_hal_conf.h. + ****************************************************************************** + * @attention + * + * Copyright (c) 2017 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ /* Define to prevent recursive inclusion -------------------------------------*/ #ifndef STM32H7xx_HAL_CONF_H #define STM32H7xx_HAL_CONF_H #ifdef __cplusplus - extern "C" { +extern "C" { #endif /* Exported types ------------------------------------------------------------*/ @@ -31,8 +31,8 @@ /* ########################## Module Selection ############################## */ /** - * @brief This is the list of modules to be used in the HAL driver - */ + * @brief This is the list of modules to be used in the HAL driver + */ #define HAL_MODULE_ENABLED #define HAL_ADC_MODULE_ENABLED #define HAL_CEC_MODULE_ENABLED @@ -98,287 +98,285 @@ /* ########################## Oscillator Values adaptation ####################*/ /** - * @brief Adjust the value of External High Speed oscillator (HSE) used in your application. - * This value is used by the RCC HAL module to compute the system frequency - * (when HSE is used as system clock source, directly or through the PLL). - */ -#if !defined (HSE_VALUE) -#define HSE_VALUE (25000000UL) /*!< Value of the External oscillator in Hz */ -#endif /* HSE_VALUE */ - -#if !defined (HSE_STARTUP_TIMEOUT) - #define HSE_STARTUP_TIMEOUT (100UL) /*!< Time out for HSE start up, in ms */ -#endif /* HSE_STARTUP_TIMEOUT */ + * @brief Adjust the value of External High Speed oscillator (HSE) used in your application. + * This value is used by the RCC HAL module to compute the system frequency + * (when HSE is used as system clock source, directly or through the PLL). + */ +#if !defined(HSE_VALUE) +#define HSE_VALUE (25000000UL) /*!< Value of the External oscillator in Hz */ +#endif /* HSE_VALUE */ + +#if !defined(HSE_STARTUP_TIMEOUT) +#define HSE_STARTUP_TIMEOUT (100UL) /*!< Time out for HSE start up, in ms */ +#endif /* HSE_STARTUP_TIMEOUT */ /** - * @brief Internal oscillator (CSI) default value. - * This value is the default CSI value after Reset. - */ -#if !defined (CSI_VALUE) - #define CSI_VALUE (4000000UL) /*!< Value of the Internal oscillator in Hz*/ -#endif /* CSI_VALUE */ + * @brief Internal oscillator (CSI) default value. + * This value is the default CSI value after Reset. + */ +#if !defined(CSI_VALUE) +#define CSI_VALUE (4000000UL) /*!< Value of the Internal oscillator in Hz*/ +#endif /* CSI_VALUE */ /** - * @brief Internal High Speed oscillator (HSI) value. - * This value is used by the RCC HAL module to compute the system frequency - * (when HSI is used as system clock source, directly or through the PLL). - */ -#if !defined (HSI_VALUE) - #define HSI_VALUE (64000000UL) /*!< Value of the Internal oscillator in Hz*/ -#endif /* HSI_VALUE */ + * @brief Internal High Speed oscillator (HSI) value. + * This value is used by the RCC HAL module to compute the system frequency + * (when HSI is used as system clock source, directly or through the PLL). + */ +#if !defined(HSI_VALUE) +#define HSI_VALUE (64000000UL) /*!< Value of the Internal oscillator in Hz*/ +#endif /* HSI_VALUE */ /** - * @brief External Low Speed oscillator (LSE) value. - * This value is used by the UART, RTC HAL module to compute the system frequency - */ -#if !defined (LSE_VALUE) - #define LSE_VALUE (32768UL) /*!< Value of the External oscillator in Hz*/ -#endif /* LSE_VALUE */ - - -#if !defined (LSE_STARTUP_TIMEOUT) - #define LSE_STARTUP_TIMEOUT (5000UL) /*!< Time out for LSE start up, in ms */ -#endif /* LSE_STARTUP_TIMEOUT */ - -#if !defined (LSI_VALUE) - #define LSI_VALUE (32000UL) /*!< LSI Typical Value in Hz*/ -#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz - The real value may vary depending on the variations - in voltage and temperature.*/ + * @brief External Low Speed oscillator (LSE) value. + * This value is used by the UART, RTC HAL module to compute the system frequency + */ +#if !defined(LSE_VALUE) +#define LSE_VALUE (32768UL) /*!< Value of the External oscillator in Hz*/ +#endif /* LSE_VALUE */ + +#if !defined(LSE_STARTUP_TIMEOUT) +#define LSE_STARTUP_TIMEOUT (5000UL) /*!< Time out for LSE start up, in ms */ +#endif /* LSE_STARTUP_TIMEOUT */ + +#if !defined(LSI_VALUE) +#define LSI_VALUE (32000UL) /*!< LSI Typical Value in Hz*/ +#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz \ + The real value may vary depending on the variations \ + in voltage and temperature.*/ /** - * @brief External clock source for I2S peripheral - * This value is used by the I2S HAL module to compute the I2S clock source - * frequency, this source is inserted directly through I2S_CKIN pad. - */ -#if !defined (EXTERNAL_CLOCK_VALUE) - #define EXTERNAL_CLOCK_VALUE 12288000UL /*!< Value of the External clock in Hz*/ -#endif /* EXTERNAL_CLOCK_VALUE */ + * @brief External clock source for I2S peripheral + * This value is used by the I2S HAL module to compute the I2S clock source + * frequency, this source is inserted directly through I2S_CKIN pad. + */ +#if !defined(EXTERNAL_CLOCK_VALUE) +#define EXTERNAL_CLOCK_VALUE 12288000UL /*!< Value of the External clock in Hz*/ +#endif /* EXTERNAL_CLOCK_VALUE */ /* Tip: To avoid modifying this file each time you need to use different HSE, === you can define the HSE value in your toolchain compiler preprocessor. */ /* ########################### System Configuration ######################### */ /** - * @brief This is the HAL system configuration section - */ -#define VDD_VALUE (3300UL) /*!< Value of VDD in mv */ -#define TICK_INT_PRIORITY (0x0FUL) /*!< tick interrupt priority */ -#define USE_RTOS 0 -#define USE_SD_TRANSCEIVER 0U /*!< use uSD Transceiver */ -#define USE_SPI_CRC 1U /*!< use CRC in SPI */ - -#define USE_HAL_ADC_REGISTER_CALLBACKS 0U /* ADC register callback disabled */ -#define USE_HAL_CEC_REGISTER_CALLBACKS 0U /* CEC register callback disabled */ -#define USE_HAL_COMP_REGISTER_CALLBACKS 0U /* COMP register callback disabled */ -#define USE_HAL_CORDIC_REGISTER_CALLBACKS 0U /* CORDIC register callback disabled */ -#define USE_HAL_CRYP_REGISTER_CALLBACKS 0U /* CRYP register callback disabled */ -#define USE_HAL_DAC_REGISTER_CALLBACKS 0U /* DAC register callback disabled */ -#define USE_HAL_DCMI_REGISTER_CALLBACKS 0U /* DCMI register callback disabled */ -#define USE_HAL_DFSDM_REGISTER_CALLBACKS 0U /* DFSDM register callback disabled */ -#define USE_HAL_DMA2D_REGISTER_CALLBACKS 0U /* DMA2D register callback disabled */ -#define USE_HAL_DSI_REGISTER_CALLBACKS 0U /* DSI register callback disabled */ -#define USE_HAL_DTS_REGISTER_CALLBACKS 0U /* DTS register callback disabled */ -#define USE_HAL_ETH_REGISTER_CALLBACKS 0U /* ETH register callback disabled */ -#define USE_HAL_FDCAN_REGISTER_CALLBACKS 0U /* FDCAN register callback disabled */ -#define USE_HAL_FMAC_REGISTER_CALLBACKS 0U /* FMAC register callback disabled */ -#define USE_HAL_NAND_REGISTER_CALLBACKS 0U /* NAND register callback disabled */ -#define USE_HAL_NOR_REGISTER_CALLBACKS 0U /* NOR register callback disabled */ -#define USE_HAL_SDRAM_REGISTER_CALLBACKS 0U /* SDRAM register callback disabled */ -#define USE_HAL_SRAM_REGISTER_CALLBACKS 0U /* SRAM register callback disabled */ -#define USE_HAL_HASH_REGISTER_CALLBACKS 0U /* HASH register callback disabled */ -#define USE_HAL_HCD_REGISTER_CALLBACKS 0U /* HCD register callback disabled */ -#define USE_HAL_GFXMMU_REGISTER_CALLBACKS 0U /* GFXMMU register callback disabled */ -#define USE_HAL_HRTIM_REGISTER_CALLBACKS 0U /* HRTIM register callback disabled */ -#define USE_HAL_I2C_REGISTER_CALLBACKS 0U /* I2C register callback disabled */ -#define USE_HAL_I2S_REGISTER_CALLBACKS 0U /* I2S register callback disabled */ -#define USE_HAL_IRDA_REGISTER_CALLBACKS 0U /* IRDA register callback disabled */ -#define USE_HAL_JPEG_REGISTER_CALLBACKS 0U /* JPEG register callback disabled */ -#define USE_HAL_LPTIM_REGISTER_CALLBACKS 0U /* LPTIM register callback disabled */ -#define USE_HAL_LTDC_REGISTER_CALLBACKS 0U /* LTDC register callback disabled */ -#define USE_HAL_MDIOS_REGISTER_CALLBACKS 0U /* MDIO register callback disabled */ -#define USE_HAL_MMC_REGISTER_CALLBACKS 0U /* MMC register callback disabled */ -#define USE_HAL_OPAMP_REGISTER_CALLBACKS 0U /* MDIO register callback disabled */ -#define USE_HAL_OSPI_REGISTER_CALLBACKS 0U /* OSPI register callback disabled */ -#define USE_HAL_OTFDEC_REGISTER_CALLBACKS 0U /* OTFDEC register callback disabled */ -#define USE_HAL_PCD_REGISTER_CALLBACKS 0U /* PCD register callback disabled */ -#define USE_HAL_QSPI_REGISTER_CALLBACKS 0U /* QSPI register callback disabled */ -#define USE_HAL_RNG_REGISTER_CALLBACKS 0U /* RNG register callback disabled */ -#define USE_HAL_RTC_REGISTER_CALLBACKS 0U /* RTC register callback disabled */ -#define USE_HAL_SAI_REGISTER_CALLBACKS 0U /* SAI register callback disabled */ -#define USE_HAL_SD_REGISTER_CALLBACKS 0U /* SD register callback disabled */ -#define USE_HAL_SMARTCARD_REGISTER_CALLBACKS 0U /* SMARTCARD register callback disabled */ -#define USE_HAL_SPDIFRX_REGISTER_CALLBACKS 0U /* SPDIFRX register callback disabled */ -#define USE_HAL_SMBUS_REGISTER_CALLBACKS 0U /* SMBUS register callback disabled */ -#define USE_HAL_SPI_REGISTER_CALLBACKS 0U /* SPI register callback disabled */ -#define USE_HAL_SWPMI_REGISTER_CALLBACKS 0U /* SWPMI register callback disabled */ -#define USE_HAL_TIM_REGISTER_CALLBACKS 0U /* TIM register callback disabled */ -#define USE_HAL_UART_REGISTER_CALLBACKS 0U /* UART register callback disabled */ -#define USE_HAL_USART_REGISTER_CALLBACKS 0U /* USART register callback disabled */ -#define USE_HAL_WWDG_REGISTER_CALLBACKS 0U /* WWDG register callback disabled */ + * @brief This is the HAL system configuration section + */ +#define VDD_VALUE (3300UL) /*!< Value of VDD in mv */ +#define TICK_INT_PRIORITY (0x0FUL) /*!< tick interrupt priority */ +#define USE_RTOS 0 +#define USE_SD_TRANSCEIVER 0U /*!< use uSD Transceiver */ +#define USE_SPI_CRC 1U /*!< use CRC in SPI */ + +#define USE_HAL_ADC_REGISTER_CALLBACKS 0U /* ADC register callback disabled */ +#define USE_HAL_CEC_REGISTER_CALLBACKS 0U /* CEC register callback disabled */ +#define USE_HAL_COMP_REGISTER_CALLBACKS 0U /* COMP register callback disabled */ +#define USE_HAL_CORDIC_REGISTER_CALLBACKS 0U /* CORDIC register callback disabled */ +#define USE_HAL_CRYP_REGISTER_CALLBACKS 0U /* CRYP register callback disabled */ +#define USE_HAL_DAC_REGISTER_CALLBACKS 0U /* DAC register callback disabled */ +#define USE_HAL_DCMI_REGISTER_CALLBACKS 0U /* DCMI register callback disabled */ +#define USE_HAL_DFSDM_REGISTER_CALLBACKS 0U /* DFSDM register callback disabled */ +#define USE_HAL_DMA2D_REGISTER_CALLBACKS 0U /* DMA2D register callback disabled */ +#define USE_HAL_DSI_REGISTER_CALLBACKS 0U /* DSI register callback disabled */ +#define USE_HAL_DTS_REGISTER_CALLBACKS 0U /* DTS register callback disabled */ +#define USE_HAL_ETH_REGISTER_CALLBACKS 0U /* ETH register callback disabled */ +#define USE_HAL_FDCAN_REGISTER_CALLBACKS 0U /* FDCAN register callback disabled */ +#define USE_HAL_FMAC_REGISTER_CALLBACKS 0U /* FMAC register callback disabled */ +#define USE_HAL_NAND_REGISTER_CALLBACKS 0U /* NAND register callback disabled */ +#define USE_HAL_NOR_REGISTER_CALLBACKS 0U /* NOR register callback disabled */ +#define USE_HAL_SDRAM_REGISTER_CALLBACKS 0U /* SDRAM register callback disabled */ +#define USE_HAL_SRAM_REGISTER_CALLBACKS 0U /* SRAM register callback disabled */ +#define USE_HAL_HASH_REGISTER_CALLBACKS 0U /* HASH register callback disabled */ +#define USE_HAL_HCD_REGISTER_CALLBACKS 0U /* HCD register callback disabled */ +#define USE_HAL_GFXMMU_REGISTER_CALLBACKS 0U /* GFXMMU register callback disabled */ +#define USE_HAL_HRTIM_REGISTER_CALLBACKS 0U /* HRTIM register callback disabled */ +#define USE_HAL_I2C_REGISTER_CALLBACKS 0U /* I2C register callback disabled */ +#define USE_HAL_I2S_REGISTER_CALLBACKS 0U /* I2S register callback disabled */ +#define USE_HAL_IRDA_REGISTER_CALLBACKS 0U /* IRDA register callback disabled */ +#define USE_HAL_JPEG_REGISTER_CALLBACKS 0U /* JPEG register callback disabled */ +#define USE_HAL_LPTIM_REGISTER_CALLBACKS 0U /* LPTIM register callback disabled */ +#define USE_HAL_LTDC_REGISTER_CALLBACKS 0U /* LTDC register callback disabled */ +#define USE_HAL_MDIOS_REGISTER_CALLBACKS 0U /* MDIO register callback disabled */ +#define USE_HAL_MMC_REGISTER_CALLBACKS 0U /* MMC register callback disabled */ +#define USE_HAL_OPAMP_REGISTER_CALLBACKS 0U /* MDIO register callback disabled */ +#define USE_HAL_OSPI_REGISTER_CALLBACKS 0U /* OSPI register callback disabled */ +#define USE_HAL_OTFDEC_REGISTER_CALLBACKS 0U /* OTFDEC register callback disabled */ +#define USE_HAL_PCD_REGISTER_CALLBACKS 0U /* PCD register callback disabled */ +#define USE_HAL_QSPI_REGISTER_CALLBACKS 0U /* QSPI register callback disabled */ +#define USE_HAL_RNG_REGISTER_CALLBACKS 0U /* RNG register callback disabled */ +#define USE_HAL_RTC_REGISTER_CALLBACKS 0U /* RTC register callback disabled */ +#define USE_HAL_SAI_REGISTER_CALLBACKS 0U /* SAI register callback disabled */ +#define USE_HAL_SD_REGISTER_CALLBACKS 0U /* SD register callback disabled */ +#define USE_HAL_SMARTCARD_REGISTER_CALLBACKS 0U /* SMARTCARD register callback disabled */ +#define USE_HAL_SPDIFRX_REGISTER_CALLBACKS 0U /* SPDIFRX register callback disabled */ +#define USE_HAL_SMBUS_REGISTER_CALLBACKS 0U /* SMBUS register callback disabled */ +#define USE_HAL_SPI_REGISTER_CALLBACKS 0U /* SPI register callback disabled */ +#define USE_HAL_SWPMI_REGISTER_CALLBACKS 0U /* SWPMI register callback disabled */ +#define USE_HAL_TIM_REGISTER_CALLBACKS 0U /* TIM register callback disabled */ +#define USE_HAL_UART_REGISTER_CALLBACKS 0U /* UART register callback disabled */ +#define USE_HAL_USART_REGISTER_CALLBACKS 0U /* USART register callback disabled */ +#define USE_HAL_WWDG_REGISTER_CALLBACKS 0U /* WWDG register callback disabled */ /* ########################### Ethernet Configuration ######################### */ -#define ETH_TX_DESC_CNT 4 /* number of Ethernet Tx DMA descriptors */ -#define ETH_RX_DESC_CNT 4 /* number of Ethernet Rx DMA descriptors */ +#define ETH_TX_DESC_CNT 4 /* number of Ethernet Tx DMA descriptors */ +#define ETH_RX_DESC_CNT 4 /* number of Ethernet Rx DMA descriptors */ -#define ETH_MAC_ADDR0 (0x02UL) -#define ETH_MAC_ADDR1 (0x00UL) -#define ETH_MAC_ADDR2 (0x00UL) -#define ETH_MAC_ADDR3 (0x00UL) -#define ETH_MAC_ADDR4 (0x00UL) -#define ETH_MAC_ADDR5 (0x00UL) +#define ETH_MAC_ADDR0 (0x02UL) +#define ETH_MAC_ADDR1 (0x00UL) +#define ETH_MAC_ADDR2 (0x00UL) +#define ETH_MAC_ADDR3 (0x00UL) +#define ETH_MAC_ADDR4 (0x00UL) +#define ETH_MAC_ADDR5 (0x00UL) /* ########################## Assert Selection ############################## */ /** - * @brief Uncomment the line below to expanse the "assert_param" macro in the - * HAL drivers code - */ + * @brief Uncomment the line below to expanse the "assert_param" macro in the + * HAL drivers code + */ /* #define USE_FULL_ASSERT 1U */ - /* Includes ------------------------------------------------------------------*/ /** - * @brief Include module's header file - */ + * @brief Include module's header file + */ #ifdef HAL_RCC_MODULE_ENABLED - #include "stm32h7xx_hal_rcc.h" +#include "stm32h7xx_hal_rcc.h" #endif /* HAL_RCC_MODULE_ENABLED */ #ifdef HAL_GPIO_MODULE_ENABLED - #include "stm32h7xx_hal_gpio.h" +#include "stm32h7xx_hal_gpio.h" #endif /* HAL_GPIO_MODULE_ENABLED */ #ifdef HAL_DMA_MODULE_ENABLED - #include "stm32h7xx_hal_dma.h" +#include "stm32h7xx_hal_dma.h" #endif /* HAL_DMA_MODULE_ENABLED */ #ifdef HAL_MDMA_MODULE_ENABLED - #include "stm32h7xx_hal_mdma.h" +#include "stm32h7xx_hal_mdma.h" #endif /* HAL_MDMA_MODULE_ENABLED */ #ifdef HAL_HASH_MODULE_ENABLED - #include "stm32h7xx_hal_hash.h" +#include "stm32h7xx_hal_hash.h" #endif /* HAL_HASH_MODULE_ENABLED */ #ifdef HAL_DCMI_MODULE_ENABLED - #include "stm32h7xx_hal_dcmi.h" +#include "stm32h7xx_hal_dcmi.h" #endif /* HAL_DCMI_MODULE_ENABLED */ #ifdef HAL_DMA2D_MODULE_ENABLED - #include "stm32h7xx_hal_dma2d.h" +#include "stm32h7xx_hal_dma2d.h" #endif /* HAL_DMA2D_MODULE_ENABLED */ #ifdef HAL_DSI_MODULE_ENABLED - #include "stm32h7xx_hal_dsi.h" +#include "stm32h7xx_hal_dsi.h" #endif /* HAL_DSI_MODULE_ENABLED */ #ifdef HAL_DFSDM_MODULE_ENABLED - #include "stm32h7xx_hal_dfsdm.h" +#include "stm32h7xx_hal_dfsdm.h" #endif /* HAL_DFSDM_MODULE_ENABLED */ #ifdef HAL_DTS_MODULE_ENABLED - #include "stm32h7xx_hal_dts.h" +#include "stm32h7xx_hal_dts.h" #endif /* HAL_DTS_MODULE_ENABLED */ #ifdef HAL_ETH_MODULE_ENABLED - #include "stm32h7xx_hal_eth.h" +#include "stm32h7xx_hal_eth.h" #endif /* HAL_ETH_MODULE_ENABLED */ #ifdef HAL_EXTI_MODULE_ENABLED - #include "stm32h7xx_hal_exti.h" +#include "stm32h7xx_hal_exti.h" #endif /* HAL_EXTI_MODULE_ENABLED */ #ifdef HAL_CORTEX_MODULE_ENABLED - #include "stm32h7xx_hal_cortex.h" +#include "stm32h7xx_hal_cortex.h" #endif /* HAL_CORTEX_MODULE_ENABLED */ #ifdef HAL_ADC_MODULE_ENABLED - #include "stm32h7xx_hal_adc.h" +#include "stm32h7xx_hal_adc.h" #endif /* HAL_ADC_MODULE_ENABLED */ #ifdef HAL_FDCAN_MODULE_ENABLED - #include "stm32h7xx_hal_fdcan.h" +#include "stm32h7xx_hal_fdcan.h" #endif /* HAL_FDCAN_MODULE_ENABLED */ #ifdef HAL_CEC_MODULE_ENABLED - #include "stm32h7xx_hal_cec.h" +#include "stm32h7xx_hal_cec.h" #endif /* HAL_CEC_MODULE_ENABLED */ #ifdef HAL_COMP_MODULE_ENABLED - #include "stm32h7xx_hal_comp.h" +#include "stm32h7xx_hal_comp.h" #endif /* HAL_COMP_MODULE_ENABLED */ #ifdef HAL_CORDIC_MODULE_ENABLED - #include "stm32h7xx_hal_cordic.h" +#include "stm32h7xx_hal_cordic.h" #endif /* HAL_CORDIC_MODULE_ENABLED */ #ifdef HAL_CRC_MODULE_ENABLED - #include "stm32h7xx_hal_crc.h" +#include "stm32h7xx_hal_crc.h" #endif /* HAL_CRC_MODULE_ENABLED */ #ifdef HAL_CRYP_MODULE_ENABLED - #include "stm32h7xx_hal_cryp.h" +#include "stm32h7xx_hal_cryp.h" #endif /* HAL_CRYP_MODULE_ENABLED */ #ifdef HAL_DAC_MODULE_ENABLED - #include "stm32h7xx_hal_dac.h" +#include "stm32h7xx_hal_dac.h" #endif /* HAL_DAC_MODULE_ENABLED */ #ifdef HAL_FLASH_MODULE_ENABLED - #include "stm32h7xx_hal_flash.h" +#include "stm32h7xx_hal_flash.h" #endif /* HAL_FLASH_MODULE_ENABLED */ #ifdef HAL_GFXMMU_MODULE_ENABLED - #include "stm32h7xx_hal_gfxmmu.h" +#include "stm32h7xx_hal_gfxmmu.h" #endif /* HAL_GFXMMU_MODULE_ENABLED */ #ifdef HAL_FMAC_MODULE_ENABLED - #include "stm32h7xx_hal_fmac.h" +#include "stm32h7xx_hal_fmac.h" #endif /* HAL_FMAC_MODULE_ENABLED */ #ifdef HAL_HRTIM_MODULE_ENABLED - #include "stm32h7xx_hal_hrtim.h" +#include "stm32h7xx_hal_hrtim.h" #endif /* HAL_HRTIM_MODULE_ENABLED */ #ifdef HAL_HSEM_MODULE_ENABLED - #include "stm32h7xx_hal_hsem.h" +#include "stm32h7xx_hal_hsem.h" #endif /* HAL_HSEM_MODULE_ENABLED */ #ifdef HAL_SRAM_MODULE_ENABLED - #include "stm32h7xx_hal_sram.h" +#include "stm32h7xx_hal_sram.h" #endif /* HAL_SRAM_MODULE_ENABLED */ #ifdef HAL_NOR_MODULE_ENABLED - #include "stm32h7xx_hal_nor.h" +#include "stm32h7xx_hal_nor.h" #endif /* HAL_NOR_MODULE_ENABLED */ #ifdef HAL_NAND_MODULE_ENABLED - #include "stm32h7xx_hal_nand.h" +#include "stm32h7xx_hal_nand.h" #endif /* HAL_NAND_MODULE_ENABLED */ #ifdef HAL_I2C_MODULE_ENABLED - #include "stm32h7xx_hal_i2c.h" +#include "stm32h7xx_hal_i2c.h" #endif /* HAL_I2C_MODULE_ENABLED */ #ifdef HAL_I2S_MODULE_ENABLED - #include "stm32h7xx_hal_i2s.h" +#include "stm32h7xx_hal_i2s.h" #endif /* HAL_I2S_MODULE_ENABLED */ #ifdef HAL_IWDG_MODULE_ENABLED - #include "stm32h7xx_hal_iwdg.h" +#include "stm32h7xx_hal_iwdg.h" #endif /* HAL_IWDG_MODULE_ENABLED */ #ifdef HAL_JPEG_MODULE_ENABLED - #include "stm32h7xx_hal_jpeg.h" +#include "stm32h7xx_hal_jpeg.h" #endif /* HAL_JPEG_MODULE_ENABLED */ #ifdef HAL_MDIOS_MODULE_ENABLED - #include "stm32h7xx_hal_mdios.h" +#include "stm32h7xx_hal_mdios.h" #endif /* HAL_MDIOS_MODULE_ENABLED */ #ifdef HAL_MMC_MODULE_ENABLED - #include "stm32h7xx_hal_mmc.h" +#include "stm32h7xx_hal_mmc.h" #endif /* HAL_MMC_MODULE_ENABLED */ #ifdef HAL_LPTIM_MODULE_ENABLED @@ -394,7 +392,7 @@ #endif /* HAL_OPAMP_MODULE_ENABLED */ #ifdef HAL_OSPI_MODULE_ENABLED - #include "stm32h7xx_hal_ospi.h" +#include "stm32h7xx_hal_ospi.h" #endif /* HAL_OSPI_MODULE_ENABLED */ #ifdef HAL_OTFDEC_MODULE_ENABLED @@ -402,104 +400,104 @@ #endif /* HAL_OTFDEC_MODULE_ENABLED */ #ifdef HAL_PSSI_MODULE_ENABLED - #include "stm32h7xx_hal_pssi.h" +#include "stm32h7xx_hal_pssi.h" #endif /* HAL_PSSI_MODULE_ENABLED */ #ifdef HAL_PWR_MODULE_ENABLED - #include "stm32h7xx_hal_pwr.h" +#include "stm32h7xx_hal_pwr.h" #endif /* HAL_PWR_MODULE_ENABLED */ #ifdef HAL_QSPI_MODULE_ENABLED - #include "stm32h7xx_hal_qspi.h" +#include "stm32h7xx_hal_qspi.h" #endif /* HAL_QSPI_MODULE_ENABLED */ #ifdef HAL_RAMECC_MODULE_ENABLED - #include "stm32h7xx_hal_ramecc.h" +#include "stm32h7xx_hal_ramecc.h" #endif /* HAL_RAMECC_MODULE_ENABLED */ #ifdef HAL_RNG_MODULE_ENABLED - #include "stm32h7xx_hal_rng.h" +#include "stm32h7xx_hal_rng.h" #endif /* HAL_RNG_MODULE_ENABLED */ #ifdef HAL_RTC_MODULE_ENABLED - #include "stm32h7xx_hal_rtc.h" +#include "stm32h7xx_hal_rtc.h" #endif /* HAL_RTC_MODULE_ENABLED */ #ifdef HAL_SAI_MODULE_ENABLED - #include "stm32h7xx_hal_sai.h" +#include "stm32h7xx_hal_sai.h" #endif /* HAL_SAI_MODULE_ENABLED */ #ifdef HAL_SD_MODULE_ENABLED - #include "stm32h7xx_hal_sd.h" +#include "stm32h7xx_hal_sd.h" #endif /* HAL_SD_MODULE_ENABLED */ #ifdef HAL_SDRAM_MODULE_ENABLED - #include "stm32h7xx_hal_sdram.h" +#include "stm32h7xx_hal_sdram.h" #endif /* HAL_SDRAM_MODULE_ENABLED */ #ifdef HAL_SPI_MODULE_ENABLED - #include "stm32h7xx_hal_spi.h" +#include "stm32h7xx_hal_spi.h" #endif /* HAL_SPI_MODULE_ENABLED */ #ifdef HAL_SPDIFRX_MODULE_ENABLED - #include "stm32h7xx_hal_spdifrx.h" +#include "stm32h7xx_hal_spdifrx.h" #endif /* HAL_SPDIFRX_MODULE_ENABLED */ #ifdef HAL_SWPMI_MODULE_ENABLED - #include "stm32h7xx_hal_swpmi.h" +#include "stm32h7xx_hal_swpmi.h" #endif /* HAL_SWPMI_MODULE_ENABLED */ #ifdef HAL_TIM_MODULE_ENABLED - #include "stm32h7xx_hal_tim.h" +#include "stm32h7xx_hal_tim.h" #endif /* HAL_TIM_MODULE_ENABLED */ #ifdef HAL_UART_MODULE_ENABLED - #include "stm32h7xx_hal_uart.h" +#include "stm32h7xx_hal_uart.h" #endif /* HAL_UART_MODULE_ENABLED */ #ifdef HAL_USART_MODULE_ENABLED - #include "stm32h7xx_hal_usart.h" +#include "stm32h7xx_hal_usart.h" #endif /* HAL_USART_MODULE_ENABLED */ #ifdef HAL_IRDA_MODULE_ENABLED - #include "stm32h7xx_hal_irda.h" +#include "stm32h7xx_hal_irda.h" #endif /* HAL_IRDA_MODULE_ENABLED */ #ifdef HAL_SMARTCARD_MODULE_ENABLED - #include "stm32h7xx_hal_smartcard.h" +#include "stm32h7xx_hal_smartcard.h" #endif /* HAL_SMARTCARD_MODULE_ENABLED */ #ifdef HAL_SMBUS_MODULE_ENABLED - #include "stm32h7xx_hal_smbus.h" +#include "stm32h7xx_hal_smbus.h" #endif /* HAL_SMBUS_MODULE_ENABLED */ #ifdef HAL_WWDG_MODULE_ENABLED - #include "stm32h7xx_hal_wwdg.h" +#include "stm32h7xx_hal_wwdg.h" #endif /* HAL_WWDG_MODULE_ENABLED */ #ifdef HAL_PCD_MODULE_ENABLED - #include "stm32h7xx_hal_pcd.h" +#include "stm32h7xx_hal_pcd.h" #endif /* HAL_PCD_MODULE_ENABLED */ #ifdef HAL_HCD_MODULE_ENABLED - #include "stm32h7xx_hal_hcd.h" +#include "stm32h7xx_hal_hcd.h" #endif /* HAL_HCD_MODULE_ENABLED */ /* Exported macro ------------------------------------------------------------*/ -#ifdef USE_FULL_ASSERT +#ifdef USE_FULL_ASSERT /** - * @brief The assert_param macro is used for function's parameters check. - * @param expr: If expr is false, it calls assert_failed function - * which reports the name of the source file and the source - * line number of the call that failed. - * If expr is true, it returns no value. - * @retval None - */ - #define assert_param(expr) ((expr) ? (void)0U : assert_failed((uint8_t *)__FILE__, __LINE__)) + * @brief The assert_param macro is used for function's parameters check. + * @param expr: If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * @retval None + */ +#define assert_param(expr) ((expr) ? (void)0U : assert_failed((uint8_t*)__FILE__, __LINE__)) /* Exported functions ------------------------------------------------------- */ - void assert_failed(uint8_t *file, uint32_t line); +void assert_failed(uint8_t* file, uint32_t line); #else - #define assert_param(expr) ((void)0U) +#define assert_param(expr) ((void)0U) #endif /* USE_FULL_ASSERT */ #ifdef __cplusplus @@ -507,6 +505,3 @@ #endif #endif /* STM32H7xx_HAL_CONF_H */ - - - diff --git a/README.md b/README.md index 6cacdd7ed..284143617 100644 --- a/README.md +++ b/README.md @@ -25,3 +25,17 @@ git submodule update --init Drivers/STM32H7xx_HAL_Driver Drivers/CMSIS/Device/ST ``` The submodules initialization is also available by executing [this script](tools/init-submodules.sh) + +## Automatic formatting (commit/push) +This repository uses `pre-commit` + `clang-format` to format C/C++ files automatically on `git commit` and `git push`. + +Install hooks once: +```sh +pip install pre-commit +./tools/install-git-hooks.sh +``` + +Run manually on all files: +```sh +pre-commit run --all-files +``` diff --git a/Src/HALAL/HALAL.cpp b/Src/HALAL/HALAL.cpp index 29b087ec9..c56e3795d 100644 --- a/Src/HALAL/HALAL.cpp +++ b/Src/HALAL/HALAL.cpp @@ -11,29 +11,29 @@ #ifndef STLIB_ETH ETH_HandleTypeDef heth; -void HAL_ETH_IRQHandler(ETH_HandleTypeDef *heth_arg) { (void)heth_arg; } +void HAL_ETH_IRQHandler(ETH_HandleTypeDef* heth_arg) { (void)heth_arg; } #endif // STLIB_ETH namespace HALAL { -static void common_start(UART::Peripheral &printf_peripheral) { +static void common_start(UART::Peripheral& printf_peripheral) { #ifdef HAL_IWDG_MODULE_ENABLED - Watchdog::check_reset_flag(); + Watchdog::check_reset_flag(); #endif - HAL_Init(); - HALconfig::system_clock(); - HALconfig::peripheral_clock(); + HAL_Init(); + HALconfig::system_clock(); + HALconfig::peripheral_clock(); #ifdef HAL_UART_MODULE_ENABLED - UART::set_up_printf(printf_peripheral); + UART::set_up_printf(printf_peripheral); #endif #ifdef HAL_GPIO_MODULE_ENABLED - Pin::start(); + Pin::start(); #endif #ifdef HAL_DMA_MODULE_ENABLED - // DMA::start(); + // DMA::start(); #endif #ifdef HAL_MDMA_MODULE_ENABLED @@ -41,71 +41,68 @@ static void common_start(UART::Peripheral &printf_peripheral) { #endif #ifdef HAL_FMAC_MODULE_ENABLED - MultiplierAccelerator::start(); + MultiplierAccelerator::start(); #endif #ifdef HAL_CORDIC_MODULE_ENABLED - CORDIC_HandleTypeDef hcordic; - hcordic.Instance = CORDIC; - if (HAL_CORDIC_Init(&hcordic) != HAL_OK) { - ErrorHandler("Unable to init CORDIC"); - } + CORDIC_HandleTypeDef hcordic; + hcordic.Instance = CORDIC; + if (HAL_CORDIC_Init(&hcordic) != HAL_OK) { + ErrorHandler("Unable to init CORDIC"); + } #endif #ifdef HAL_I2C_MODULE_ENABLED - I2C::start(); + I2C::start(); #endif #ifdef HAL_SPI_MODULE_ENABLED - SPI::start(); + SPI::start(); #endif #ifdef HAL_UART_MODULE_ENABLED - UART::start(); + UART::start(); #endif #ifdef HAL_FDCAN_MODULE_ENABLED - FDCAN::start(); + FDCAN::start(); #endif #ifdef HAL_TIM_MODULE_ENABLED - // Encoder::start(); - Global_RTC::start_rtc(); - //TimerPeripheral::start(); - //Time::start(); + // Encoder::start(); + Global_RTC::start_rtc(); + // TimerPeripheral::start(); + // Time::start(); #endif #ifdef HAL_EXTI_MODULE_ENABLED - ExternalInterrupt::start(); + ExternalInterrupt::start(); #endif #ifdef NDEBUG #ifdef HAL_IWDG_MODULE_ENABLED - Watchdog::start(); + Watchdog::start(); #endif #endif } #ifdef STLIB_ETH -void start(MAC mac, IPV4 ip, IPV4 subnet_mask, IPV4 gateway, - UART::Peripheral &printf_peripheral) { - Ethernet::inscribe(); +void start(MAC mac, IPV4 ip, IPV4 subnet_mask, IPV4 gateway, UART::Peripheral& printf_peripheral) { + Ethernet::inscribe(); - common_start(printf_peripheral); + common_start(printf_peripheral); - Ethernet::start(mac, ip, subnet_mask, gateway); + Ethernet::start(mac, ip, subnet_mask, gateway); #ifdef HAL_TIM_MODULE_ENABLED - SNTP::sntp_update(); + SNTP::sntp_update(); #endif } #else // !STLIB_ETH -void start(UART::Peripheral &printf_peripheral) { - common_start(printf_peripheral); -} +void start(UART::Peripheral& printf_peripheral) { common_start(printf_peripheral); } #endif // STLIB_ETH diff --git a/Src/HALAL/HardFault/HardfaultTrace.c b/Src/HALAL/HardFault/HardfaultTrace.c index 8f5a9ad45..38721d6f7 100644 --- a/Src/HALAL/HardFault/HardfaultTrace.c +++ b/Src/HALAL/HardFault/HardfaultTrace.c @@ -6,7 +6,7 @@ #ifdef BOARD #define REPS 600000 // Three times faster because the board frequency #endif - + extern GPIO_TypeDef* ports_hard_fault[]; extern uint16_t pins_hard_fault[]; extern uint8_t hard_fault_leds_count; @@ -14,56 +14,62 @@ extern uint8_t hard_fault_leds_count; static void LED_Blink(); static void LED_init(void); static void EnableGPIOClock(GPIO_TypeDef* port); -static void InitGPIO_Output(GPIO_TypeDef* port,uint16_t pin); +static void InitGPIO_Output(GPIO_TypeDef* port, uint16_t pin); static void delay(); -__attribute__((optimize("O0"))) -static void delay(){ - for(volatile uint64_t i = 0; i < REPS; i++){ - __NOP(); +__attribute__((optimize("O0"))) static void delay() { + for (volatile uint64_t i = 0; i < REPS; i++) { + __NOP(); } } -static void LED_Blink(){ - for(volatile int i = 0; i < hard_fault_leds_count;i++){ - LL_GPIO_TogglePin(ports_hard_fault[i],pins_hard_fault[i]); +static void LED_Blink() { + for (volatile int i = 0; i < hard_fault_leds_count; i++) { + LL_GPIO_TogglePin(ports_hard_fault[i], pins_hard_fault[i]); } delay(); } -static void LED_init(void){ - for(int i = 0; i < hard_fault_leds_count;i++){ +static void LED_init(void) { + for (int i = 0; i < hard_fault_leds_count; i++) { EnableGPIOClock(ports_hard_fault[i]); - InitGPIO_Output(ports_hard_fault[i],pins_hard_fault[i]); - } + InitGPIO_Output(ports_hard_fault[i], pins_hard_fault[i]); + } } -void Hard_fault_check(void){ - if(*(volatile uint32_t*)HF_FLASH_ADDR == HF_FLAG_VALUE){ +void Hard_fault_check(void) { + if (*(volatile uint32_t*)HF_FLASH_ADDR == HF_FLAG_VALUE) { volatile HardFaultLog log; - memcpy(&log,(void*)HF_FLASH_ADDR,sizeof(HardFaultLog)); - #ifdef DEBUG - __asm__ __volatile__("bkpt 1"); - #endif + memcpy(&log, (void*)HF_FLASH_ADDR, sizeof(HardFaultLog)); +#ifdef DEBUG + __asm__ __volatile__("bkpt 1"); +#endif LED_init(); - while(1){ - LED_Blink(); + while (1) { + LED_Blink(); } - } } -static void EnableGPIOClock(GPIO_TypeDef* port){ - if(port == GPIOA) LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOA); - else if(port == GPIOB) LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOB); - else if(port == GPIOC) LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOC); - else if(port == GPIOD) LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOD); - else if(port == GPIOE) LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOE); - else if(port == GPIOF) LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOF); - else if(port == GPIOG) LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOG); - else if(port == GPIOJ) LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOJ); - else if(port == GPIOK) LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOK); +static void EnableGPIOClock(GPIO_TypeDef* port) { + if (port == GPIOA) + LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOA); + else if (port == GPIOB) + LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOB); + else if (port == GPIOC) + LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOC); + else if (port == GPIOD) + LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOD); + else if (port == GPIOE) + LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOE); + else if (port == GPIOF) + LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOF); + else if (port == GPIOG) + LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOG); + else if (port == GPIOJ) + LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOJ); + else if (port == GPIOK) + LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOK); } -static void InitGPIO_Output(GPIO_TypeDef* port,uint16_t pin){ +static void InitGPIO_Output(GPIO_TypeDef* port, uint16_t pin) { LL_GPIO_SetPinMode(port, pin, LL_GPIO_MODE_OUTPUT); LL_GPIO_SetPinOutputType(port, pin, LL_GPIO_OUTPUT_PUSHPULL); LL_GPIO_SetPinSpeed(port, pin, LL_GPIO_SPEED_FREQ_LOW); LL_GPIO_SetPinPull(port, pin, LL_GPIO_PULL_NO); } - diff --git a/Src/HALAL/Models/DMA/DMA.cpp b/Src/HALAL/Models/DMA/DMA.cpp index d0f9a95f5..32974dfad 100644 --- a/Src/HALAL/Models/DMA/DMA.cpp +++ b/Src/HALAL/Models/DMA/DMA.cpp @@ -4,39 +4,52 @@ * Created on: 10 dic. 2022 * Author: aleja */ - - #include "HALAL/Models/DMA/DMA.hpp" #include "ErrorHandler/ErrorHandler.hpp" vector DMA::available_streams = { - DMA1Stream0, DMA1Stream1, DMA1Stream2, DMA1Stream3, DMA1Stream4, DMA1Stream5, DMA1Stream6, DMA2Stream0, DMA2Stream1, DMA2Stream2, DMA2Stream3, DMA2Stream4, DMA2Stream5, DMA2Stream6, DMA2Stream7, + DMA1Stream0, + DMA1Stream1, + DMA1Stream2, + DMA1Stream3, + DMA1Stream4, + DMA1Stream5, + DMA1Stream6, + DMA2Stream0, + DMA2Stream1, + DMA2Stream2, + DMA2Stream3, + DMA2Stream4, + DMA2Stream5, + DMA2Stream6, + DMA2Stream7, }; vector DMA::inscribed_streams = {}; void DMA::inscribe_stream() { - if (available_streams.empty()) { - ErrorHandler("There are not any DMA Streams availables"); - return; - } - inscribed_streams.push_back(available_streams.back()); - available_streams.pop_back(); + if (available_streams.empty()) { + ErrorHandler("There are not any DMA Streams availables"); + return; + } + inscribed_streams.push_back(available_streams.back()); + available_streams.pop_back(); } void DMA::inscribe_stream(Stream dma_stream) { - if (std::find(available_streams.begin(), available_streams.end(), dma_stream) == available_streams.end()) { - ErrorHandler("The DMA stream %d is not available", dma_stream); - return; - } - inscribed_streams.push_back(dma_stream); + if (std::find(available_streams.begin(), available_streams.end(), dma_stream) == + available_streams.end()) { + ErrorHandler("The DMA stream %d is not available", dma_stream); + return; + } + inscribed_streams.push_back(dma_stream); } void DMA::start() { - __HAL_RCC_DMA1_CLK_ENABLE(); - __HAL_RCC_DMA2_CLK_ENABLE(); - for (Stream dma_stream : inscribed_streams) { - HAL_NVIC_SetPriority( (IRQn_Type)dma_stream, 0, 0); - HAL_NVIC_EnableIRQ( (IRQn_Type)dma_stream); - } + __HAL_RCC_DMA1_CLK_ENABLE(); + __HAL_RCC_DMA2_CLK_ENABLE(); + for (Stream dma_stream : inscribed_streams) { + HAL_NVIC_SetPriority((IRQn_Type)dma_stream, 0, 0); + HAL_NVIC_EnableIRQ((IRQn_Type)dma_stream); + } } \ No newline at end of file diff --git a/Src/HALAL/Models/DMA/DMA2.cpp b/Src/HALAL/Models/DMA/DMA2.cpp index 27e1c9ac4..eb25e132b 100644 --- a/Src/HALAL/Models/DMA/DMA2.cpp +++ b/Src/HALAL/Models/DMA/DMA2.cpp @@ -1,66 +1,33 @@ #include "HALAL/Models/DMA/DMA2.hpp" +extern "C" void DMA1_Stream0_IRQHandler(void) { HAL_DMA_IRQHandler(dma_irq_table[0]); } -extern "C" void DMA1_Stream0_IRQHandler(void) { - HAL_DMA_IRQHandler(dma_irq_table[0]); -} +extern "C" void DMA1_Stream1_IRQHandler(void) { HAL_DMA_IRQHandler(dma_irq_table[1]); } -extern "C" void DMA1_Stream1_IRQHandler(void) { - HAL_DMA_IRQHandler(dma_irq_table[1]); -} +extern "C" void DMA1_Stream2_IRQHandler(void) { HAL_DMA_IRQHandler(dma_irq_table[2]); } -extern "C" void DMA1_Stream2_IRQHandler(void) { - HAL_DMA_IRQHandler(dma_irq_table[2]); -} +extern "C" void DMA1_Stream3_IRQHandler(void) { HAL_DMA_IRQHandler(dma_irq_table[3]); } -extern "C" void DMA1_Stream3_IRQHandler(void) { - HAL_DMA_IRQHandler(dma_irq_table[3]); -} +extern "C" void DMA1_Stream4_IRQHandler(void) { HAL_DMA_IRQHandler(dma_irq_table[4]); } -extern "C" void DMA1_Stream4_IRQHandler(void) { - HAL_DMA_IRQHandler(dma_irq_table[4]); -} +extern "C" void DMA1_Stream5_IRQHandler(void) { HAL_DMA_IRQHandler(dma_irq_table[5]); } -extern "C" void DMA1_Stream5_IRQHandler(void) { - HAL_DMA_IRQHandler(dma_irq_table[5]); -} +extern "C" void DMA1_Stream6_IRQHandler(void) { HAL_DMA_IRQHandler(dma_irq_table[6]); } -extern "C" void DMA1_Stream6_IRQHandler(void) { - HAL_DMA_IRQHandler(dma_irq_table[6]); -} +extern "C" void DMA1_Stream7_IRQHandler(void) { HAL_DMA_IRQHandler(dma_irq_table[7]); } -extern "C" void DMA1_Stream7_IRQHandler(void) { - HAL_DMA_IRQHandler(dma_irq_table[7]); -} +extern "C" void DMA2_Stream0_IRQHandler(void) { HAL_DMA_IRQHandler(dma_irq_table[8]); } -extern "C" void DMA2_Stream0_IRQHandler(void) { - HAL_DMA_IRQHandler(dma_irq_table[8]); -} +extern "C" void DMA2_Stream1_IRQHandler(void) { HAL_DMA_IRQHandler(dma_irq_table[9]); } -extern "C" void DMA2_Stream1_IRQHandler(void) { - HAL_DMA_IRQHandler(dma_irq_table[9]); -} +extern "C" void DMA2_Stream2_IRQHandler(void) { HAL_DMA_IRQHandler(dma_irq_table[10]); } -extern "C" void DMA2_Stream2_IRQHandler(void) { - HAL_DMA_IRQHandler(dma_irq_table[10]); -} +extern "C" void DMA2_Stream3_IRQHandler(void) { HAL_DMA_IRQHandler(dma_irq_table[11]); } -extern "C" void DMA2_Stream3_IRQHandler(void) { - HAL_DMA_IRQHandler(dma_irq_table[11]); -} +extern "C" void DMA2_Stream4_IRQHandler(void) { HAL_DMA_IRQHandler(dma_irq_table[12]); } -extern "C" void DMA2_Stream4_IRQHandler(void) { - HAL_DMA_IRQHandler(dma_irq_table[12]); -} +extern "C" void DMA2_Stream5_IRQHandler(void) { HAL_DMA_IRQHandler(dma_irq_table[13]); } -extern "C" void DMA2_Stream5_IRQHandler(void) { - HAL_DMA_IRQHandler(dma_irq_table[13]); -} +extern "C" void DMA2_Stream6_IRQHandler(void) { HAL_DMA_IRQHandler(dma_irq_table[14]); } -extern "C" void DMA2_Stream6_IRQHandler(void) { - HAL_DMA_IRQHandler(dma_irq_table[14]); -} - -extern "C" void DMA2_Stream7_IRQHandler(void) { - HAL_DMA_IRQHandler(dma_irq_table[15]); -} +extern "C" void DMA2_Stream7_IRQHandler(void) { HAL_DMA_IRQHandler(dma_irq_table[15]); } diff --git a/Src/HALAL/Models/HALconfig/Halconfig.cpp b/Src/HALAL/Models/HALconfig/Halconfig.cpp index d9773d376..14c0d9c3b 100644 --- a/Src/HALAL/Models/HALconfig/Halconfig.cpp +++ b/Src/HALAL/Models/HALconfig/Halconfig.cpp @@ -8,121 +8,119 @@ #include "HALAL/Models/HALconfig/HALconfig.hpp" void HALconfig::system_clock() { - RCC_OscInitTypeDef RCC_OscInitStruct = {0}; - RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; + RCC_OscInitTypeDef RCC_OscInitStruct = {0}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; - HAL_PWREx_ConfigSupply(PWR_LDO_SUPPLY); - __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0); + HAL_PWREx_ConfigSupply(PWR_LDO_SUPPLY); + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0); - while(!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {} + while (!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) { + } - __HAL_RCC_PLL_PLLSOURCE_CONFIG(RCC_PLLSOURCE_HSE); + __HAL_RCC_PLL_PLLSOURCE_CONFIG(RCC_PLLSOURCE_HSE); - RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_LSI|RCC_OSCILLATORTYPE_HSE; - RCC_OscInitStruct.LSIState = RCC_LSI_ON; - RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; - RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_LSI | RCC_OSCILLATORTYPE_HSE; + RCC_OscInitStruct.LSIState = RCC_LSI_ON; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; #ifdef NUCLEO - RCC_OscInitStruct.HSEState = RCC_HSE_BYPASS; - RCC_OscInitStruct.PLL.PLLM = 4; - RCC_OscInitStruct.PLL.PLLN = 275; - RCC_OscInitStruct.PLL.PLLP = 1; - RCC_OscInitStruct.PLL.PLLQ = 4; - RCC_OscInitStruct.PLL.PLLR = 2; - RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_1; - RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE; - RCC_OscInitStruct.PLL.PLLFRACN = 0; + RCC_OscInitStruct.HSEState = RCC_HSE_BYPASS; + RCC_OscInitStruct.PLL.PLLM = 4; + RCC_OscInitStruct.PLL.PLLN = 275; + RCC_OscInitStruct.PLL.PLLP = 1; + RCC_OscInitStruct.PLL.PLLQ = 4; + RCC_OscInitStruct.PLL.PLLR = 2; + RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_1; + RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE; + RCC_OscInitStruct.PLL.PLLFRACN = 0; #else #ifdef BOARD - RCC_OscInitStruct.HSEState = RCC_HSE_ON; - RCC_OscInitStruct.PLL.PLLM = 5; - RCC_OscInitStruct.PLL.PLLN = 110; - RCC_OscInitStruct.PLL.PLLP = 1; - RCC_OscInitStruct.PLL.PLLQ = 4; - RCC_OscInitStruct.PLL.PLLR = 2; - RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_2; - RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE; - RCC_OscInitStruct.PLL.PLLFRACN = 0; + RCC_OscInitStruct.HSEState = RCC_HSE_ON; + RCC_OscInitStruct.PLL.PLLM = 5; + RCC_OscInitStruct.PLL.PLLN = 110; + RCC_OscInitStruct.PLL.PLLP = 1; + RCC_OscInitStruct.PLL.PLLQ = 4; + RCC_OscInitStruct.PLL.PLLR = 2; + RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_2; + RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE; + RCC_OscInitStruct.PLL.PLLFRACN = 0; #else -static_assert(false, "No TARGET is choosen. Choose NUCLEO or BOARD"); + static_assert(false, "No TARGET is choosen. Choose NUCLEO or BOARD"); #endif #endif - if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { - ErrorHandler("The RCC Osc config did not start correctly"); - } - - RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK - |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2 - |RCC_CLOCKTYPE_D3PCLK1|RCC_CLOCKTYPE_D1PCLK1; - RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; - RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1; - RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2; - RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2; - RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2; - RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2; - RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2; - if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK) { - ErrorHandler("The RCC clock config did not start correctly"); - } + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { + ErrorHandler("The RCC Osc config did not start correctly"); + } + + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | + RCC_CLOCKTYPE_PCLK2 | RCC_CLOCKTYPE_D3PCLK1 | + RCC_CLOCKTYPE_D1PCLK1; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2; + RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2; + RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2; + RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2; + RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2; + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK) { + ErrorHandler("The RCC clock config did not start correctly"); + } } - - - void HALconfig::peripheral_clock() { - RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0}; + RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0}; #ifdef NUCLEO - PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_ADC|RCC_PERIPHCLK_FDCAN; - PeriphClkInitStruct.PLL2.PLL2M = 8; - PeriphClkInitStruct.PLL2.PLL2N = 200; - PeriphClkInitStruct.PLL2.PLL2P = 2; - PeriphClkInitStruct.PLL2.PLL2Q = 10; - PeriphClkInitStruct.PLL2.PLL2R = 2; - PeriphClkInitStruct.PLL2.PLL2RGE = RCC_PLL2VCIRANGE_0; - PeriphClkInitStruct.PLL2.PLL2VCOSEL = RCC_PLL2VCOMEDIUM; - PeriphClkInitStruct.PLL2.PLL2FRACN = 0; - PeriphClkInitStruct.PLL3.PLL3M = 8; - PeriphClkInitStruct.PLL3.PLL3N = 192; - PeriphClkInitStruct.PLL3.PLL3P = 2; - PeriphClkInitStruct.PLL3.PLL3Q = 2; - PeriphClkInitStruct.PLL3.PLL3R = 2; - PeriphClkInitStruct.PLL3.PLL3RGE = RCC_PLL3VCIRANGE_0; - PeriphClkInitStruct.PLL3.PLL3VCOSEL = RCC_PLL3VCOMEDIUM; - PeriphClkInitStruct.PLL3.PLL3FRACN = 0; - PeriphClkInitStruct.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL2; - PeriphClkInitStruct.AdcClockSelection = RCC_ADCCLKSOURCE_PLL3; + PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_ADC | RCC_PERIPHCLK_FDCAN; + PeriphClkInitStruct.PLL2.PLL2M = 8; + PeriphClkInitStruct.PLL2.PLL2N = 200; + PeriphClkInitStruct.PLL2.PLL2P = 2; + PeriphClkInitStruct.PLL2.PLL2Q = 10; + PeriphClkInitStruct.PLL2.PLL2R = 2; + PeriphClkInitStruct.PLL2.PLL2RGE = RCC_PLL2VCIRANGE_0; + PeriphClkInitStruct.PLL2.PLL2VCOSEL = RCC_PLL2VCOMEDIUM; + PeriphClkInitStruct.PLL2.PLL2FRACN = 0; + PeriphClkInitStruct.PLL3.PLL3M = 8; + PeriphClkInitStruct.PLL3.PLL3N = 192; + PeriphClkInitStruct.PLL3.PLL3P = 2; + PeriphClkInitStruct.PLL3.PLL3Q = 2; + PeriphClkInitStruct.PLL3.PLL3R = 2; + PeriphClkInitStruct.PLL3.PLL3RGE = RCC_PLL3VCIRANGE_0; + PeriphClkInitStruct.PLL3.PLL3VCOSEL = RCC_PLL3VCOMEDIUM; + PeriphClkInitStruct.PLL3.PLL3FRACN = 0; + PeriphClkInitStruct.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL2; + PeriphClkInitStruct.AdcClockSelection = RCC_ADCCLKSOURCE_PLL3; #else #ifdef BOARD - PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_ADC|RCC_PERIPHCLK_FDCAN; - PeriphClkInitStruct.PLL2.PLL2M = 25; - PeriphClkInitStruct.PLL2.PLL2N = 200; - PeriphClkInitStruct.PLL2.PLL2P = 2; - PeriphClkInitStruct.PLL2.PLL2Q = 10; - PeriphClkInitStruct.PLL2.PLL2R = 2; - PeriphClkInitStruct.PLL2.PLL2RGE = RCC_PLL2VCIRANGE_0; - PeriphClkInitStruct.PLL2.PLL2VCOSEL = RCC_PLL2VCOMEDIUM; - PeriphClkInitStruct.PLL2.PLL2FRACN = 0; - PeriphClkInitStruct.PLL3.PLL3M = 25; - PeriphClkInitStruct.PLL3.PLL3N = 192; - PeriphClkInitStruct.PLL3.PLL3P = 2; - PeriphClkInitStruct.PLL3.PLL3Q = 2; - PeriphClkInitStruct.PLL3.PLL3R = 2; - PeriphClkInitStruct.PLL3.PLL3RGE = RCC_PLL3VCIRANGE_0; - PeriphClkInitStruct.PLL3.PLL3VCOSEL = RCC_PLL3VCOMEDIUM; - PeriphClkInitStruct.PLL3.PLL3FRACN = 0; - PeriphClkInitStruct.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL2; - PeriphClkInitStruct.AdcClockSelection = RCC_ADCCLKSOURCE_PLL3; + PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_ADC | RCC_PERIPHCLK_FDCAN; + PeriphClkInitStruct.PLL2.PLL2M = 25; + PeriphClkInitStruct.PLL2.PLL2N = 200; + PeriphClkInitStruct.PLL2.PLL2P = 2; + PeriphClkInitStruct.PLL2.PLL2Q = 10; + PeriphClkInitStruct.PLL2.PLL2R = 2; + PeriphClkInitStruct.PLL2.PLL2RGE = RCC_PLL2VCIRANGE_0; + PeriphClkInitStruct.PLL2.PLL2VCOSEL = RCC_PLL2VCOMEDIUM; + PeriphClkInitStruct.PLL2.PLL2FRACN = 0; + PeriphClkInitStruct.PLL3.PLL3M = 25; + PeriphClkInitStruct.PLL3.PLL3N = 192; + PeriphClkInitStruct.PLL3.PLL3P = 2; + PeriphClkInitStruct.PLL3.PLL3Q = 2; + PeriphClkInitStruct.PLL3.PLL3R = 2; + PeriphClkInitStruct.PLL3.PLL3RGE = RCC_PLL3VCIRANGE_0; + PeriphClkInitStruct.PLL3.PLL3VCOSEL = RCC_PLL3VCOMEDIUM; + PeriphClkInitStruct.PLL3.PLL3FRACN = 0; + PeriphClkInitStruct.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL2; + PeriphClkInitStruct.AdcClockSelection = RCC_ADCCLKSOURCE_PLL3; #else - static_assert(false, "No TARGET is choosen. Choose NUCLEO or BOARD"); + static_assert(false, "No TARGET is choosen. Choose NUCLEO or BOARD"); #endif #endif - if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) { - ErrorHandler("The RCCEx peripheral clock did not start correctly"); - } + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) { + ErrorHandler("The RCCEx peripheral clock did not start correctly"); + } } diff --git a/Src/HALAL/Models/IPV4/IPV4.cpp b/Src/HALAL/Models/IPV4/IPV4.cpp index 759f881fb..7b2b754bf 100644 --- a/Src/HALAL/Models/IPV4/IPV4.cpp +++ b/Src/HALAL/Models/IPV4/IPV4.cpp @@ -1,54 +1,54 @@ #include "HALAL/Models/IPV4/IPV4.hpp" #ifdef HAL_ETH_MODULE_ENABLED -IPV4::IPV4(string address) : string_address(address){ - stringstream sstream(address); - int ip_bytes[4]; - for(int& byte : ip_bytes){ - string temp; - getline(sstream, temp, '.'); - byte = stoi(temp); - } - IP_ADDR4(&(this->address), ip_bytes[0], ip_bytes[1], ip_bytes[2], ip_bytes[3]); +IPV4::IPV4(string address) : string_address(address) { + stringstream sstream(address); + int ip_bytes[4]; + for (int& byte : ip_bytes) { + string temp; + getline(sstream, temp, '.'); + byte = stoi(temp); + } + IP_ADDR4(&(this->address), ip_bytes[0], ip_bytes[1], ip_bytes[2], ip_bytes[3]); } IPV4::IPV4(const char* address) { - char* temp_ip = (char*)malloc(strlen(address)+1); - strcpy(temp_ip,address); + char* temp_ip = (char*)malloc(strlen(address) + 1); + strcpy(temp_ip, address); string_address = temp_ip; - const char* token = strtok(temp_ip,"."); + const char* token = strtok(temp_ip, "."); int i = 0; uint8_t ip_bytes[4]; - while(token!=nullptr){ + while (token != nullptr) { ip_bytes[i++] = atoi(token); - token = strtok(nullptr,"."); + token = strtok(nullptr, "."); } free(temp_ip); - IP_ADDR4(&(this->address), ip_bytes[0], ip_bytes[1], ip_bytes[2], ip_bytes[3]); + IP_ADDR4(&(this->address), ip_bytes[0], ip_bytes[1], ip_bytes[2], ip_bytes[3]); } -IPV4::IPV4(ip_addr_t address) : address(address){ - string_address = std::to_string((u8_t) address.addr) + "." + std::to_string((u8_t) (address.addr >> 8))+ "." - + std::to_string((uint8_t) (address.addr >> 16)) + "." + std::to_string((uint8_t) (address.addr >> 24)); +IPV4::IPV4(ip_addr_t address) : address(address) { + string_address = std::to_string((u8_t)address.addr) + "." + + std::to_string((u8_t)(address.addr >> 8)) + "." + + std::to_string((uint8_t)(address.addr >> 16)) + "." + + std::to_string((uint8_t)(address.addr >> 24)); } IPV4::IPV4() = default; -void IPV4::operator =(const char* address){ - char* temp_ip = (char*)malloc(strlen(address)+1); - strcpy(temp_ip,address); +void IPV4::operator=(const char* address) { + char* temp_ip = (char*)malloc(strlen(address) + 1); + strcpy(temp_ip, address); string_address = temp_ip; - const char* token = strtok(temp_ip,"."); + const char* token = strtok(temp_ip, "."); int i = 0; uint8_t ip_bytes[4]; - while(token!=nullptr){ + while (token != nullptr) { ip_bytes[i++] = atoi(token); - token = strtok(nullptr,"."); + token = strtok(nullptr, "."); } free(temp_ip); - IP_ADDR4(&(this->address), ip_bytes[0], ip_bytes[1], ip_bytes[2], ip_bytes[3]); + IP_ADDR4(&(this->address), ip_bytes[0], ip_bytes[1], ip_bytes[2], ip_bytes[3]); } - - #endif diff --git a/Src/HALAL/Models/LowPowerTimer/LowPowerTimer.cpp b/Src/HALAL/Models/LowPowerTimer/LowPowerTimer.cpp index 2f2af0bd5..f1632f92b 100644 --- a/Src/HALAL/Models/LowPowerTimer/LowPowerTimer.cpp +++ b/Src/HALAL/Models/LowPowerTimer/LowPowerTimer.cpp @@ -9,17 +9,17 @@ #include "ErrorHandler/ErrorHandler.hpp" void LowPowerTimer::init() { - handle.Instance = &instance; - handle.Init.Clock.Source = LPTIM_CLOCKSOURCE_APBCLOCK_LPOSC; - handle.Init.Clock.Prescaler = LPTIM_PRESCALER_DIV1; - handle.Init.Trigger.Source = LPTIM_TRIGSOURCE_SOFTWARE; - handle.Init.OutputPolarity = LPTIM_OUTPUTPOLARITY_HIGH; - handle.Init.UpdateMode = LPTIM_UPDATE_IMMEDIATE; - handle.Init.CounterSource = LPTIM_COUNTERSOURCE_INTERNAL; - handle.Init.Input1Source = LPTIM_INPUT1SOURCE_GPIO; - handle.Init.Input2Source = LPTIM_INPUT2SOURCE_GPIO; + handle.Instance = &instance; + handle.Init.Clock.Source = LPTIM_CLOCKSOURCE_APBCLOCK_LPOSC; + handle.Init.Clock.Prescaler = LPTIM_PRESCALER_DIV1; + handle.Init.Trigger.Source = LPTIM_TRIGSOURCE_SOFTWARE; + handle.Init.OutputPolarity = LPTIM_OUTPUTPOLARITY_HIGH; + handle.Init.UpdateMode = LPTIM_UPDATE_IMMEDIATE; + handle.Init.CounterSource = LPTIM_COUNTERSOURCE_INTERNAL; + handle.Init.Input1Source = LPTIM_INPUT1SOURCE_GPIO; + handle.Init.Input2Source = LPTIM_INPUT2SOURCE_GPIO; - if (HAL_LPTIM_Init(&handle) != HAL_OK) { - ErrorHandler("The LPTIM %s could not be registered", name); - } + if (HAL_LPTIM_Init(&handle) != HAL_OK) { + ErrorHandler("The LPTIM %s could not be registered", name); + } } diff --git a/Src/HALAL/Models/MAC/MAC.cpp b/Src/HALAL/Models/MAC/MAC.cpp index 7de3e8833..37d5d2954 100644 --- a/Src/HALAL/Models/MAC/MAC.cpp +++ b/Src/HALAL/Models/MAC/MAC.cpp @@ -12,13 +12,12 @@ MAC::MAC(string address) : string_address(address) { } MAC::MAC(u8_t addr[6]) - : address{addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]}, - string_address([&]() { + : address{addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]}, string_address([&]() { stringstream sstream; for (int i = 0; i < 6; ++i) { - if (i > 0) sstream << ":"; - sstream << std::hex << std::setw(2) << std::setfill('0') - << static_cast(addr[i]); + if (i > 0) + sstream << ":"; + sstream << std::hex << std::setw(2) << std::setfill('0') << static_cast(addr[i]); } return sstream.str(); }()) {} diff --git a/Src/HALAL/Models/MDMA/MDMA.cpp b/Src/HALAL/Models/MDMA/MDMA.cpp index 328a28ca2..9dc71f1a7 100644 --- a/Src/HALAL/Models/MDMA/MDMA.cpp +++ b/Src/HALAL/Models/MDMA/MDMA.cpp @@ -18,11 +18,8 @@ uint8_t MDMA::get_instance_id(MDMA_Channel_TypeDef* channel) { return static_cast((address - (MDMA_BASE + 0x40UL)) / 0x40UL); } - -void MDMA::prepare_transfer(Instance& instance, MDMA_LinkNodeTypeDef* first_node) -{ - if (instance.handle.State == HAL_MDMA_STATE_BUSY ) - { +void MDMA::prepare_transfer(Instance& instance, MDMA_LinkNodeTypeDef* first_node) { + if (instance.handle.State == HAL_MDMA_STATE_BUSY) { ErrorHandler("MDMA transfer already in progress"); return; } @@ -44,41 +41,36 @@ void MDMA::prepare_transfer(Instance& instance, MDMA_LinkNodeTypeDef* first_node channel->CMDR = first_node->CMDR; channel->CLAR = first_node->CLAR; - const uint32_t clear_flags = MDMA_FLAG_TE | MDMA_FLAG_CTC | MDMA_FLAG_BT | MDMA_FLAG_BFTC | MDMA_FLAG_BRT; + const uint32_t clear_flags = + MDMA_FLAG_TE | MDMA_FLAG_CTC | MDMA_FLAG_BT | MDMA_FLAG_BFTC | MDMA_FLAG_BRT; __HAL_MDMA_CLEAR_FLAG(&instance.handle, clear_flags); __HAL_MDMA_ENABLE_IT(&instance.handle, MDMA_IT_TE | MDMA_IT_CTC); __HAL_MDMA_ENABLE(&instance.handle); - if (HAL_MDMA_GenerateSWRequest(&instance.handle) != HAL_OK) - { + if (HAL_MDMA_GenerateSWRequest(&instance.handle) != HAL_OK) { instance.handle.State = HAL_MDMA_STATE_BUSY; ErrorHandler("Error generating MDMA SW request"); return; } - } -void MDMA::prepare_transfer(Instance& instance, LinkedListNode* first_node) { MDMA::prepare_transfer(instance, first_node->get_node()); } - - +void MDMA::prepare_transfer(Instance& instance, LinkedListNode* first_node) { + MDMA::prepare_transfer(instance, first_node->get_node()); +} -MDMA::Instance& MDMA::get_instance(uint8_t id) -{ +MDMA::Instance& MDMA::get_instance(uint8_t id) { Instance& instance = instances[id]; return instance; } - -void MDMA::inscribe(Instance& instance,uint8_t id) -{ +void MDMA::inscribe(Instance& instance, uint8_t id) { MDMA_HandleTypeDef mdma_handle{}; MDMA_Channel_TypeDef* channel = get_channel(id); - if (channel == nullptr) - { + if (channel == nullptr) { ErrorHandler("MDMA channel mapping not found"); return; } @@ -121,39 +113,40 @@ void MDMA::inscribe(Instance& instance,uint8_t id) nodeConfig.DstAddress = reinterpret_cast(nullptr); const HAL_StatusTypeDef status = HAL_MDMA_LinkedList_CreateNode(transfer_node, &nodeConfig); - if (status != HAL_OK) - { + if (status != HAL_OK) { ErrorHandler("Error creating linked list in MDMA"); } instance_free_map[id] = true; instance = Instance(mdma_handle, id, nullptr, transfer_node); - } - -void MDMA::start() -{ +void MDMA::start() { __HAL_RCC_MDMA_CLK_ENABLE(); uint8_t id = 0; - for (auto& instance : instances) - { - inscribe(instance,id); + for (auto& instance : instances) { + inscribe(instance, id); id++; - - if (instance.handle.Instance == nullptr) - { + + if (instance.handle.Instance == nullptr) { ErrorHandler("MDMA instance not initialised"); } const HAL_StatusTypeDef status = HAL_MDMA_Init(&instance.handle); - if (status != HAL_OK) - { + if (status != HAL_OK) { ErrorHandler("Error initialising MDMA instance"); } - HAL_MDMA_RegisterCallback(&instance.handle, HAL_MDMA_XFER_CPLT_CB_ID, MDMA::TransferCompleteCallback); - HAL_MDMA_RegisterCallback(&instance.handle, HAL_MDMA_XFER_ERROR_CB_ID, MDMA::TransferErrorCallback); + HAL_MDMA_RegisterCallback( + &instance.handle, + HAL_MDMA_XFER_CPLT_CB_ID, + MDMA::TransferCompleteCallback + ); + HAL_MDMA_RegisterCallback( + &instance.handle, + HAL_MDMA_XFER_ERROR_CB_ID, + MDMA::TransferErrorCallback + ); __HAL_MDMA_ENABLE_IT(&instance.handle, MDMA_IT_TE | MDMA_IT_CTC); } @@ -161,21 +154,16 @@ void MDMA::start() HAL_NVIC_EnableIRQ(MDMA_IRQn); } -void MDMA::update() -{ - if(transfer_queue.empty()) - { +void MDMA::update() { + if (transfer_queue.empty()) { return; } - for(size_t i = 0; i < instances.size(); i++) - { - if(transfer_queue.empty()) - { + for (size_t i = 0; i < instances.size(); i++) { + if (transfer_queue.empty()) { return; } - if(instance_free_map[i]) - { + if (instance_free_map[i]) { Instance& instance = get_instance(i); auto transfer = transfer_queue.top(); instance.done = transfer.second; @@ -185,38 +173,31 @@ void MDMA::update() } } - -void MDMA::irq_handler() -{ - for (auto& instance : instances) - { - if (instance.handle.Instance == nullptr) - { +void MDMA::irq_handler() { + for (auto& instance : instances) { + if (instance.handle.Instance == nullptr) { continue; } HAL_MDMA_IRQHandler(&instance.handle); } } - -void MDMA::transfer_list(MDMA::LinkedListNode* first_node, volatile bool* done) -{ - if(transfer_queue.size() >= TRANSFER_QUEUE_MAX_SIZE) - { +void MDMA::transfer_list(MDMA::LinkedListNode* first_node, volatile bool* done) { + if (transfer_queue.size() >= TRANSFER_QUEUE_MAX_SIZE) { ErrorHandler("MDMA transfer queue full"); return; } transfer_queue.push({first_node, done}); } - - -void MDMA::transfer_data(uint8_t* source_address, uint8_t* destination_address, const uint32_t data_length, volatile bool* done) -{ - for(size_t i = 0; i < instances.size(); i++) - { - if(instance_free_map[i]) - { +void MDMA::transfer_data( + uint8_t* source_address, + uint8_t* destination_address, + const uint32_t data_length, + volatile bool* done +) { + for (size_t i = 0; i < instances.size(); i++) { + if (instance_free_map[i]) { Instance& instance = get_instance(i); instance.done = done; @@ -227,20 +208,16 @@ void MDMA::transfer_data(uint8_t* source_address, uint8_t* destination_address, prepare_transfer(instance, instance.transfer_node); return; } - if(done) - { - *done = false; - } + if (done) { + *done = false; + } } return; } - -void MDMA::TransferCompleteCallback(MDMA_HandleTypeDef *hmdma) -{ +void MDMA::TransferCompleteCallback(MDMA_HandleTypeDef* hmdma) { uint8_t id = get_instance_id(hmdma->Instance); - if (id >= instances.size()) - { + if (id >= instances.size()) { ErrorHandler("MDMA channel not registered"); return; } @@ -248,27 +225,21 @@ void MDMA::TransferCompleteCallback(MDMA_HandleTypeDef *hmdma) Instance& instance = get_instance(id); instance.handle.State = HAL_MDMA_STATE_READY; instance_free_map[instance.id] = true; - if(instance.done == nullptr) - { + if (instance.done == nullptr) { return; } *(instance.done) = true; - } - -void MDMA::TransferErrorCallback(MDMA_HandleTypeDef *hmdma) -{ +void MDMA::TransferErrorCallback(MDMA_HandleTypeDef* hmdma) { uint8_t id = get_instance_id(hmdma->Instance); - if (id >= instances.size()) - { + if (id >= instances.size()) { ErrorHandler("MDMA channel not registered"); return; } Instance& instance = get_instance(id); - if(instance.done != nullptr) - { + if (instance.done != nullptr) { *(instance.done) = false; } @@ -276,8 +247,4 @@ void MDMA::TransferErrorCallback(MDMA_HandleTypeDef *hmdma) ErrorHandler("MDMA Transfer Error, code: " + std::to_string(error_code)); } - -extern "C" void MDMA_IRQHandler(void) -{ - MDMA::irq_handler(); -} +extern "C" void MDMA_IRQHandler(void) { MDMA::irq_handler(); } diff --git a/Src/HALAL/Models/MPUManager/MPUManager.cpp b/Src/HALAL/Models/MPUManager/MPUManager.cpp index f3894c139..dad7395d0 100644 --- a/Src/HALAL/Models/MPUManager/MPUManager.cpp +++ b/Src/HALAL/Models/MPUManager/MPUManager.cpp @@ -1,7 +1,7 @@ #include "HALAL/Models/MPUManager/MPUManager.hpp" -__attribute__((section(".mpu_ram_d3_nc.legacy"))) alignas(32) -uint8_t mpu_manager_memory_pool[NO_CACHED_RAM_MAXIMUM_SPACE]; +__attribute__((section(".mpu_ram_d3_nc.legacy"))) alignas(32 +) uint8_t mpu_manager_memory_pool[NO_CACHED_RAM_MAXIMUM_SPACE]; void* MPUManager::no_cached_ram_start = mpu_manager_memory_pool; uint32_t MPUManager::no_cached_ram_occupied_bytes = 0; diff --git a/Src/HALAL/Models/Packets/Packet.cpp b/Src/HALAL/Models/Packets/Packet.cpp index 47c013263..57994987b 100644 --- a/Src/HALAL/Models/Packets/Packet.cpp +++ b/Src/HALAL/Models/Packets/Packet.cpp @@ -1,5 +1,5 @@ #include "HALAL/Models/Packets/Packet.hpp" #include "HALAL/Models/Packets/Order.hpp" -map Order::orders = {}; -map Packet::packets = {}; +map Order::orders = {}; +map Packet::packets = {}; diff --git a/Src/HALAL/Models/Packets/SPIOrder.cpp b/Src/HALAL/Models/Packets/SPIOrder.cpp index a4712e7c4..736b0fcbc 100644 --- a/Src/HALAL/Models/Packets/SPIOrder.cpp +++ b/Src/HALAL/Models/Packets/SPIOrder.cpp @@ -7,6 +7,6 @@ #include "HALAL/Models/Packets/SPIOrder.hpp" -map SPIBaseOrder::SPIOrdersByID {}; +map SPIBaseOrder::SPIOrdersByID{}; SPIBaseOrder::~SPIBaseOrder(){}; diff --git a/Src/HALAL/Models/PinModel/Pin.cpp b/Src/HALAL/Models/PinModel/Pin.cpp index da409644b..6beb2a566 100644 --- a/Src/HALAL/Models/PinModel/Pin.cpp +++ b/Src/HALAL/Models/PinModel/Pin.cpp @@ -11,38 +11,50 @@ Pin::Pin() {} -Pin::Pin(GPIOPort port, GPIOPin gpio_pin) - : port((GPIO_TypeDef*)port), gpio_pin(gpio_pin) {} +Pin::Pin(GPIOPort port, GPIOPin gpio_pin) : port((GPIO_TypeDef*)port), gpio_pin(gpio_pin) {} -Pin::Pin(GPIOPort port, GPIOPin gpio_pin, - AlternativeFunction alternative_function) - : port((GPIO_TypeDef*)port), - gpio_pin(gpio_pin), - alternative_function(alternative_function) {} +Pin::Pin(GPIOPort port, GPIOPin gpio_pin, AlternativeFunction alternative_function) + : port((GPIO_TypeDef*)port), gpio_pin(gpio_pin), alternative_function(alternative_function) {} const vector> Pin::pinVector = { - PA0, PA1, PA10, PA11, PA12, PA9, PB0, PB1, PB10, PB11, PB12, - PB13, PB14, PB15, PB2, PB4, PB5, PB6, PB7, PB8, PB9, PC0, - PC1, PC10, PC11, PC12, PC13, PC14, PC15, PC2, PC3, PC4, PC5, - PC6, PC7, PC8, PC9, PD0, PD1, PD10, PD11, PD12, PD13, PD14, - PD15, PD2, PD3, PD4, PD5, PD6, PD7, PD8, PD9, PE0, PE1, - PE10, PE11, PE12, PE13, PE14, PE15, PE2, PE3, PE4, PE5, PE6, - PE7, PE8, PE9, PF0, PF1, PF10, PF11, PF12, PF13, PF14, PF15, - PF2, PF3, PF4, PF5, PF6, PF7, PF8, PF9, PG0, PG1, PG10, - PG11, PG12, PG13, PG14, PG15, PG2, PG3, PG4, PG5, PG6, PG7, - PG8, PG9, PH0, PH1, PA2, PA3, PA4, PA5, PA6, PA7, PA8}; + PA0, PA1, PA10, PA11, PA12, PA9, PB0, PB1, PB10, PB11, PB12, PB13, PB14, PB15, PB2, PB4, + PB5, PB6, PB7, PB8, PB9, PC0, PC1, PC10, PC11, PC12, PC13, PC14, PC15, PC2, PC3, PC4, + PC5, PC6, PC7, PC8, PC9, PD0, PD1, PD10, PD11, PD12, PD13, PD14, PD15, PD2, PD3, PD4, + PD5, PD6, PD7, PD8, PD9, PE0, PE1, PE10, PE11, PE12, PE13, PE14, PE15, PE2, PE3, PE4, + PE5, PE6, PE7, PE8, PE9, PF0, PF1, PF10, PF11, PF12, PF13, PF14, PF15, PF2, PF3, PF4, + PF5, PF6, PF7, PF8, PF9, PG0, PG1, PG10, PG11, PG12, PG13, PG14, PG15, PG2, PG3, PG4, + PG5, PG6, PG7, PG8, PG9, PH0, PH1, PA2, PA3, PA4, PA5, PA6, PA7, PA8 +}; const map Pin::gpio_pin_to_string = { - {PIN_0, "0"}, {PIN_1, "1"}, {PIN_2, "2"}, {PIN_3, "3"}, - {PIN_4, "4"}, {PIN_5, "5"}, {PIN_6, "6"}, {PIN_7, "7"}, - {PIN_8, "8"}, {PIN_9, "9"}, {PIN_10, "10"}, {PIN_11, "11"}, - {PIN_12, "12"}, {PIN_13, "13"}, {PIN_14, "14"}, {PIN_15, "15"}, - {PIN_ALL, "ALL"}}; + {PIN_0, "0"}, + {PIN_1, "1"}, + {PIN_2, "2"}, + {PIN_3, "3"}, + {PIN_4, "4"}, + {PIN_5, "5"}, + {PIN_6, "6"}, + {PIN_7, "7"}, + {PIN_8, "8"}, + {PIN_9, "9"}, + {PIN_10, "10"}, + {PIN_11, "11"}, + {PIN_12, "12"}, + {PIN_13, "13"}, + {PIN_14, "14"}, + {PIN_15, "15"}, + {PIN_ALL, "ALL"} +}; const map Pin::port_to_string = { - {(GPIO_TypeDef*)PORT_A, "PA"}, {(GPIO_TypeDef*)PORT_B, "PB"}, - {(GPIO_TypeDef*)PORT_C, "PC"}, {(GPIO_TypeDef*)PORT_D, "PD"}, - {(GPIO_TypeDef*)PORT_E, "PE"}, {(GPIO_TypeDef*)PORT_F, "PF"}, - {(GPIO_TypeDef*)PORT_G, "PG"}, {(GPIO_TypeDef*)PORT_H, "PH"}}; + {(GPIO_TypeDef*)PORT_A, "PA"}, + {(GPIO_TypeDef*)PORT_B, "PB"}, + {(GPIO_TypeDef*)PORT_C, "PC"}, + {(GPIO_TypeDef*)PORT_D, "PD"}, + {(GPIO_TypeDef*)PORT_E, "PE"}, + {(GPIO_TypeDef*)PORT_F, "PF"}, + {(GPIO_TypeDef*)PORT_G, "PG"}, + {(GPIO_TypeDef*)PORT_H, "PH"} +}; const string Pin::to_string() const { return (port_to_string.at(port) + gpio_pin_to_string.at(gpio_pin)); @@ -50,8 +62,10 @@ const string Pin::to_string() const { void Pin::inscribe(Pin& pin, OperationMode mode) { if (pin.mode != OperationMode::NOT_USED) { - ErrorHandler("Pin %s is already registered, cannot register twice", - pin.to_string().c_str()); + ErrorHandler( + "Pin %s is already registered, cannot register twice", + pin.to_string().c_str() + ); return; } pin.mode = mode; @@ -71,52 +85,52 @@ void Pin::start() { GPIO_InitStruct = {0}; GPIO_InitStruct.Pin = pin.gpio_pin; switch (pin.mode) { - case OperationMode::NOT_USED: - break; + case OperationMode::NOT_USED: + break; - case OperationMode::OUTPUT: - GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - HAL_GPIO_Init(pin.port, &GPIO_InitStruct); - break; + case OperationMode::OUTPUT: + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + HAL_GPIO_Init(pin.port, &GPIO_InitStruct); + break; - case OperationMode::INPUT: - GPIO_InitStruct.Mode = GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(pin.port, &GPIO_InitStruct); - break; + case OperationMode::INPUT: + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(pin.port, &GPIO_InitStruct); + break; - case OperationMode::ANALOG: - GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(pin.port, &GPIO_InitStruct); - break; - case OperationMode::EXTERNAL_INTERRUPT_RISING: - GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; - GPIO_InitStruct.Pull = GPIO_PULLDOWN; - HAL_GPIO_Init(pin.port, &GPIO_InitStruct); - break; - case OperationMode::EXTERNAL_INTERRUPT_FALLING: - GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; - GPIO_InitStruct.Pull = GPIO_PULLDOWN; - HAL_GPIO_Init(pin.port, &GPIO_InitStruct); - break; - case OperationMode::EXTERNAL_INTERRUPT_RISING_FALLING: - GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; - GPIO_InitStruct.Pull = GPIO_PULLDOWN; - HAL_GPIO_Init(pin.port, &GPIO_InitStruct); - break; - case OperationMode::TIMER_ALTERNATE_FUNCTION: - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - GPIO_InitStruct.Alternate = pin.alternative_function; - HAL_GPIO_Init(pin.port, &GPIO_InitStruct); - break; + case OperationMode::ANALOG: + GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(pin.port, &GPIO_InitStruct); + break; + case OperationMode::EXTERNAL_INTERRUPT_RISING: + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; + GPIO_InitStruct.Pull = GPIO_PULLDOWN; + HAL_GPIO_Init(pin.port, &GPIO_InitStruct); + break; + case OperationMode::EXTERNAL_INTERRUPT_FALLING: + GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; + GPIO_InitStruct.Pull = GPIO_PULLDOWN; + HAL_GPIO_Init(pin.port, &GPIO_InitStruct); + break; + case OperationMode::EXTERNAL_INTERRUPT_RISING_FALLING: + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; + GPIO_InitStruct.Pull = GPIO_PULLDOWN; + HAL_GPIO_Init(pin.port, &GPIO_InitStruct); + break; + case OperationMode::TIMER_ALTERNATE_FUNCTION: + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = pin.alternative_function; + HAL_GPIO_Init(pin.port, &GPIO_InitStruct); + break; - default: - break; + default: + break; } } } diff --git a/Src/HALAL/Models/SPI/SPI2.cpp b/Src/HALAL/Models/SPI/SPI2.cpp index f83ea553c..f860af798 100644 --- a/Src/HALAL/Models/SPI/SPI2.cpp +++ b/Src/HALAL/Models/SPI/SPI2.cpp @@ -9,7 +9,7 @@ uint32_t ST_LIB::SPIDomain::calculate_prescaler(uint32_t src_freq, uint32_t max_ ErrorHandler("Cannot achieve desired baudrate, speed is too low"); } } - + return get_prescaler_flag(prescaler); } @@ -70,14 +70,13 @@ void SPI6_IRQHandler(void) { HAL_SPI_IRQHandler(&inst->hspi); } - /** * ========================================= * HAL Callbacks * ========================================= */ -void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi) { +void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef* hspi) { auto& spi_instances = ST_LIB::SPIDomain::spi_instances; ST_LIB::SPIDomain::Instance* inst = nullptr; @@ -103,15 +102,15 @@ void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi) { } } -void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) { +void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef* hspi) { HAL_SPI_TxCpltCallback(hspi); // Same logic } -void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) { +void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef* hspi) { HAL_SPI_TxCpltCallback(hspi); // Same logic } -void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi) { +void HAL_SPI_ErrorCallback(SPI_HandleTypeDef* hspi) { auto& spi_instances = ST_LIB::SPIDomain::spi_instances; uint32_t error_code = hspi->ErrorCode; @@ -135,5 +134,4 @@ void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi) { ErrorHandler("SPI%i failed with error number %u", inst_idx + 1, error_code); } - } \ No newline at end of file diff --git a/Src/HALAL/Models/TimerDomain/TimerDomain.cpp b/Src/HALAL/Models/TimerDomain/TimerDomain.cpp index 90180e0b9..cdafeeef2 100644 --- a/Src/HALAL/Models/TimerDomain/TimerDomain.cpp +++ b/Src/HALAL/Models/TimerDomain/TimerDomain.cpp @@ -6,8 +6,8 @@ using namespace ST_LIB; TimerXList #undef X -void (*TimerDomain::callbacks[TimerDomain::max_instances])(void*) = {nullptr}; -void *TimerDomain::callback_data[TimerDomain::max_instances] = {nullptr}; + void (*TimerDomain::callbacks[TimerDomain::max_instances])(void*) = {nullptr}; +void* TimerDomain::callback_data[TimerDomain::max_instances] = {nullptr}; extern "C" void TIM1_UP_IRQHandler(void) { CLEAR_BIT(TimerDomain::cmsis_timers[timer_idxmap[1]]->SR, TIM_SR_UIF); @@ -45,11 +45,11 @@ extern "C" void TIM7_IRQHandler(void) { } extern "C" void TIM8_BRK_TIM12_IRQHandler(void) { - if((TimerDomain::cmsis_timers[timer_idxmap[12]]->SR & TIM_SR_UIF) != 0) { + if ((TimerDomain::cmsis_timers[timer_idxmap[12]]->SR & TIM_SR_UIF) != 0) { CLEAR_BIT(TimerDomain::cmsis_timers[timer_idxmap[12]]->SR, TIM_SR_UIF); TimerDomain::callbacks[timer_idxmap[12]](TimerDomain::callback_data[timer_idxmap[12]]); } - if((TimerDomain::cmsis_timers[timer_idxmap[8]]->SR & TIM_SR_BIF) != 0) { + if ((TimerDomain::cmsis_timers[timer_idxmap[8]]->SR & TIM_SR_BIF) != 0) { CLEAR_BIT(TimerDomain::cmsis_timers[timer_idxmap[8]]->SR, TIM_SR_BIF); /* this could probably have some other callback */ TimerDomain::callbacks[timer_idxmap[8]](TimerDomain::callback_data[timer_idxmap[8]]); @@ -57,23 +57,23 @@ extern "C" void TIM8_BRK_TIM12_IRQHandler(void) { } extern "C" void TIM8_UP_TIM13_IRQHandler(void) { - if((TimerDomain::cmsis_timers[timer_idxmap[13]]->SR & TIM_SR_UIF) != 0) { + if ((TimerDomain::cmsis_timers[timer_idxmap[13]]->SR & TIM_SR_UIF) != 0) { CLEAR_BIT(TimerDomain::cmsis_timers[timer_idxmap[13]]->SR, TIM_SR_UIF); TimerDomain::callbacks[timer_idxmap[13]](TimerDomain::callback_data[timer_idxmap[13]]); } - if((TimerDomain::cmsis_timers[timer_idxmap[8]]->SR & TIM_SR_UIF) != 0) { + if ((TimerDomain::cmsis_timers[timer_idxmap[8]]->SR & TIM_SR_UIF) != 0) { CLEAR_BIT(TimerDomain::cmsis_timers[timer_idxmap[8]]->SR, TIM_SR_UIF); TimerDomain::callbacks[timer_idxmap[8]](TimerDomain::callback_data[timer_idxmap[8]]); } } extern "C" void TIM8_TRG_COM_TIM14_IRQHandler(void) { - if((TimerDomain::cmsis_timers[timer_idxmap[14]]->SR & TIM_SR_UIF) != 0) { + if ((TimerDomain::cmsis_timers[timer_idxmap[14]]->SR & TIM_SR_UIF) != 0) { CLEAR_BIT(TimerDomain::cmsis_timers[timer_idxmap[14]]->SR, TIM_SR_UIF); TimerDomain::callbacks[timer_idxmap[14]](TimerDomain::callback_data[timer_idxmap[14]]); } constexpr uint32_t com_trg_flags = TIM_SR_TIF | TIM_SR_COMIF; - if((TimerDomain::cmsis_timers[timer_idxmap[14]]->SR & com_trg_flags) != 0) { + if ((TimerDomain::cmsis_timers[timer_idxmap[14]]->SR & com_trg_flags) != 0) { CLEAR_BIT(TimerDomain::cmsis_timers[timer_idxmap[8]]->SR, com_trg_flags); /* this could probably have some other callback */ TimerDomain::callbacks[timer_idxmap[8]](TimerDomain::callback_data[timer_idxmap[8]]); @@ -104,4 +104,3 @@ extern "C" void TIM24_IRQHandler(void) { CLEAR_BIT(TimerDomain::cmsis_timers[timer_idxmap[24]]->SR, TIM_SR_UIF); TimerDomain::callbacks[timer_idxmap[24]](TimerDomain::callback_data[timer_idxmap[24]]); } - diff --git a/Src/HALAL/Models/TimerPeripheral/TimerPeripheral.cpp b/Src/HALAL/Models/TimerPeripheral/TimerPeripheral.cpp index a4b3bdddd..45ad6920e 100644 --- a/Src/HALAL/Models/TimerPeripheral/TimerPeripheral.cpp +++ b/Src/HALAL/Models/TimerPeripheral/TimerPeripheral.cpp @@ -7,177 +7,168 @@ #include "HALAL/Models/TimerPeripheral/TimerPeripheral.hpp" -map TimerPeripheral::handle_to_timer= { - {&htim1, TIM1}, - {&htim2, TIM2}, - {&htim3, TIM3}, - {&htim4, TIM4}, - {&htim8, TIM8}, - {&htim12, TIM12}, - {&htim15, TIM15}, - {&htim16, TIM16}, - {&htim17, TIM17}, - {&htim23, TIM23}, - {&htim24, TIM24} +map TimerPeripheral::handle_to_timer = { + {&htim1, TIM1}, + {&htim2, TIM2}, + {&htim3, TIM3}, + {&htim4, TIM4}, + {&htim8, TIM8}, + {&htim12, TIM12}, + {&htim15, TIM15}, + {&htim16, TIM16}, + {&htim17, TIM17}, + {&htim23, TIM23}, + {&htim24, TIM24} }; - TimerPeripheral::InitData::InitData( - TIM_TYPE type, uint32_t prescaler, uint32_t period, uint32_t deadtime, uint32_t polarity, uint32_t negated_polarity) : - type(type), - prescaler(prescaler), - period(period), - deadtime(deadtime), - polarity(polarity), - negated_polarity(negated_polarity) - {} - -TimerPeripheral::TimerPeripheral( - TIM_HandleTypeDef* handle, InitData&& init_data, string name) : - handle(handle), - init_data(init_data), - name(name) {} + TIM_TYPE type, + uint32_t prescaler, + uint32_t period, + uint32_t deadtime, + uint32_t polarity, + uint32_t negated_polarity +) + : type(type), prescaler(prescaler), period(period), deadtime(deadtime), polarity(polarity), + negated_polarity(negated_polarity) {} + +TimerPeripheral::TimerPeripheral(TIM_HandleTypeDef* handle, InitData&& init_data, string name) + : handle(handle), init_data(init_data), name(name) {} void TimerPeripheral::init() { - TIM_MasterConfigTypeDef sMasterConfig = {0}; - TIM_IC_InitTypeDef sConfigIC = {0}; - TIM_OC_InitTypeDef sConfigOC = {0}; - TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; - - handle->Instance = handle_to_timer[handle]; - handle->Init.Prescaler = init_data.prescaler; - handle->Init.CounterMode = TIM_COUNTERMODE_UP; - handle->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; - for (PWMData &pwm_data : init_data.pwm_channels) { - if (pwm_data.mode == PHASED) { - handle->Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1; - break; - } else if (pwm_data.mode == CENTER_ALIGNED) { - handle->Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; - handle->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; - break; - } - } - handle->Init.Period = init_data.period; - handle->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - handle->Init.RepetitionCounter = 0; - - if (init_data.type == BASE) { - if (HAL_TIM_Base_Init(handle) != HAL_OK){ - ErrorHandler("Unable to init base timer on %d", name.c_str()); - } - } - - if (!init_data.input_capture_channels.empty()) { - //TO READ LOW FREQUENCIES WE NEED TO PRESCALE - //ALSO NEED CHANGE TIM23 TIMER FROM BASE TO ADVANCE - handle->Init.Prescaler = 2000; - if (HAL_TIM_IC_Init(handle) != HAL_OK) - { - ErrorHandler("Unable to init input capture on %d", name.c_str()); - } - } - - if (!init_data.pwm_channels.empty()) { - if (HAL_TIM_PWM_Init(handle) != HAL_OK) { - ErrorHandler("Unable to init PWM on %d", name.c_str()); - } - } - - sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; - sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET; - sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(handle, &sMasterConfig) != HAL_OK) { - ErrorHandler("Unable to configure master synchronization on %d", name.c_str()); - } - - for (pair &channels_rising_falling : init_data.input_capture_channels) { - sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING; - sConfigIC.ICPrescaler = TIM_ICPSC_DIV1; - sConfigIC.ICFilter = 0; - sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI; - if (HAL_TIM_IC_ConfigChannel(handle, &sConfigIC, channels_rising_falling.first) != HAL_OK) { - ErrorHandler("Unable to configure a IC channel on %d", name.c_str()); - } - - sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_FALLING; - sConfigIC.ICSelection = TIM_ICSELECTION_INDIRECTTI; - if (HAL_TIM_IC_ConfigChannel(handle, &sConfigIC, channels_rising_falling.second) != HAL_OK) { - ErrorHandler("Unable to configure a IC channel on %d", name.c_str()); - } - } - - for (PWMData &pwm_data : init_data.pwm_channels) { - sConfigOC.OCPolarity = init_data.polarity; - sConfigOC.OCNPolarity = init_data.negated_polarity; - sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; - sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; - sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; - - if (pwm_data.mode == PHASED) { - //ASSYMETRIC_MODE_1 means one output per pair of registers (CCR1 - CCR2) for example - sConfigOC.OCMode = TIM_OCMODE_ASSYMETRIC_PWM1; - if (HAL_TIM_PWM_ConfigChannel(handle, &sConfigOC, pwm_data.channel) != HAL_OK) { - ErrorHandler("Unable to configure a PWM channel on %d", name.c_str()); - } - // if the channel number is even the pair is the previous channel, example, - // if channel is 2 then CCRX is CCR2 and the pair is CCR1 - // note that TIM_CHANNEL_1 is not 1 is actually 0x00000000, therefore the %8 - if (pwm_data.channel % 8 == 1) { - if (HAL_TIM_PWM_ConfigChannel(handle, &sConfigOC, pwm_data.channel-4) != HAL_OK) { - ErrorHandler("Unable to configure a PWM channel on %d", name.c_str()); - } - } else { - if (HAL_TIM_PWM_ConfigChannel(handle, &sConfigOC, pwm_data.channel+4) != HAL_OK) { - ErrorHandler("Unable to configure a PWM channel on %d", name.c_str()); - } - } - } else { - sConfigOC.OCMode = TIM_OCMODE_PWM1; - if (HAL_TIM_PWM_ConfigChannel(handle, &sConfigOC, pwm_data.channel) != HAL_OK) { - ErrorHandler("Unable to configure a PWM channel on %d", name.c_str()); - } - } - } - - sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; - sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; - sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; - sBreakDeadTimeConfig.DeadTime = init_data.deadtime; - sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; - sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; - sBreakDeadTimeConfig.BreakFilter = 0; - sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE; - sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH; - sBreakDeadTimeConfig.Break2Filter = 0; - sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; - if (HAL_TIMEx_ConfigBreakDeadTime(handle, &sBreakDeadTimeConfig) != HAL_OK) { - ErrorHandler("Unable to configure break dead time on %d", name.c_str()); - } - - + TIM_MasterConfigTypeDef sMasterConfig = {0}; + TIM_IC_InitTypeDef sConfigIC = {0}; + TIM_OC_InitTypeDef sConfigOC = {0}; + TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; + + handle->Instance = handle_to_timer[handle]; + handle->Init.Prescaler = init_data.prescaler; + handle->Init.CounterMode = TIM_COUNTERMODE_UP; + handle->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + for (PWMData& pwm_data : init_data.pwm_channels) { + if (pwm_data.mode == PHASED) { + handle->Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1; + break; + } else if (pwm_data.mode == CENTER_ALIGNED) { + handle->Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + handle->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; + break; + } + } + handle->Init.Period = init_data.period; + handle->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + handle->Init.RepetitionCounter = 0; + + if (init_data.type == BASE) { + if (HAL_TIM_Base_Init(handle) != HAL_OK) { + ErrorHandler("Unable to init base timer on %d", name.c_str()); + } + } + + if (!init_data.input_capture_channels.empty()) { + // TO READ LOW FREQUENCIES WE NEED TO PRESCALE + // ALSO NEED CHANGE TIM23 TIMER FROM BASE TO ADVANCE + handle->Init.Prescaler = 2000; + if (HAL_TIM_IC_Init(handle) != HAL_OK) { + ErrorHandler("Unable to init input capture on %d", name.c_str()); + } + } + + if (!init_data.pwm_channels.empty()) { + if (HAL_TIM_PWM_Init(handle) != HAL_OK) { + ErrorHandler("Unable to init PWM on %d", name.c_str()); + } + } + + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(handle, &sMasterConfig) != HAL_OK) { + ErrorHandler("Unable to configure master synchronization on %d", name.c_str()); + } + + for (pair& channels_rising_falling : init_data.input_capture_channels) { + sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING; + sConfigIC.ICPrescaler = TIM_ICPSC_DIV1; + sConfigIC.ICFilter = 0; + sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI; + if (HAL_TIM_IC_ConfigChannel(handle, &sConfigIC, channels_rising_falling.first) != HAL_OK) { + ErrorHandler("Unable to configure a IC channel on %d", name.c_str()); + } + + sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_FALLING; + sConfigIC.ICSelection = TIM_ICSELECTION_INDIRECTTI; + if (HAL_TIM_IC_ConfigChannel(handle, &sConfigIC, channels_rising_falling.second) != + HAL_OK) { + ErrorHandler("Unable to configure a IC channel on %d", name.c_str()); + } + } + + for (PWMData& pwm_data : init_data.pwm_channels) { + sConfigOC.OCPolarity = init_data.polarity; + sConfigOC.OCNPolarity = init_data.negated_polarity; + sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; + sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; + sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; + + if (pwm_data.mode == PHASED) { + // ASSYMETRIC_MODE_1 means one output per pair of registers (CCR1 - CCR2) for example + sConfigOC.OCMode = TIM_OCMODE_ASSYMETRIC_PWM1; + if (HAL_TIM_PWM_ConfigChannel(handle, &sConfigOC, pwm_data.channel) != HAL_OK) { + ErrorHandler("Unable to configure a PWM channel on %d", name.c_str()); + } + // if the channel number is even the pair is the previous channel, example, + // if channel is 2 then CCRX is CCR2 and the pair is CCR1 + // note that TIM_CHANNEL_1 is not 1 is actually 0x00000000, therefore the %8 + if (pwm_data.channel % 8 == 1) { + if (HAL_TIM_PWM_ConfigChannel(handle, &sConfigOC, pwm_data.channel - 4) != HAL_OK) { + ErrorHandler("Unable to configure a PWM channel on %d", name.c_str()); + } + } else { + if (HAL_TIM_PWM_ConfigChannel(handle, &sConfigOC, pwm_data.channel + 4) != HAL_OK) { + ErrorHandler("Unable to configure a PWM channel on %d", name.c_str()); + } + } + } else { + sConfigOC.OCMode = TIM_OCMODE_PWM1; + if (HAL_TIM_PWM_ConfigChannel(handle, &sConfigOC, pwm_data.channel) != HAL_OK) { + ErrorHandler("Unable to configure a PWM channel on %d", name.c_str()); + } + } + } + + sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; + sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; + sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; + sBreakDeadTimeConfig.DeadTime = init_data.deadtime; + sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; + sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; + sBreakDeadTimeConfig.BreakFilter = 0; + sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE; + sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH; + sBreakDeadTimeConfig.Break2Filter = 0; + sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; + if (HAL_TIMEx_ConfigBreakDeadTime(handle, &sBreakDeadTimeConfig) != HAL_OK) { + ErrorHandler("Unable to configure break dead time on %d", name.c_str()); + } } void TimerPeripheral::start() { - for (TimerPeripheral &timer : timers) { - if (timer.is_registered()) { - timer.init(); - } - } + for (TimerPeripheral& timer : timers) { + if (timer.is_registered()) { + timer.init(); + } + } } bool TimerPeripheral::is_registered() { - return init_data.pwm_channels.size() + init_data.input_capture_channels.size(); + return init_data.pwm_channels.size() + init_data.input_capture_channels.size(); } -uint32_t TimerPeripheral::get_prescaler() { - return handle->Instance->PSC; -} +uint32_t TimerPeripheral::get_prescaler() { return handle->Instance->PSC; } -uint32_t TimerPeripheral::get_period() { - return handle->Instance->ARR; -} +uint32_t TimerPeripheral::get_period() { return handle->Instance->ARR; } -bool TimerPeripheral::is_occupied(){ - return init_data.pwm_channels.size() && init_data.input_capture_channels.size(); +bool TimerPeripheral::is_occupied() { + return init_data.pwm_channels.size() && init_data.input_capture_channels.size(); } diff --git a/Src/HALAL/Services/ADC/ADC.cpp b/Src/HALAL/Services/ADC/ADC.cpp index 4abc262f1..8d2596ede 100644 --- a/Src/HALAL/Services/ADC/ADC.cpp +++ b/Src/HALAL/Services/ADC/ADC.cpp @@ -15,160 +15,176 @@ extern ADC_HandleTypeDef hadc3; uint8_t ADC::id_counter = 0; unordered_map ADC::active_instances = {}; - -ADC::InitData::InitData(ADC_TypeDef* adc, uint32_t resolution, uint32_t external_trigger, vector& channels, ST_LIB::DMA_Domain::Instance *dma_instance, string name) : - adc(adc), resolution(resolution), external_trigger(external_trigger), channels(channels), dma_instance(dma_instance), name(name) {} - -ADC::Peripheral::Peripheral(ADC_HandleTypeDef* handle, LowPowerTimer& timer, InitData& init_data) : - handle(handle), timer(timer), init_data(init_data) { - dma_data_buffer = (uint16_t*)MPUManager::allocate_non_cached_memory(2*ADC_BUF_LEN); +ADC::InitData::InitData( + ADC_TypeDef* adc, + uint32_t resolution, + uint32_t external_trigger, + vector& channels, + ST_LIB::DMA_Domain::Instance* dma_instance, + string name +) + : adc(adc), resolution(resolution), external_trigger(external_trigger), channels(channels), + dma_instance(dma_instance), name(name) {} + +ADC::Peripheral::Peripheral(ADC_HandleTypeDef* handle, LowPowerTimer& timer, InitData& init_data) + : handle(handle), timer(timer), init_data(init_data) { + dma_data_buffer = (uint16_t*)MPUManager::allocate_non_cached_memory(2 * ADC_BUF_LEN); } -bool ADC::Peripheral::is_registered() { - return init_data.channels.size(); -} +bool ADC::Peripheral::is_registered() { return init_data.channels.size(); } -ADC::Instance::Instance(ADC::Peripheral* peripheral, uint32_t channel) : - peripheral(peripheral), channel(channel) {} +ADC::Instance::Instance(ADC::Peripheral* peripheral, uint32_t channel) + : peripheral(peripheral), channel(channel) {} uint8_t ADC::inscribe(Pin pin) { - if (not available_instances.contains(pin)) { - ErrorHandler("Pin %s is already used or isn t available for ADC usage", pin.to_string().c_str()); - return 0; - } - - Pin::inscribe(pin, ANALOG); - active_instances[id_counter] = available_instances[pin]; - - - InitData& init_data = active_instances[id_counter].peripheral->init_data; - //DMA::inscribe_stream(init_data.dma_stream); - active_instances[id_counter].rank = init_data.channels.size(); - init_data.channels.push_back(active_instances[id_counter].channel); - return id_counter++; + if (not available_instances.contains(pin)) { + ErrorHandler( + "Pin %s is already used or isn t available for ADC usage", + pin.to_string().c_str() + ); + return 0; + } + + Pin::inscribe(pin, ANALOG); + active_instances[id_counter] = available_instances[pin]; + + InitData& init_data = active_instances[id_counter].peripheral->init_data; + // DMA::inscribe_stream(init_data.dma_stream); + active_instances[id_counter].rank = init_data.channels.size(); + init_data.channels.push_back(active_instances[id_counter].channel); + return id_counter++; } void ADC::start() { - for(Peripheral& peripheral : peripherals) { - if (peripheral.is_registered()) { - ADC::init(peripheral); - } - } + for (Peripheral& peripheral : peripherals) { + if (peripheral.is_registered()) { + ADC::init(peripheral); + } + } } -void ADC::turn_on(uint8_t id){ - if (not active_instances.contains(id)) { - return; - } - - Peripheral* peripheral = active_instances[id].peripheral; - if (peripheral->is_on) { - return; - } - - uint32_t buffer_length = peripheral->init_data.channels.size(); - if (HAL_ADC_Start_DMA(peripheral->handle, (uint32_t*) peripheral->dma_data_buffer, buffer_length) != HAL_OK) { - ErrorHandler("DMA - %d - of ADC - %d - did not start correctly", peripheral->init_data.dma_instance, id); - return; - } - - LowPowerTimer& timer = peripheral->timer; - if (HAL_LPTIM_TimeOut_Start_IT(&timer.handle, timer.period, timer.period / 2) != HAL_OK) { - ErrorHandler("LPTIM - %d - of ADC - %d - did not start correctly", timer.name, peripheral->handle); - return; - } - peripheral->is_on = true; +void ADC::turn_on(uint8_t id) { + if (not active_instances.contains(id)) { + return; + } + + Peripheral* peripheral = active_instances[id].peripheral; + if (peripheral->is_on) { + return; + } + + uint32_t buffer_length = peripheral->init_data.channels.size(); + if (HAL_ADC_Start_DMA( + peripheral->handle, + (uint32_t*)peripheral->dma_data_buffer, + buffer_length + ) != HAL_OK) { + ErrorHandler( + "DMA - %d - of ADC - %d - did not start correctly", + peripheral->init_data.dma_instance, + id + ); + return; + } + + LowPowerTimer& timer = peripheral->timer; + if (HAL_LPTIM_TimeOut_Start_IT(&timer.handle, timer.period, timer.period / 2) != HAL_OK) { + ErrorHandler( + "LPTIM - %d - of ADC - %d - did not start correctly", + timer.name, + peripheral->handle + ); + return; + } + peripheral->is_on = true; } float ADC::get_value(uint8_t id) { - Instance& instance = active_instances[id]; - uint16_t raw = instance.peripheral->dma_data_buffer[instance.rank]; - if(instance.peripheral->handle == &hadc3) { - return raw / MAX_12BIT * ADC_MAX_VOLTAGE; - } - else { - return raw / MAX_16BIT * ADC_MAX_VOLTAGE; - } + Instance& instance = active_instances[id]; + uint16_t raw = instance.peripheral->dma_data_buffer[instance.rank]; + if (instance.peripheral->handle == &hadc3) { + return raw / MAX_12BIT * ADC_MAX_VOLTAGE; + } else { + return raw / MAX_16BIT * ADC_MAX_VOLTAGE; + } } uint16_t ADC::get_int_value(uint8_t id) { - Instance& instance = active_instances[id]; - uint16_t raw = instance.peripheral->dma_data_buffer[instance.rank]; - - if(instance.peripheral->handle == &hadc3) { - return raw << 4; - } - else { - return raw; - } + Instance& instance = active_instances[id]; + uint16_t raw = instance.peripheral->dma_data_buffer[instance.rank]; + + if (instance.peripheral->handle == &hadc3) { + return raw << 4; + } else { + return raw; + } } uint16_t* ADC::get_value_pointer(uint8_t id) { - Instance& instance = active_instances[id]; - return &instance.peripheral->dma_data_buffer[instance.rank]; + Instance& instance = active_instances[id]; + return &instance.peripheral->dma_data_buffer[instance.rank]; } void ADC::init(Peripheral& peripheral) { - ADC_MultiModeTypeDef multimode = {0}; - ADC_ChannelConfTypeDef sConfig = {0}; - ADC_HandleTypeDef& adc_handle = *peripheral.handle; - InitData init_data = peripheral.init_data; - - adc_handle.Instance = init_data.adc; - adc_handle.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV1; - adc_handle.Init.Resolution = init_data.resolution; - adc_handle.Init.ScanConvMode = ADC_SCAN_ENABLE; - adc_handle.Init.EOCSelection = ADC_EOC_SEQ_CONV; - adc_handle.Init.LowPowerAutoWait = DISABLE; - adc_handle.Init.ContinuousConvMode = DISABLE; - adc_handle.Init.NbrOfConversion = init_data.channels.size(); - adc_handle.Init.DiscontinuousConvMode = DISABLE; - adc_handle.Init.ExternalTrigConv = init_data.external_trigger; - adc_handle.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; - if (adc_handle.Instance == ADC3) { - adc_handle.Init.DMAContinuousRequests = ENABLE; - } else { - adc_handle.Init.ConversionDataManagement = ADC_CONVERSIONDATA_DMA_CIRCULAR; - } - adc_handle.Init.Overrun = ADC_OVR_DATA_PRESERVED; - adc_handle.Init.LeftBitShift = ADC_LEFTBITSHIFT_NONE; - adc_handle.Init.OversamplingMode = DISABLE; - if (HAL_ADC_Init(&adc_handle) != HAL_OK) { - ErrorHandler("ADC - %d - did not start correctly", init_data.name); - return; - } - - multimode.Mode = ADC_MODE_INDEPENDENT; - if(adc_handle.Instance == ADC1){ - if (HAL_ADCEx_MultiModeConfigChannel(&adc_handle, &multimode) != HAL_OK) { - ErrorHandler("ADC MultiModeConfigChannel - %d - did not start correctly", init_data.name); - return; - } - } + ADC_MultiModeTypeDef multimode = {0}; + ADC_ChannelConfTypeDef sConfig = {0}; + ADC_HandleTypeDef& adc_handle = *peripheral.handle; + InitData init_data = peripheral.init_data; + + adc_handle.Instance = init_data.adc; + adc_handle.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV1; + adc_handle.Init.Resolution = init_data.resolution; + adc_handle.Init.ScanConvMode = ADC_SCAN_ENABLE; + adc_handle.Init.EOCSelection = ADC_EOC_SEQ_CONV; + adc_handle.Init.LowPowerAutoWait = DISABLE; + adc_handle.Init.ContinuousConvMode = DISABLE; + adc_handle.Init.NbrOfConversion = init_data.channels.size(); + adc_handle.Init.DiscontinuousConvMode = DISABLE; + adc_handle.Init.ExternalTrigConv = init_data.external_trigger; + adc_handle.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; + if (adc_handle.Instance == ADC3) { + adc_handle.Init.DMAContinuousRequests = ENABLE; + } else { + adc_handle.Init.ConversionDataManagement = ADC_CONVERSIONDATA_DMA_CIRCULAR; + } + adc_handle.Init.Overrun = ADC_OVR_DATA_PRESERVED; + adc_handle.Init.LeftBitShift = ADC_LEFTBITSHIFT_NONE; + adc_handle.Init.OversamplingMode = DISABLE; + if (HAL_ADC_Init(&adc_handle) != HAL_OK) { + ErrorHandler("ADC - %d - did not start correctly", init_data.name); + return; + } + + multimode.Mode = ADC_MODE_INDEPENDENT; + if (adc_handle.Instance == ADC1) { + if (HAL_ADCEx_MultiModeConfigChannel(&adc_handle, &multimode) != HAL_OK) { + ErrorHandler( + "ADC MultiModeConfigChannel - %d - did not start correctly", + init_data.name + ); + return; + } + } __HAL_LINKDMA(&adc_handle, DMA_Handle, init_data.dma_instance->dma); - uint8_t counter = 0; - for(uint32_t channel : peripheral.init_data.channels) { - sConfig.Channel = channel; - sConfig.Rank = ranks[counter]; - sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5; - sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.OffsetNumber = ADC_OFFSET_NONE; - sConfig.Offset = 0; - sConfig.OffsetSignedSaturation = DISABLE; - if (HAL_ADC_ConfigChannel(&adc_handle, &sConfig) != HAL_OK) { - ErrorHandler("ADC ConfigChannel - %d - did not start correctly", init_data.name); - } - counter++; - } - - peripheral.timer.init(); + uint8_t counter = 0; + for (uint32_t channel : peripheral.init_data.channels) { + sConfig.Channel = channel; + sConfig.Rank = ranks[counter]; + sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + sConfig.OffsetSignedSaturation = DISABLE; + if (HAL_ADC_ConfigChannel(&adc_handle, &sConfig) != HAL_OK) { + ErrorHandler("ADC ConfigChannel - %d - did not start correctly", init_data.name); + } + counter++; + } + + peripheral.timer.init(); } - - -void HAL_ADC_ConvCpltCallback (ADC_HandleTypeDef *hadc){ - -} +void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc) {} #endif \ No newline at end of file diff --git a/Src/HALAL/Services/CORDIC/CORDIC.cpp b/Src/HALAL/Services/CORDIC/CORDIC.cpp index ed7f4b919..4ffb0e518 100644 --- a/Src/HALAL/Services/CORDIC/CORDIC.cpp +++ b/Src/HALAL/Services/CORDIC/CORDIC.cpp @@ -7,108 +7,94 @@ #include "HALAL/Services/CORDIC/CORDIC.hpp" - Operation_Computation RotationComputer::mode = NONE; -void RotationComputer::start(){ - __HAL_RCC_CORDIC_CLK_ENABLE(); -} +void RotationComputer::start() { __HAL_RCC_CORDIC_CLK_ENABLE(); } -void RotationComputer::cos(int32_t *angle, int32_t *out, int32_t size){ - if(RotationComputer::mode != COSINE){ - RotationComputer::mode = COSINE; - MODIFY_REG(CORDIC -> CSR, - RESET_MASK, - COSINE_CONFIG - ); - } - for(int n = 0; n < size; n++){ - CORDIC -> WDATA = angle[n]; - out[n] = CORDIC -> RDATA; - } +void RotationComputer::cos(int32_t* angle, int32_t* out, int32_t size) { + if (RotationComputer::mode != COSINE) { + RotationComputer::mode = COSINE; + MODIFY_REG(CORDIC->CSR, RESET_MASK, COSINE_CONFIG); + } + for (int n = 0; n < size; n++) { + CORDIC->WDATA = angle[n]; + out[n] = CORDIC->RDATA; + } } -void RotationComputer::sin(int32_t *angle, int32_t *out, int32_t size){ - if(RotationComputer::mode != SINE){ - RotationComputer::mode = SINE; - MODIFY_REG(CORDIC -> CSR, - RESET_MASK, - SINE_CONFIG - ); - } - for(int n = 0; n < size; n++){ - CORDIC -> WDATA = angle[n]; - out[n] = CORDIC -> RDATA; - } +void RotationComputer::sin(int32_t* angle, int32_t* out, int32_t size) { + if (RotationComputer::mode != SINE) { + RotationComputer::mode = SINE; + MODIFY_REG(CORDIC->CSR, RESET_MASK, SINE_CONFIG); + } + for (int n = 0; n < size; n++) { + CORDIC->WDATA = angle[n]; + out[n] = CORDIC->RDATA; + } } -void RotationComputer::cos_and_sin(int32_t *angle, int32_t *cos_out, int32_t *sin_out, int32_t size){ - if(RotationComputer::mode != SINE_COSINE){ - RotationComputer::mode = SINE_COSINE; - MODIFY_REG(CORDIC -> CSR, - RESET_MASK, - SINE_COSINE_CONFIG - ); - } - for(int n = 0; n < size; n++){ - CORDIC -> WDATA = angle[n]; - cos_out[n] = CORDIC -> RDATA; - sin_out[n] = CORDIC -> RDATA; - } +void RotationComputer::cos_and_sin( + int32_t* angle, + int32_t* cos_out, + int32_t* sin_out, + int32_t size +) { + if (RotationComputer::mode != SINE_COSINE) { + RotationComputer::mode = SINE_COSINE; + MODIFY_REG(CORDIC->CSR, RESET_MASK, SINE_COSINE_CONFIG); + } + for (int n = 0; n < size; n++) { + CORDIC->WDATA = angle[n]; + cos_out[n] = CORDIC->RDATA; + sin_out[n] = CORDIC->RDATA; + } } -void RotationComputer::phase(int32_t *x, int32_t *y, int32_t *angle_out, int32_t size){ - if(RotationComputer::mode != PHASE){ - RotationComputer::mode = PHASE; - MODIFY_REG(CORDIC -> CSR, - RESET_MASK, - PHASE_CONFIG - ); - } - for(int n = 0; n < size; n++){ - CORDIC -> WDATA = x[n]; - CORDIC -> WDATA = y[n]; - angle_out[n] = CORDIC -> RDATA; - } +void RotationComputer::phase(int32_t* x, int32_t* y, int32_t* angle_out, int32_t size) { + if (RotationComputer::mode != PHASE) { + RotationComputer::mode = PHASE; + MODIFY_REG(CORDIC->CSR, RESET_MASK, PHASE_CONFIG); + } + for (int n = 0; n < size; n++) { + CORDIC->WDATA = x[n]; + CORDIC->WDATA = y[n]; + angle_out[n] = CORDIC->RDATA; + } } -void RotationComputer::modulus(int32_t *x, int32_t *y, int32_t *out, int32_t size){ - if(RotationComputer::mode != MODULUS){ - RotationComputer::mode = MODULUS; - MODIFY_REG(CORDIC -> CSR, - RESET_MASK, - MODULUS_CONFIG - ); - } - for(int n = 0; n < size; n++){ - CORDIC -> WDATA = x[n]; - CORDIC -> WDATA = y[n]; - out[n] = CORDIC -> RDATA; - } +void RotationComputer::modulus(int32_t* x, int32_t* y, int32_t* out, int32_t size) { + if (RotationComputer::mode != MODULUS) { + RotationComputer::mode = MODULUS; + MODIFY_REG(CORDIC->CSR, RESET_MASK, MODULUS_CONFIG); + } + for (int n = 0; n < size; n++) { + CORDIC->WDATA = x[n]; + CORDIC->WDATA = y[n]; + out[n] = CORDIC->RDATA; + } } -void RotationComputer::phase_and_modulus(int32_t *x, int32_t *y, int32_t *angle_out, int32_t *mod_out, int32_t size){ - if(RotationComputer::mode != PHASE_MODULUS){ - RotationComputer::mode = PHASE_MODULUS; - MODIFY_REG(CORDIC -> CSR, - RESET_MASK, - PHASE_MODULUS_CONFIG - ); - } - for(int n = 0; n < size; n++){ - CORDIC -> WDATA = x[n]; - CORDIC -> WDATA = y[n]; - angle_out[n] = CORDIC -> RDATA; - mod_out[n] = CORDIC -> RDATA; - } +void RotationComputer::phase_and_modulus( + int32_t* x, + int32_t* y, + int32_t* angle_out, + int32_t* mod_out, + int32_t size +) { + if (RotationComputer::mode != PHASE_MODULUS) { + RotationComputer::mode = PHASE_MODULUS; + MODIFY_REG(CORDIC->CSR, RESET_MASK, PHASE_MODULUS_CONFIG); + } + for (int n = 0; n < size; n++) { + CORDIC->WDATA = x[n]; + CORDIC->WDATA = y[n]; + angle_out[n] = CORDIC->RDATA; + mod_out[n] = CORDIC->RDATA; + } } +float RotationComputer::q31_to_radian_f32(uint32_t in) { return M_PI * ldexp((int32_t)in, -31); } -float RotationComputer::q31_to_radian_f32(uint32_t in){ - return M_PI*ldexp((int32_t) in, -31); +int32_t RotationComputer::radian_f32_to_q31(double in) { + return (int)roundf(scalbnf(fmaxf(fminf(in / M_PI, 0.9995), -0.9995), 31)); } - -int32_t RotationComputer::radian_f32_to_q31(double in){ - return (int)roundf(scalbnf(fmaxf(fminf(in/M_PI,0.9995),-0.9995),31)); -} - diff --git a/Src/HALAL/Services/Communication/Ethernet/LWIP/Ethernet.cpp b/Src/HALAL/Services/Communication/Ethernet/LWIP/Ethernet.cpp index 7ea237fb3..43632b8bf 100644 --- a/Src/HALAL/Services/Communication/Ethernet/LWIP/Ethernet.cpp +++ b/Src/HALAL/Services/Communication/Ethernet/LWIP/Ethernet.cpp @@ -18,82 +18,80 @@ extern uint8_t IP_ADDRESS[4], NETMASK_ADDRESS[4], GATEWAY_ADDRESS[4]; bool Ethernet::is_ready = false; bool Ethernet::is_running = false; -void Ethernet::start(string local_mac, string local_ip, string subnet_mask, - string gateway) { - start(MAC(local_mac), IPV4(local_ip), IPV4(subnet_mask), IPV4(gateway)); +void Ethernet::start(string local_mac, string local_ip, string subnet_mask, string gateway) { + start(MAC(local_mac), IPV4(local_ip), IPV4(subnet_mask), IPV4(gateway)); } -void Ethernet::start(MAC local_mac, IPV4 local_ip, IPV4 subnet_mask, - IPV4 gateway) { - if (!is_running && is_ready) { - ipaddr = local_ip.address; - netmask = subnet_mask.address; - gw = gateway.address; - IP_ADDRESS[0] = ipaddr.addr & 0xFF; - IP_ADDRESS[1] = (ipaddr.addr >> 8) & 0xFF; - IP_ADDRESS[2] = (ipaddr.addr >> 16) & 0xFF; - IP_ADDRESS[3] = (ipaddr.addr >> 24) & 0xFF; - NETMASK_ADDRESS[0] = netmask.addr & 0xFF; - NETMASK_ADDRESS[1] = (netmask.addr >> 8) & 0xFF; - NETMASK_ADDRESS[2] = (netmask.addr >> 16) & 0xFF; - NETMASK_ADDRESS[3] = (netmask.addr >> 24) & 0xFF; - GATEWAY_ADDRESS[0] = gw.addr & 0xFF; - GATEWAY_ADDRESS[1] = (gw.addr >> 8) & 0xFF; - GATEWAY_ADDRESS[2] = (gw.addr >> 16) & 0xFF; - GATEWAY_ADDRESS[3] = (gw.addr >> 24) & 0xFF; - gnetif.hwaddr[0] = local_mac.address[0]; - gnetif.hwaddr[1] = local_mac.address[1]; - gnetif.hwaddr[2] = local_mac.address[2]; - gnetif.hwaddr[3] = local_mac.address[3]; - gnetif.hwaddr[4] = local_mac.address[4]; - gnetif.hwaddr[5] = local_mac.address[5]; - MX_LWIP_Init(); - is_running = true; - } else { - ErrorHandler("Unable to start Ethernet!"); - } +void Ethernet::start(MAC local_mac, IPV4 local_ip, IPV4 subnet_mask, IPV4 gateway) { + if (!is_running && is_ready) { + ipaddr = local_ip.address; + netmask = subnet_mask.address; + gw = gateway.address; + IP_ADDRESS[0] = ipaddr.addr & 0xFF; + IP_ADDRESS[1] = (ipaddr.addr >> 8) & 0xFF; + IP_ADDRESS[2] = (ipaddr.addr >> 16) & 0xFF; + IP_ADDRESS[3] = (ipaddr.addr >> 24) & 0xFF; + NETMASK_ADDRESS[0] = netmask.addr & 0xFF; + NETMASK_ADDRESS[1] = (netmask.addr >> 8) & 0xFF; + NETMASK_ADDRESS[2] = (netmask.addr >> 16) & 0xFF; + NETMASK_ADDRESS[3] = (netmask.addr >> 24) & 0xFF; + GATEWAY_ADDRESS[0] = gw.addr & 0xFF; + GATEWAY_ADDRESS[1] = (gw.addr >> 8) & 0xFF; + GATEWAY_ADDRESS[2] = (gw.addr >> 16) & 0xFF; + GATEWAY_ADDRESS[3] = (gw.addr >> 24) & 0xFF; + gnetif.hwaddr[0] = local_mac.address[0]; + gnetif.hwaddr[1] = local_mac.address[1]; + gnetif.hwaddr[2] = local_mac.address[2]; + gnetif.hwaddr[3] = local_mac.address[3]; + gnetif.hwaddr[4] = local_mac.address[4]; + gnetif.hwaddr[5] = local_mac.address[5]; + MX_LWIP_Init(); + is_running = true; + } else { + ErrorHandler("Unable to start Ethernet!"); + } - if (not is_ready) { - ErrorHandler("Ethernet is not ready"); - return; - } + if (not is_ready) { + ErrorHandler("Ethernet is not ready"); + return; + } } void Ethernet::inscribe() { - if (!is_ready) { - Pin::inscribe(PA1, ALTERNATIVE); - Pin::inscribe(PA2, ALTERNATIVE); - Pin::inscribe(PA7, ALTERNATIVE); - Pin::inscribe(PB13, ALTERNATIVE); - Pin::inscribe(PC1, ALTERNATIVE); - Pin::inscribe(PC4, ALTERNATIVE); - Pin::inscribe(PC5, ALTERNATIVE); - Pin::inscribe(PG11, ALTERNATIVE); - Pin::inscribe(PG0, ALTERNATIVE); - Pin::inscribe(PG13, ALTERNATIVE); - is_ready = true; - } else { - ErrorHandler("Unable to inscribe Ethernet because is already ready!"); - } + if (!is_ready) { + Pin::inscribe(PA1, ALTERNATIVE); + Pin::inscribe(PA2, ALTERNATIVE); + Pin::inscribe(PA7, ALTERNATIVE); + Pin::inscribe(PB13, ALTERNATIVE); + Pin::inscribe(PC1, ALTERNATIVE); + Pin::inscribe(PC4, ALTERNATIVE); + Pin::inscribe(PC5, ALTERNATIVE); + Pin::inscribe(PG11, ALTERNATIVE); + Pin::inscribe(PG0, ALTERNATIVE); + Pin::inscribe(PG13, ALTERNATIVE); + is_ready = true; + } else { + ErrorHandler("Unable to inscribe Ethernet because is already ready!"); + } } void Ethernet::update() { - if (not is_running) { - ErrorHandler("Ethernet is not running, check if its been inscribed"); - return; - } + if (not is_running) { + ErrorHandler("Ethernet is not running, check if its been inscribed"); + return; + } - ethernetif_input(&gnetif); - sys_check_timeouts(); + ethernetif_input(&gnetif); + sys_check_timeouts(); - if (HAL_GetTick() - EthernetLinkTimer >= 100) { - EthernetLinkTimer = HAL_GetTick(); - ethernet_link_check_state(&gnetif); + if (HAL_GetTick() - EthernetLinkTimer >= 100) { + EthernetLinkTimer = HAL_GetTick(); + ethernet_link_check_state(&gnetif); - if (gnetif.flags == 15) { - netif_set_up(&gnetif); + if (gnetif.flags == 15) { + netif_set_up(&gnetif); + } } - } } #endif diff --git a/Src/HALAL/Services/Communication/Ethernet/LWIP/EthernetNode.cpp b/Src/HALAL/Services/Communication/Ethernet/LWIP/EthernetNode.cpp index 3a49e7e32..81f61a77b 100644 --- a/Src/HALAL/Services/Communication/Ethernet/LWIP/EthernetNode.cpp +++ b/Src/HALAL/Services/Communication/Ethernet/LWIP/EthernetNode.cpp @@ -9,16 +9,15 @@ EthernetNode::EthernetNode(IPV4 ip, uint32_t port) : ip(ip), port(port) {} -bool EthernetNode::operator==(const EthernetNode &other) const { - return ip.address.addr == other.ip.address.addr && port == other.port; +bool EthernetNode::operator==(const EthernetNode& other) const { + return ip.address.addr == other.ip.address.addr && port == other.port; } -std::size_t hash::operator()(const EthernetNode &key) const { - using std::hash; - using std::size_t; - using std::string; +std::size_t hash::operator()(const EthernetNode& key) const { + using std::hash; + using std::size_t; + using std::string; - return (hash()(key.ip.address.addr)) ^ - (hash()(key.port) << 1); + return (hash()(key.ip.address.addr)) ^ (hash()(key.port) << 1); } #endif diff --git a/Src/HALAL/Services/Communication/Ethernet/LWIP/TCP/ServerSocket.cpp b/Src/HALAL/Services/Communication/Ethernet/LWIP/TCP/ServerSocket.cpp index 9e3fa1638..6829086b2 100644 --- a/Src/HALAL/Services/Communication/Ethernet/LWIP/TCP/ServerSocket.cpp +++ b/Src/HALAL/Services/Communication/Ethernet/LWIP/TCP/ServerSocket.cpp @@ -11,298 +11,297 @@ #ifdef HAL_ETH_MODULE_ENABLED uint8_t ServerSocket::priority = 1; -unordered_map ServerSocket::listening_sockets = {}; +unordered_map ServerSocket::listening_sockets = {}; ServerSocket::ServerSocket() = default; ServerSocket::ServerSocket(IPV4 local_ip, uint32_t local_port) : local_ip(local_ip), local_port(local_port) { - if (not Ethernet::is_running) { - ErrorHandler("Cannot declare UDP socket before Ethernet::start()"); - return; - } - tx_packet_buffer = {}; - rx_packet_buffer = {}; - state = INACTIVE; - server_control_block = tcp_new(); - tcp_nagle_disable(server_control_block); - ip_set_option(server_control_block, SOF_REUSEADDR); - err_t error = tcp_bind(server_control_block, &local_ip.address, local_port); - - if (error == ERR_OK) { - server_control_block = tcp_listen(server_control_block); - state = LISTENING; - listening_sockets[local_port] = this; - tcp_accept(server_control_block, accept_callback); - } else { - memp_free(MEMP_TCP_PCB, server_control_block); - ErrorHandler("Cannot bind server socket, error %d", (int16_t)error); - } - OrderProtocol::sockets.push_back(this); + if (not Ethernet::is_running) { + ErrorHandler("Cannot declare UDP socket before Ethernet::start()"); + return; + } + tx_packet_buffer = {}; + rx_packet_buffer = {}; + state = INACTIVE; + server_control_block = tcp_new(); + tcp_nagle_disable(server_control_block); + ip_set_option(server_control_block, SOF_REUSEADDR); + err_t error = tcp_bind(server_control_block, &local_ip.address, local_port); + + if (error == ERR_OK) { + server_control_block = tcp_listen(server_control_block); + state = LISTENING; + listening_sockets[local_port] = this; + tcp_accept(server_control_block, accept_callback); + } else { + memp_free(MEMP_TCP_PCB, server_control_block); + ErrorHandler("Cannot bind server socket, error %d", (int16_t)error); + } + OrderProtocol::sockets.push_back(this); } -ServerSocket::ServerSocket(IPV4 local_ip, uint32_t local_port, - uint32_t inactivity_time_until_keepalive_ms, - uint32_t space_between_tries_ms, - uint32_t tries_until_disconnection) +ServerSocket::ServerSocket( + IPV4 local_ip, + uint32_t local_port, + uint32_t inactivity_time_until_keepalive_ms, + uint32_t space_between_tries_ms, + uint32_t tries_until_disconnection +) : ServerSocket(local_ip, local_port) { - keepalive_config.inactivity_time_until_keepalive_ms = - inactivity_time_until_keepalive_ms; - keepalive_config.space_between_tries_ms = space_between_tries_ms; - keepalive_config.tries_until_disconnection = tries_until_disconnection; + keepalive_config.inactivity_time_until_keepalive_ms = inactivity_time_until_keepalive_ms; + keepalive_config.space_between_tries_ms = space_between_tries_ms; + keepalive_config.tries_until_disconnection = tries_until_disconnection; } -ServerSocket::ServerSocket(ServerSocket &&other) - : local_ip(move(other.local_ip)), local_port(move(other.local_port)), - state(other.state), +ServerSocket::ServerSocket(ServerSocket&& other) + : local_ip(move(other.local_ip)), local_port(move(other.local_port)), state(other.state), server_control_block(move(other.server_control_block)) { - listening_sockets[local_port] = this; - tx_packet_buffer = {}; - rx_packet_buffer = {}; + listening_sockets[local_port] = this; + tx_packet_buffer = {}; + rx_packet_buffer = {}; } -void ServerSocket::operator=(ServerSocket &&other) { - local_ip = move(other.local_ip); - local_port = move(other.local_port); - server_control_block = move(other.server_control_block); - state = other.state; - listening_sockets[local_port] = this; - tx_packet_buffer = {}; - rx_packet_buffer = {}; - if (not(std::find(OrderProtocol::sockets.begin(), - OrderProtocol::sockets.end(), - this) != OrderProtocol::sockets.end())) - OrderProtocol::sockets.push_back(this); +void ServerSocket::operator=(ServerSocket&& other) { + local_ip = move(other.local_ip); + local_port = move(other.local_port); + server_control_block = move(other.server_control_block); + state = other.state; + listening_sockets[local_port] = this; + tx_packet_buffer = {}; + rx_packet_buffer = {}; + if (not(std::find(OrderProtocol::sockets.begin(), OrderProtocol::sockets.end(), this) != + OrderProtocol::sockets.end())) + OrderProtocol::sockets.push_back(this); } ServerSocket::~ServerSocket() { - // el destructor no destruye - auto it = std::find(OrderProtocol::sockets.begin(), - OrderProtocol::sockets.end(), this); - if (it == OrderProtocol::sockets.end()) - return; - else - OrderProtocol::sockets.erase(it); - tcp_abort(client_control_block); - tcp_abort(server_control_block); - while (!tx_packet_buffer.empty()) { - free(tx_packet_buffer.front()); - tx_packet_buffer.pop(); - } - while (!rx_packet_buffer.empty()) { - free(rx_packet_buffer.front()); - rx_packet_buffer.pop(); - } + // el destructor no destruye + auto it = std::find(OrderProtocol::sockets.begin(), OrderProtocol::sockets.end(), this); + if (it == OrderProtocol::sockets.end()) + return; + else + OrderProtocol::sockets.erase(it); + tcp_abort(client_control_block); + tcp_abort(server_control_block); + while (!tx_packet_buffer.empty()) { + free(tx_packet_buffer.front()); + tx_packet_buffer.pop(); + } + while (!rx_packet_buffer.empty()) { + free(rx_packet_buffer.front()); + rx_packet_buffer.pop(); + } } ServerSocket::ServerSocket(EthernetNode local_node) - : ServerSocket(local_node.ip, local_node.port) {}; + : ServerSocket(local_node.ip, local_node.port){}; void ServerSocket::close() { - // Clean all callbacks - tcp_arg(client_control_block, nullptr); - tcp_sent(client_control_block, nullptr); - tcp_recv(client_control_block, nullptr); - tcp_err(client_control_block, nullptr); - tcp_poll(client_control_block, nullptr, 0); - - tcp_close(client_control_block); - while (!tx_packet_buffer.empty()) { - pbuf_free(tx_packet_buffer.front()); - tx_packet_buffer.pop(); - } - while (!rx_packet_buffer.empty()) { - pbuf_free(rx_packet_buffer.front()); - rx_packet_buffer.pop(); - } - - tcp_pcb_remove(&tcp_active_pcbs, client_control_block); - tcp_free(client_control_block); - - listening_sockets[local_port] = this; - state = CLOSED; - - priority--; + // Clean all callbacks + tcp_arg(client_control_block, nullptr); + tcp_sent(client_control_block, nullptr); + tcp_recv(client_control_block, nullptr); + tcp_err(client_control_block, nullptr); + tcp_poll(client_control_block, nullptr, 0); + + tcp_close(client_control_block); + while (!tx_packet_buffer.empty()) { + pbuf_free(tx_packet_buffer.front()); + tx_packet_buffer.pop(); + } + while (!rx_packet_buffer.empty()) { + pbuf_free(rx_packet_buffer.front()); + rx_packet_buffer.pop(); + } + + tcp_pcb_remove(&tcp_active_pcbs, client_control_block); + tcp_free(client_control_block); + + listening_sockets[local_port] = this; + state = CLOSED; + + priority--; } void ServerSocket::process_data() { - while (!rx_packet_buffer.empty()) { - struct pbuf *packet = rx_packet_buffer.front(); - rx_packet_buffer.pop(); - uint8_t *new_data = (uint8_t *)(packet->payload); - tcp_recved(client_control_block, packet->tot_len); - uint16_t id = Packet::get_id(new_data); - if (Order::orders.contains(id)) { - Order::orders[id]->store_ip_order(remote_ip.string_address); - Order::process_data(this, new_data); + while (!rx_packet_buffer.empty()) { + struct pbuf* packet = rx_packet_buffer.front(); + rx_packet_buffer.pop(); + uint8_t* new_data = (uint8_t*)(packet->payload); + tcp_recved(client_control_block, packet->tot_len); + uint16_t id = Packet::get_id(new_data); + if (Order::orders.contains(id)) { + Order::orders[id]->store_ip_order(remote_ip.string_address); + Order::process_data(this, new_data); + } + pbuf_free(packet); } - pbuf_free(packet); - } } -bool ServerSocket::add_order_to_queue(Order &order) { - if (state == ACCEPTED) { - return false; // yet to decide if add_order_to_queue should send the order - // when used after the connection is accepted or just return - // false - } - struct memp *next_memory_pointer_in_packet_buffer_pool = - (*(memp_pools[PBUF_POOL_MEMORY_DESC_POSITION]->tab))->next; - if (next_memory_pointer_in_packet_buffer_pool == nullptr) { - memp_free_pool(memp_pools[PBUF_POOL_MEMORY_DESC_POSITION], - next_memory_pointer_in_packet_buffer_pool); - return false; - } - - uint8_t *order_buffer = order.build(); - - struct pbuf *packet = pbuf_alloc(PBUF_TRANSPORT, order.get_size(), PBUF_POOL); - pbuf_take(packet, order_buffer, order.get_size()); - tx_packet_buffer.push(packet); - return true; +bool ServerSocket::add_order_to_queue(Order& order) { + if (state == ACCEPTED) { + return false; // yet to decide if add_order_to_queue should send the order + // when used after the connection is accepted or just return + // false + } + struct memp* next_memory_pointer_in_packet_buffer_pool = + (*(memp_pools[PBUF_POOL_MEMORY_DESC_POSITION]->tab))->next; + if (next_memory_pointer_in_packet_buffer_pool == nullptr) { + memp_free_pool( + memp_pools[PBUF_POOL_MEMORY_DESC_POSITION], + next_memory_pointer_in_packet_buffer_pool + ); + return false; + } + + uint8_t* order_buffer = order.build(); + + struct pbuf* packet = pbuf_alloc(PBUF_TRANSPORT, order.get_size(), PBUF_POOL); + pbuf_take(packet, order_buffer, order.get_size()); + tx_packet_buffer.push(packet); + return true; } void ServerSocket::send() { - pbuf *temporal_packet_buffer; - err_t error = ERR_OK; - while (error == ERR_OK && !tx_packet_buffer.empty() && - tx_packet_buffer.front()->len <= tcp_sndbuf(client_control_block)) { - temporal_packet_buffer = tx_packet_buffer.front(); - error = tcp_write(client_control_block, temporal_packet_buffer->payload, - temporal_packet_buffer->len, TCP_WRITE_FLAG_COPY); - if (error == ERR_OK) { - tx_packet_buffer.pop(); - tcp_output(client_control_block); - memp_free_pool(memp_pools[PBUF_POOL_MEMORY_DESC_POSITION], - temporal_packet_buffer); - } else { - ErrorHandler("Cannot write to socket, error: %d", error); + pbuf* temporal_packet_buffer; + err_t error = ERR_OK; + while (error == ERR_OK && !tx_packet_buffer.empty() && + tx_packet_buffer.front()->len <= tcp_sndbuf(client_control_block)) { + temporal_packet_buffer = tx_packet_buffer.front(); + error = tcp_write( + client_control_block, + temporal_packet_buffer->payload, + temporal_packet_buffer->len, + TCP_WRITE_FLAG_COPY + ); + if (error == ERR_OK) { + tx_packet_buffer.pop(); + tcp_output(client_control_block); + memp_free_pool(memp_pools[PBUF_POOL_MEMORY_DESC_POSITION], temporal_packet_buffer); + } else { + ErrorHandler("Cannot write to socket, error: %d", error); + } } - } -} - -bool ServerSocket::is_connected() { - return state == ServerSocket::ServerState::ACCEPTED; } -err_t ServerSocket::accept_callback(void *arg, - struct tcp_pcb *incomming_control_block, - err_t error) { - if (listening_sockets.contains(incomming_control_block->local_port)) { - ServerSocket *server_socket = - listening_sockets[incomming_control_block->local_port]; - - server_socket->state = ACCEPTED; - server_socket->client_control_block = incomming_control_block; - server_socket->remote_ip = IPV4(incomming_control_block->remote_ip); - server_socket->rx_packet_buffer = {}; - - tcp_setprio(incomming_control_block, priority); - tcp_nagle_disable(incomming_control_block); - ip_set_option(incomming_control_block, SOF_REUSEADDR); - - tcp_arg(incomming_control_block, server_socket); - tcp_recv(incomming_control_block, receive_callback); - tcp_sent(incomming_control_block, send_callback); - tcp_err(incomming_control_block, error_callback); - tcp_poll(incomming_control_block, poll_callback, 0); - config_keepalive(incomming_control_block, server_socket); - - tcp_close(server_socket->server_control_block); - priority++; - - return ERR_OK; - } else - return ERROR; +bool ServerSocket::is_connected() { return state == ServerSocket::ServerState::ACCEPTED; } + +err_t ServerSocket::accept_callback( + void* arg, + struct tcp_pcb* incomming_control_block, + err_t error +) { + if (listening_sockets.contains(incomming_control_block->local_port)) { + ServerSocket* server_socket = listening_sockets[incomming_control_block->local_port]; + + server_socket->state = ACCEPTED; + server_socket->client_control_block = incomming_control_block; + server_socket->remote_ip = IPV4(incomming_control_block->remote_ip); + server_socket->rx_packet_buffer = {}; + + tcp_setprio(incomming_control_block, priority); + tcp_nagle_disable(incomming_control_block); + ip_set_option(incomming_control_block, SOF_REUSEADDR); + + tcp_arg(incomming_control_block, server_socket); + tcp_recv(incomming_control_block, receive_callback); + tcp_sent(incomming_control_block, send_callback); + tcp_err(incomming_control_block, error_callback); + tcp_poll(incomming_control_block, poll_callback, 0); + config_keepalive(incomming_control_block, server_socket); + + tcp_close(server_socket->server_control_block); + priority++; + + return ERR_OK; + } else + return ERROR; } -err_t ServerSocket::receive_callback(void *arg, - struct tcp_pcb *client_control_block, - struct pbuf *packet_buffer, err_t error) { - ServerSocket *server_socket = (ServerSocket *)arg; - server_socket->client_control_block = client_control_block; - - if (packet_buffer == nullptr) { // FIN has been received - server_socket->state = CLOSING; - return ERR_OK; - } +err_t ServerSocket::receive_callback( + void* arg, + struct tcp_pcb* client_control_block, + struct pbuf* packet_buffer, + err_t error +) { + ServerSocket* server_socket = (ServerSocket*)arg; + server_socket->client_control_block = client_control_block; + + if (packet_buffer == nullptr) { // FIN has been received + server_socket->state = CLOSING; + return ERR_OK; + } - if (error != ERR_OK) { // Check if packet is valid - if (packet_buffer != nullptr) { - pbuf_free(packet_buffer); + if (error != ERR_OK) { // Check if packet is valid + if (packet_buffer != nullptr) { + pbuf_free(packet_buffer); + } + return error; + } else if (server_socket->state == ACCEPTED) { + server_socket->rx_packet_buffer.push(packet_buffer); + server_socket->process_data(); + return ERR_OK; } - return error; - } else if (server_socket->state == ACCEPTED) { - server_socket->rx_packet_buffer.push(packet_buffer); - server_socket->process_data(); - return ERR_OK; - } - else if (server_socket->state == CLOSING) { // Socket is already closed - while (not server_socket->rx_packet_buffer.empty()) { - pbuf_free(server_socket->rx_packet_buffer.front()); - server_socket->rx_packet_buffer.pop(); + else if (server_socket->state == CLOSING) { // Socket is already closed + while (not server_socket->rx_packet_buffer.empty()) { + pbuf_free(server_socket->rx_packet_buffer.front()); + server_socket->rx_packet_buffer.pop(); + } + server_socket->rx_packet_buffer = {}; + pbuf_free(packet_buffer); + return ERR_OK; } - server_socket->rx_packet_buffer = {}; - pbuf_free(packet_buffer); return ERR_OK; - } - return ERR_OK; } -void ServerSocket::error_callback(void *arg, err_t error) { - ServerSocket *server_socket = (ServerSocket *)arg; - server_socket->close(); - ErrorHandler("Socket error: %d. Socket closed", error); +void ServerSocket::error_callback(void* arg, err_t error) { + ServerSocket* server_socket = (ServerSocket*)arg; + server_socket->close(); + ErrorHandler("Socket error: %d. Socket closed", error); } -err_t ServerSocket::poll_callback(void *arg, - struct tcp_pcb *client_control_block) { - ServerSocket *server_socket = (ServerSocket *)arg; - server_socket->client_control_block = client_control_block; +err_t ServerSocket::poll_callback(void* arg, struct tcp_pcb* client_control_block) { + ServerSocket* server_socket = (ServerSocket*)arg; + server_socket->client_control_block = client_control_block; - if (server_socket == nullptr) { // Polling non existing pcb, fatal error - tcp_abort(client_control_block); - return ERR_ABRT; - } + if (server_socket == nullptr) { // Polling non existing pcb, fatal error + tcp_abort(client_control_block); + return ERR_ABRT; + } - while (not server_socket->tx_packet_buffer.empty()) { // TX FIFO is not empty - server_socket->send(); - } + while (not server_socket->tx_packet_buffer.empty()) { // TX FIFO is not empty + server_socket->send(); + } - while (not server_socket->rx_packet_buffer.empty()) { // RX FIFO is not empty - server_socket->process_data(); - } + while (not server_socket->rx_packet_buffer.empty()) { // RX FIFO is not empty + server_socket->process_data(); + } - if (server_socket->state == CLOSING) { // pcb has been polled to close - server_socket->close(); - } + if (server_socket->state == CLOSING) { // pcb has been polled to close + server_socket->close(); + } - return ERR_OK; + return ERR_OK; } -err_t ServerSocket::send_callback(void *arg, - struct tcp_pcb *client_control_block, - u16_t len) { - ServerSocket *server_socket = (ServerSocket *)arg; - server_socket->client_control_block = client_control_block; - if (!server_socket->tx_packet_buffer.empty()) { - server_socket->send(); - } else if (server_socket->state == CLOSING) { - server_socket->close(); - } - return ERR_OK; +err_t ServerSocket::send_callback(void* arg, struct tcp_pcb* client_control_block, u16_t len) { + ServerSocket* server_socket = (ServerSocket*)arg; + server_socket->client_control_block = client_control_block; + if (!server_socket->tx_packet_buffer.empty()) { + server_socket->send(); + } else if (server_socket->state == CLOSING) { + server_socket->close(); + } + return ERR_OK; } -void ServerSocket::config_keepalive(tcp_pcb *control_block, - ServerSocket *server_socket) { - control_block->so_options |= SOF_KEEPALIVE; - control_block->keep_idle = - server_socket->keepalive_config.inactivity_time_until_keepalive_ms; - control_block->keep_intvl = - server_socket->keepalive_config.space_between_tries_ms; - control_block->keep_cnt = - server_socket->keepalive_config.tries_until_disconnection; +void ServerSocket::config_keepalive(tcp_pcb* control_block, ServerSocket* server_socket) { + control_block->so_options |= SOF_KEEPALIVE; + control_block->keep_idle = server_socket->keepalive_config.inactivity_time_until_keepalive_ms; + control_block->keep_intvl = server_socket->keepalive_config.space_between_tries_ms; + control_block->keep_cnt = server_socket->keepalive_config.tries_until_disconnection; } #endif // HAL_ETH_MODULE_ENABLED diff --git a/Src/HALAL/Services/Communication/Ethernet/LWIP/TCP/Socket.cpp b/Src/HALAL/Services/Communication/Ethernet/LWIP/TCP/Socket.cpp index ece569919..1666cde53 100644 --- a/Src/HALAL/Services/Communication/Ethernet/LWIP/TCP/Socket.cpp +++ b/Src/HALAL/Services/Communication/Ethernet/LWIP/TCP/Socket.cpp @@ -8,315 +8,328 @@ #include "ErrorHandler/ErrorHandler.hpp" #ifdef HAL_ETH_MODULE_ENABLED -unordered_map Socket::connecting_sockets = {}; +unordered_map Socket::connecting_sockets = {}; Socket::Socket() = default; -Socket::Socket(Socket &&other) +Socket::Socket(Socket&& other) : connection_control_block(move(other.connection_control_block)), remote_port(move(remote_port)), state(other.state) { - EthernetNode remote_node(other.remote_ip, other.remote_port); - connecting_sockets[remote_node] = this; + EthernetNode remote_node(other.remote_ip, other.remote_port); + connecting_sockets[remote_node] = this; } -void Socket::operator=(Socket &&other) { - connection_control_block = move(other.connection_control_block); - remote_port = move(other.remote_port); - state = other.state; - EthernetNode remote_node(other.remote_ip, other.remote_port); - connecting_sockets[remote_node] = this; - if (std::find(OrderProtocol::sockets.begin(), OrderProtocol::sockets.end(), - this) == OrderProtocol::sockets.end()) - OrderProtocol::sockets.push_back(this); +void Socket::operator=(Socket&& other) { + connection_control_block = move(other.connection_control_block); + remote_port = move(other.remote_port); + state = other.state; + EthernetNode remote_node(other.remote_ip, other.remote_port); + connecting_sockets[remote_node] = this; + if (std::find(OrderProtocol::sockets.begin(), OrderProtocol::sockets.end(), this) == + OrderProtocol::sockets.end()) + OrderProtocol::sockets.push_back(this); } Socket::~Socket() { - auto it = std::find(OrderProtocol::sockets.begin(), - OrderProtocol::sockets.end(), this); - if (it == OrderProtocol::sockets.end()) - return; - else - OrderProtocol::sockets.erase(it); + auto it = std::find(OrderProtocol::sockets.begin(), OrderProtocol::sockets.end(), this); + if (it == OrderProtocol::sockets.end()) + return; + else + OrderProtocol::sockets.erase(it); } -Socket::Socket(IPV4 local_ip, uint32_t local_port, IPV4 remote_ip, - uint32_t remote_port, bool use_keep_alive) - : local_ip(local_ip), local_port(local_port), remote_ip(remote_ip), - remote_port(remote_port), use_keep_alives{use_keep_alive} { - if (not Ethernet::is_running) { - ErrorHandler("Cannot declare TCP socket before Ethernet::start()"); - return; - } - state = INACTIVE; - tx_packet_buffer = {}; - rx_packet_buffer = {}; - EthernetNode remote_node(remote_ip, remote_port); - - connection_control_block = tcp_new(); - tcp_bind(connection_control_block, &local_ip.address, local_port); - tcp_nagle_disable(connection_control_block); - tcp_arg(connection_control_block, this); - tcp_poll(connection_control_block, connection_poll_callback, 1); - tcp_err(connection_control_block, connection_error_callback); - - connecting_sockets[remote_node] = this; - tcp_connect(connection_control_block, &remote_ip.address, remote_port, - connect_callback); - OrderProtocol::sockets.push_back(this); +Socket::Socket( + IPV4 local_ip, + uint32_t local_port, + IPV4 remote_ip, + uint32_t remote_port, + bool use_keep_alive +) + : local_ip(local_ip), local_port(local_port), remote_ip(remote_ip), remote_port(remote_port), + use_keep_alives{use_keep_alive} { + if (not Ethernet::is_running) { + ErrorHandler("Cannot declare TCP socket before Ethernet::start()"); + return; + } + state = INACTIVE; + tx_packet_buffer = {}; + rx_packet_buffer = {}; + EthernetNode remote_node(remote_ip, remote_port); + + connection_control_block = tcp_new(); + tcp_bind(connection_control_block, &local_ip.address, local_port); + tcp_nagle_disable(connection_control_block); + tcp_arg(connection_control_block, this); + tcp_poll(connection_control_block, connection_poll_callback, 1); + tcp_err(connection_control_block, connection_error_callback); + + connecting_sockets[remote_node] = this; + tcp_connect(connection_control_block, &remote_ip.address, remote_port, connect_callback); + OrderProtocol::sockets.push_back(this); } -Socket::Socket(IPV4 local_ip, uint32_t local_port, IPV4 remote_ip, - uint32_t remote_port, - uint32_t inactivity_time_until_keepalive_ms, - uint32_t space_between_tries_ms, - uint32_t tries_until_disconnection) +Socket::Socket( + IPV4 local_ip, + uint32_t local_port, + IPV4 remote_ip, + uint32_t remote_port, + uint32_t inactivity_time_until_keepalive_ms, + uint32_t space_between_tries_ms, + uint32_t tries_until_disconnection +) : Socket(local_ip, local_port, remote_ip, remote_port) { - keepalive_config.inactivity_time_until_keepalive_ms = - inactivity_time_until_keepalive_ms; - keepalive_config.space_between_tries_ms = space_between_tries_ms; - keepalive_config.tries_until_disconnection = tries_until_disconnection; + keepalive_config.inactivity_time_until_keepalive_ms = inactivity_time_until_keepalive_ms; + keepalive_config.space_between_tries_ms = space_between_tries_ms; + keepalive_config.tries_until_disconnection = tries_until_disconnection; } Socket::Socket(EthernetNode local_node, EthernetNode remote_node) - : Socket(local_node.ip, local_node.port, remote_node.ip, remote_node.port) { -} + : Socket(local_node.ip, local_node.port, remote_node.ip, remote_node.port) {} void Socket::close() { - tcp_arg(socket_control_block, nullptr); - tcp_sent(socket_control_block, nullptr); - tcp_recv(socket_control_block, nullptr); - tcp_err(socket_control_block, nullptr); - tcp_poll(socket_control_block, nullptr, 0); - - while (!tx_packet_buffer.empty()) { - pbuf_free(tx_packet_buffer.front()); - tx_packet_buffer.pop(); - } - while (!rx_packet_buffer.empty()) { - pbuf_free(rx_packet_buffer.front()); - rx_packet_buffer.pop(); - } - - tcp_close(socket_control_block); - state = INACTIVE; + tcp_arg(socket_control_block, nullptr); + tcp_sent(socket_control_block, nullptr); + tcp_recv(socket_control_block, nullptr); + tcp_err(socket_control_block, nullptr); + tcp_poll(socket_control_block, nullptr, 0); + + while (!tx_packet_buffer.empty()) { + pbuf_free(tx_packet_buffer.front()); + tx_packet_buffer.pop(); + } + while (!rx_packet_buffer.empty()) { + pbuf_free(rx_packet_buffer.front()); + rx_packet_buffer.pop(); + } + + tcp_close(socket_control_block); + state = INACTIVE; } void Socket::reconnect() { - EthernetNode remote_node(remote_ip, remote_port); - if (!connecting_sockets.contains(remote_node)) { - connecting_sockets[remote_node] = this; - } - tcp_connect(connection_control_block, &remote_ip.address, remote_port, - connect_callback); + EthernetNode remote_node(remote_ip, remote_port); + if (!connecting_sockets.contains(remote_node)) { + connecting_sockets[remote_node] = this; + } + tcp_connect(connection_control_block, &remote_ip.address, remote_port, connect_callback); } void Socket::reset() { - EthernetNode remote_node(remote_ip, remote_port); - if (!connecting_sockets.contains(remote_node)) { - connecting_sockets[remote_node] = this; - } - state = INACTIVE; - tcp_abort(connection_control_block); - connection_control_block = tcp_new(); - - tcp_bind(connection_control_block, &local_ip.address, local_port); - tcp_nagle_disable(connection_control_block); - tcp_arg(connection_control_block, this); - tcp_poll(connection_control_block, connection_poll_callback, 1); - tcp_err(connection_control_block, connection_error_callback); - - tcp_connect(connection_control_block, &remote_ip.address, remote_port, - connect_callback); + EthernetNode remote_node(remote_ip, remote_port); + if (!connecting_sockets.contains(remote_node)) { + connecting_sockets[remote_node] = this; + } + state = INACTIVE; + tcp_abort(connection_control_block); + connection_control_block = tcp_new(); + + tcp_bind(connection_control_block, &local_ip.address, local_port); + tcp_nagle_disable(connection_control_block); + tcp_arg(connection_control_block, this); + tcp_poll(connection_control_block, connection_poll_callback, 1); + tcp_err(connection_control_block, connection_error_callback); + + tcp_connect(connection_control_block, &remote_ip.address, remote_port, connect_callback); } void Socket::send() { - pbuf *temporal_packet_buffer; - err_t error = ERR_OK; - while (error == ERR_OK && !tx_packet_buffer.empty() && - tx_packet_buffer.front()->len <= tcp_sndbuf(socket_control_block)) { - temporal_packet_buffer = tx_packet_buffer.front(); - error = tcp_write(socket_control_block, temporal_packet_buffer->payload, - temporal_packet_buffer->len, TCP_WRITE_FLAG_COPY); - if (error == ERR_OK) { - tx_packet_buffer.pop(); - tcp_output(socket_control_block); - memp_free_pool(memp_pools[PBUF_POOL_MEMORY_DESC_POSITION], - temporal_packet_buffer); - } else { - if (error == ERR_MEM) { - close(); - ErrorHandler( - "Too many unacked messages on client socket, disconnecting..."); - } else { - ErrorHandler("Cannot write to client socket. Error code: %d", error); - } + pbuf* temporal_packet_buffer; + err_t error = ERR_OK; + while (error == ERR_OK && !tx_packet_buffer.empty() && + tx_packet_buffer.front()->len <= tcp_sndbuf(socket_control_block)) { + temporal_packet_buffer = tx_packet_buffer.front(); + error = tcp_write( + socket_control_block, + temporal_packet_buffer->payload, + temporal_packet_buffer->len, + TCP_WRITE_FLAG_COPY + ); + if (error == ERR_OK) { + tx_packet_buffer.pop(); + tcp_output(socket_control_block); + memp_free_pool(memp_pools[PBUF_POOL_MEMORY_DESC_POSITION], temporal_packet_buffer); + } else { + if (error == ERR_MEM) { + close(); + ErrorHandler("Too many unacked messages on client socket, disconnecting..."); + } else { + ErrorHandler("Cannot write to client socket. Error code: %d", error); + } + } } - } } void Socket::process_data() { - while (!rx_packet_buffer.empty()) { - struct pbuf *packet = rx_packet_buffer.front(); - rx_packet_buffer.pop(); - uint8_t *new_data = (uint8_t *)(packet->payload); - tcp_recved(socket_control_block, packet->tot_len); - uint16_t id = Packet::get_id(new_data); - if (Order::orders.contains(id)) { - Order::orders[id]->store_ip_order(remote_ip.string_address); - Order::process_data(this, new_data); + while (!rx_packet_buffer.empty()) { + struct pbuf* packet = rx_packet_buffer.front(); + rx_packet_buffer.pop(); + uint8_t* new_data = (uint8_t*)(packet->payload); + tcp_recved(socket_control_block, packet->tot_len); + uint16_t id = Packet::get_id(new_data); + if (Order::orders.contains(id)) { + Order::orders[id]->store_ip_order(remote_ip.string_address); + Order::process_data(this, new_data); + } + + pbuf_free(packet); } - - pbuf_free(packet); - } } -bool Socket::add_order_to_queue(Order &order) { - if (state == Socket::SocketState::CONNECTED) { - return false; - } - struct memp *next_memory_pointer_in_packet_buffer_pool = - (*(memp_pools[PBUF_POOL_MEMORY_DESC_POSITION]->tab))->next; - if (next_memory_pointer_in_packet_buffer_pool == nullptr) { - memp_free_pool(memp_pools[PBUF_POOL_MEMORY_DESC_POSITION], - next_memory_pointer_in_packet_buffer_pool); - return false; - } - - uint8_t *order_buffer = order.build(); - - struct pbuf *packet = pbuf_alloc(PBUF_TRANSPORT, order.get_size(), PBUF_POOL); - pbuf_take(packet, order_buffer, order.get_size()); - Socket::tx_packet_buffer.push(packet); - return true; +bool Socket::add_order_to_queue(Order& order) { + if (state == Socket::SocketState::CONNECTED) { + return false; + } + struct memp* next_memory_pointer_in_packet_buffer_pool = + (*(memp_pools[PBUF_POOL_MEMORY_DESC_POSITION]->tab))->next; + if (next_memory_pointer_in_packet_buffer_pool == nullptr) { + memp_free_pool( + memp_pools[PBUF_POOL_MEMORY_DESC_POSITION], + next_memory_pointer_in_packet_buffer_pool + ); + return false; + } + + uint8_t* order_buffer = order.build(); + + struct pbuf* packet = pbuf_alloc(PBUF_TRANSPORT, order.get_size(), PBUF_POOL); + pbuf_take(packet, order_buffer, order.get_size()); + Socket::tx_packet_buffer.push(packet); + return true; } bool Socket::is_connected() { return state == Socket::SocketState::CONNECTED; } -err_t Socket::connect_callback(void *arg, struct tcp_pcb *client_control_block, - err_t error) { - IPV4 remote_ip; - remote_ip.address = client_control_block->remote_ip; - EthernetNode remote_node(remote_ip, client_control_block->remote_port); - - if (connecting_sockets.contains(remote_node)) { - Socket *socket = connecting_sockets[remote_node]; - connecting_sockets.erase(remote_node); +err_t Socket::connect_callback(void* arg, struct tcp_pcb* client_control_block, err_t error) { + IPV4 remote_ip; + remote_ip.address = client_control_block->remote_ip; + EthernetNode remote_node(remote_ip, client_control_block->remote_port); + + if (connecting_sockets.contains(remote_node)) { + Socket* socket = connecting_sockets[remote_node]; + connecting_sockets.erase(remote_node); + + socket->socket_control_block = client_control_block; + socket->state = CONNECTED; + + tcp_nagle_disable(client_control_block); + tcp_arg(client_control_block, socket); + tcp_recv(client_control_block, receive_callback); + tcp_poll(client_control_block, poll_callback, 0); + tcp_sent(client_control_block, send_callback); + tcp_err(client_control_block, error_callback); + if (socket->use_keep_alives) + config_keepalive(client_control_block, socket); + + return ERR_OK; + } else + return ERROR; +} +err_t Socket::receive_callback( + void* arg, + struct tcp_pcb* client_control_block, + struct pbuf* packet_buffer, + err_t error +) { + Socket* socket = (Socket*)arg; socket->socket_control_block = client_control_block; - socket->state = CONNECTED; - - tcp_nagle_disable(client_control_block); - tcp_arg(client_control_block, socket); - tcp_recv(client_control_block, receive_callback); - tcp_poll(client_control_block, poll_callback, 0); - tcp_sent(client_control_block, send_callback); - tcp_err(client_control_block, error_callback); - if (socket->use_keep_alives) - config_keepalive(client_control_block, socket); - - return ERR_OK; - } else - return ERROR; + if (packet_buffer == nullptr) { // FIN is received + socket->state = CLOSING; + return ERR_OK; + } + if (error != ERR_OK) { + if (packet_buffer != nullptr) { + pbuf_free(packet_buffer); + } + return error; + } else if (socket->state == CONNECTED) { + socket->rx_packet_buffer.push(packet_buffer); + tcp_recved(client_control_block, packet_buffer->tot_len); + socket->process_data(); + pbuf_free(packet_buffer); + return ERR_OK; + } else { + tcp_recved(client_control_block, packet_buffer->tot_len); + socket->tx_packet_buffer = {}; + pbuf_free(packet_buffer); + return ERR_OK; + } } -err_t Socket::receive_callback(void *arg, struct tcp_pcb *client_control_block, - struct pbuf *packet_buffer, err_t error) { - Socket *socket = (Socket *)arg; - socket->socket_control_block = client_control_block; - if (packet_buffer == nullptr) { // FIN is received - socket->state = CLOSING; - return ERR_OK; - } - if (error != ERR_OK) { - if (packet_buffer != nullptr) { - pbuf_free(packet_buffer); +err_t Socket::poll_callback(void* arg, struct tcp_pcb* client_control_block) { + Socket* socket = (Socket*)arg; + socket->socket_control_block = client_control_block; + if (socket != nullptr) { + while (not socket->tx_packet_buffer.empty()) { + socket->send(); + } + if (socket->state == CLOSING) { + socket->close(); + } else if (socket->state == INACTIVE) { + tcp_connect( + socket->connection_control_block, + &socket->remote_ip.address, + socket->remote_port, + connect_callback + ); + } + return ERR_OK; + } else { + tcp_abort(client_control_block); + return ERR_ABRT; } - return error; - } else if (socket->state == CONNECTED) { - socket->rx_packet_buffer.push(packet_buffer); - tcp_recved(client_control_block, packet_buffer->tot_len); - socket->process_data(); - pbuf_free(packet_buffer); - return ERR_OK; - } else { - tcp_recved(client_control_block, packet_buffer->tot_len); - socket->tx_packet_buffer = {}; - pbuf_free(packet_buffer); - return ERR_OK; - } } -err_t Socket::poll_callback(void *arg, struct tcp_pcb *client_control_block) { - Socket *socket = (Socket *)arg; - socket->socket_control_block = client_control_block; - if (socket != nullptr) { - while (not socket->tx_packet_buffer.empty()) { - socket->send(); - } - if (socket->state == CLOSING) { - socket->close(); - } else if (socket->state == INACTIVE) { - tcp_connect(socket->connection_control_block, &socket->remote_ip.address, - socket->remote_port, connect_callback); +err_t Socket::send_callback(void* arg, struct tcp_pcb* client_control_block, uint16_t length) { + Socket* socket = (Socket*)arg; + socket->socket_control_block = client_control_block; + if (not socket->tx_packet_buffer.empty()) { + socket->send(); + } else if (socket->state == CLOSING) { + socket->close(); } return ERR_OK; - } else { - tcp_abort(client_control_block); - return ERR_ABRT; - } } -err_t Socket::send_callback(void *arg, struct tcp_pcb *client_control_block, - uint16_t length) { - Socket *socket = (Socket *)arg; - socket->socket_control_block = client_control_block; - if (not socket->tx_packet_buffer.empty()) { - socket->send(); - } else if (socket->state == CLOSING) { +void Socket::error_callback(void* arg, err_t error) { + Socket* socket = (Socket*)arg; socket->close(); - } - return ERR_OK; + ErrorHandler( + "Client socket error: %d. Socket closed, remote ip: %s", + error, + socket->remote_ip.string_address.c_str() + ); } -void Socket::error_callback(void *arg, err_t error) { - Socket *socket = (Socket *)arg; - socket->close(); - ErrorHandler("Client socket error: %d. Socket closed, remote ip: %s", error, - socket->remote_ip.string_address.c_str()); -} +void Socket::connection_error_callback(void* arg, err_t error) { + if (error == ERR_RST) { + Socket* socket = (Socket*)arg; -void Socket::connection_error_callback(void *arg, err_t error) { - if (error == ERR_RST) { - Socket *socket = (Socket *)arg; - - socket->pending_connection_reset = true; - return; - } else if (error == ERR_ABRT) { - return; - } - ErrorHandler("Connection socket error: %d. Couldn t start client socket ", - error); + socket->pending_connection_reset = true; + return; + } else if (error == ERR_ABRT) { + return; + } + ErrorHandler("Connection socket error: %d. Couldn t start client socket ", error); } -err_t Socket::connection_poll_callback( - void *arg, struct tcp_pcb *connection_control_block) { - Socket *socket = (Socket *)arg; - if (socket->pending_connection_reset) { - socket->reset(); - socket->pending_connection_reset = false; - return ERR_ABRT; - } else if (socket->connection_control_block->state == SYN_SENT) { - socket->pending_connection_reset = true; - } - return ERR_OK; +err_t Socket::connection_poll_callback(void* arg, struct tcp_pcb* connection_control_block) { + Socket* socket = (Socket*)arg; + if (socket->pending_connection_reset) { + socket->reset(); + socket->pending_connection_reset = false; + return ERR_ABRT; + } else if (socket->connection_control_block->state == SYN_SENT) { + socket->pending_connection_reset = true; + } + return ERR_OK; } -void Socket::config_keepalive(tcp_pcb *control_block, Socket *socket) { - control_block->so_options |= SOF_KEEPALIVE; - control_block->keep_idle = - socket->keepalive_config.inactivity_time_until_keepalive_ms; - control_block->keep_intvl = socket->keepalive_config.space_between_tries_ms; - control_block->keep_cnt = socket->keepalive_config.tries_until_disconnection; +void Socket::config_keepalive(tcp_pcb* control_block, Socket* socket) { + control_block->so_options |= SOF_KEEPALIVE; + control_block->keep_idle = socket->keepalive_config.inactivity_time_until_keepalive_ms; + control_block->keep_intvl = socket->keepalive_config.space_between_tries_ms; + control_block->keep_cnt = socket->keepalive_config.tries_until_disconnection; } #endif diff --git a/Src/HALAL/Services/Communication/Ethernet/LWIP/UDP/DatagramSocket.cpp b/Src/HALAL/Services/Communication/Ethernet/LWIP/UDP/DatagramSocket.cpp index 006e7995a..eac562494 100644 --- a/Src/HALAL/Services/Communication/Ethernet/LWIP/UDP/DatagramSocket.cpp +++ b/Src/HALAL/Services/Communication/Ethernet/LWIP/UDP/DatagramSocket.cpp @@ -12,85 +12,88 @@ DatagramSocket::DatagramSocket() = default; -DatagramSocket::DatagramSocket(DatagramSocket &&other) - : udp_control_block(move(other.udp_control_block)), - local_ip(move(other.local_ip)), local_port(move(other.local_port)), - remote_ip(move(other.remote_ip)), remote_port(move(remote_port)) {} +DatagramSocket::DatagramSocket(DatagramSocket&& other) + : udp_control_block(move(other.udp_control_block)), local_ip(move(other.local_ip)), + local_port(move(other.local_port)), remote_ip(move(other.remote_ip)), + remote_port(move(remote_port)) {} -DatagramSocket::DatagramSocket(IPV4 local_ip, uint32_t local_port, - IPV4 remote_ip, uint32_t remote_port) - : local_ip(local_ip), local_port(local_port), remote_ip(remote_ip), - remote_port(remote_port) { - if (not Ethernet::is_running) { - ErrorHandler("Cannot declare UDP socket before Ethernet::start()"); - return; - } - udp_control_block = udp_new(); - err_t error = udp_bind(udp_control_block, &local_ip.address, local_port); +DatagramSocket::DatagramSocket( + IPV4 local_ip, + uint32_t local_port, + IPV4 remote_ip, + uint32_t remote_port +) + : local_ip(local_ip), local_port(local_port), remote_ip(remote_ip), remote_port(remote_port) { + if (not Ethernet::is_running) { + ErrorHandler("Cannot declare UDP socket before Ethernet::start()"); + return; + } + udp_control_block = udp_new(); + err_t error = udp_bind(udp_control_block, &local_ip.address, local_port); - if (error == ERR_OK) { - udp_recv(udp_control_block, receive_callback, nullptr); - udp_connect(udp_control_block, &remote_ip.address, remote_port); - is_disconnected = false; - Ethernet::update(); - } else { - udp_remove(udp_control_block); - is_disconnected = true; - ErrorHandler("Error binding UDP socket"); - } + if (error == ERR_OK) { + udp_recv(udp_control_block, receive_callback, nullptr); + udp_connect(udp_control_block, &remote_ip.address, remote_port); + is_disconnected = false; + Ethernet::update(); + } else { + udp_remove(udp_control_block); + is_disconnected = true; + ErrorHandler("Error binding UDP socket"); + } } -DatagramSocket::DatagramSocket(EthernetNode local_node, - EthernetNode remote_node) - : DatagramSocket(local_node.ip, local_node.port, remote_node.ip, - remote_node.port) {} +DatagramSocket::DatagramSocket(EthernetNode local_node, EthernetNode remote_node) + : DatagramSocket(local_node.ip, local_node.port, remote_node.ip, remote_node.port) {} DatagramSocket::~DatagramSocket() { - if (not is_disconnected) - close(); + if (not is_disconnected) + close(); } -void DatagramSocket::operator=(DatagramSocket &&other) { - udp_control_block = move(other.udp_control_block); - local_ip = move(other.local_ip); - local_port = move(other.local_port); - remote_ip = other.remote_ip; - remote_port = other.remote_port; - other.is_disconnected = true; +void DatagramSocket::operator=(DatagramSocket&& other) { + udp_control_block = move(other.udp_control_block); + local_ip = move(other.local_ip); + local_port = move(other.local_port); + remote_ip = other.remote_ip; + remote_port = other.remote_port; + other.is_disconnected = true; } void DatagramSocket::reconnect() { - udp_disconnect(udp_control_block); - is_disconnected = true; - err_t error = udp_bind(udp_control_block, &local_ip.address, local_port); - - if (error == ERR_OK) { - udp_recv(udp_control_block, receive_callback, nullptr); - udp_connect(udp_control_block, &remote_ip.address, remote_port); - is_disconnected = false; - Ethernet::update(); - } else { - udp_remove(udp_control_block); + udp_disconnect(udp_control_block); is_disconnected = true; - ErrorHandler("Error binding UDP socket"); - } + err_t error = udp_bind(udp_control_block, &local_ip.address, local_port); + + if (error == ERR_OK) { + udp_recv(udp_control_block, receive_callback, nullptr); + udp_connect(udp_control_block, &remote_ip.address, remote_port); + is_disconnected = false; + Ethernet::update(); + } else { + udp_remove(udp_control_block); + is_disconnected = true; + ErrorHandler("Error binding UDP socket"); + } } void DatagramSocket::close() { - udp_disconnect(udp_control_block); - udp_remove(udp_control_block); - is_disconnected = true; + udp_disconnect(udp_control_block); + udp_remove(udp_control_block); + is_disconnected = true; } -void DatagramSocket::receive_callback(void *args, - struct udp_pcb *udp_control_block, - struct pbuf *packet_buffer, - const ip_addr_t *remote_address, - u16_t port) { - uint8_t *received_data = (uint8_t *)packet_buffer->payload; - Packet::parse_data(received_data); +void DatagramSocket::receive_callback( + void* args, + struct udp_pcb* udp_control_block, + struct pbuf* packet_buffer, + const ip_addr_t* remote_address, + u16_t port +) { + uint8_t* received_data = (uint8_t*)packet_buffer->payload; + Packet::parse_data(received_data); - pbuf_free(packet_buffer); + pbuf_free(packet_buffer); } #endif diff --git a/Src/HALAL/Services/Communication/Ethernet/PHY/ksz8041.c b/Src/HALAL/Services/Communication/Ethernet/PHY/ksz8041.c index 3225f984b..3f8684b69 100644 --- a/Src/HALAL/Services/Communication/Ethernet/PHY/ksz8041.c +++ b/Src/HALAL/Services/Communication/Ethernet/PHY/ksz8041.c @@ -9,32 +9,31 @@ extern ETH_HandleTypeDef heth; #endif static uint32_t phy_read(uint32_t reg) { - uint32_t val; - HAL_ETH_ReadPHYRegister(&heth, KSZ8041_PHY_ADDRESS, reg, &val); - return val; + uint32_t val; + HAL_ETH_ReadPHYRegister(&heth, KSZ8041_PHY_ADDRESS, reg, &val); + return val; } static phy_link_state_t ksz8041_get_link_state(void) { - static uint8_t link_latched = 0; + static uint8_t link_latched = 0; - uint32_t bmsr; - HAL_ETH_ReadPHYRegister(&heth, KSZ8041_PHY_ADDRESS, 1, &bmsr); - HAL_ETH_ReadPHYRegister(&heth, KSZ8041_PHY_ADDRESS, 1, &bmsr); + uint32_t bmsr; + HAL_ETH_ReadPHYRegister(&heth, KSZ8041_PHY_ADDRESS, 1, &bmsr); + HAL_ETH_ReadPHYRegister(&heth, KSZ8041_PHY_ADDRESS, 1, &bmsr); - if (!(bmsr & (1 << 2))) { - link_latched = 0; - return PHY_LINK_DOWN; - } + if (!(bmsr & (1 << 2))) { + link_latched = 0; + return PHY_LINK_DOWN; + } - if (!link_latched) { - link_latched = 1; - return PHY_100_FULL; - } + if (!link_latched) { + link_latched = 1; + return PHY_100_FULL; + } - return PHY_100_FULL; + return PHY_100_FULL; } -const phy_driver_t phy_ksz8041 = {.init = NULL, - .get_link_state = ksz8041_get_link_state}; +const phy_driver_t phy_ksz8041 = {.init = NULL, .get_link_state = ksz8041_get_link_state}; #endif diff --git a/Src/HALAL/Services/Communication/Ethernet/PHY/lan8700.c b/Src/HALAL/Services/Communication/Ethernet/PHY/lan8700.c index efef62ef9..f3b89dc86 100644 --- a/Src/HALAL/Services/Communication/Ethernet/PHY/lan8700.c +++ b/Src/HALAL/Services/Communication/Ethernet/PHY/lan8700.c @@ -31,120 +31,118 @@ extern ETH_HandleTypeDef heth; #define LAN8700_ANAR_100FD 0x0100U #define LAN8700_ANAR_100HD 0x0080U -#define LAN8700_ANAR_10FD 0x0040U -#define LAN8700_ANAR_10HD 0x0020U - +#define LAN8700_ANAR_10FD 0x0040U +#define LAN8700_ANAR_10HD 0x0020U static uint32_t lan8700_active_addr = LAN8700_PHY_ADDRESS; static uint32_t lan8700_read(uint32_t reg) { - uint32_t val = 0; - HAL_ETH_ReadPHYRegister(&heth, lan8700_active_addr, reg, &val); - return val; + uint32_t val = 0; + HAL_ETH_ReadPHYRegister(&heth, lan8700_active_addr, reg, &val); + return val; } static void lan8700_write(uint32_t reg, uint32_t val) { - HAL_ETH_WritePHYRegister(&heth, lan8700_active_addr, reg, val); + HAL_ETH_WritePHYRegister(&heth, lan8700_active_addr, reg, val); } static void lan8700_wait_reset_clear(void) { - uint32_t start = HAL_GetTick(); - while (HAL_GetTick() - start < 100U) { - if ((lan8700_read(LAN8700_BCR) & LAN8700_BCR_RESET) == 0U) { - break; + uint32_t start = HAL_GetTick(); + while (HAL_GetTick() - start < 100U) { + if ((lan8700_read(LAN8700_BCR) & LAN8700_BCR_RESET) == 0U) { + break; + } } - } } static uint8_t lan8700_scan_address(void) { - uint32_t id1 = 0U; - uint32_t id2 = 0U; - for (uint32_t addr = 0; addr <= 31; ++addr) { - if (HAL_ETH_ReadPHYRegister(&heth, addr, LAN8700_PHYID1, &id1) != HAL_OK) { - continue; - } - if (HAL_ETH_ReadPHYRegister(&heth, addr, LAN8700_PHYID2, &id2) != HAL_OK) { - continue; - } - if ((id1 != 0x0000U) && (id1 != 0xFFFFU)) { - return (uint8_t)addr; + uint32_t id1 = 0U; + uint32_t id2 = 0U; + for (uint32_t addr = 0; addr <= 31; ++addr) { + if (HAL_ETH_ReadPHYRegister(&heth, addr, LAN8700_PHYID1, &id1) != HAL_OK) { + continue; + } + if (HAL_ETH_ReadPHYRegister(&heth, addr, LAN8700_PHYID2, &id2) != HAL_OK) { + continue; + } + if ((id1 != 0x0000U) && (id1 != 0xFFFFU)) { + return (uint8_t)addr; + } } - } - return 0xFF; + return 0xFF; } static void lan8700_init(void) { - HAL_ETH_SetMDIOClockRange(&heth); - - /* Verify MDIO responds at configured address, otherwise auto-scan */ - uint32_t id1 = 0U; - if (HAL_ETH_ReadPHYRegister(&heth, lan8700_active_addr, LAN8700_PHYID1, - &id1) != HAL_OK || - (id1 == 0x0000U) || (id1 == 0xFFFFU)) { - uint8_t addr = lan8700_scan_address(); - if (addr == 0xFF) { - return; + HAL_ETH_SetMDIOClockRange(&heth); + + /* Verify MDIO responds at configured address, otherwise auto-scan */ + uint32_t id1 = 0U; + if (HAL_ETH_ReadPHYRegister(&heth, lan8700_active_addr, LAN8700_PHYID1, &id1) != HAL_OK || + (id1 == 0x0000U) || (id1 == 0xFFFFU)) { + uint8_t addr = lan8700_scan_address(); + if (addr == 0xFF) { + return; + } + lan8700_active_addr = addr; } - lan8700_active_addr = addr; - } - uint32_t bcr = lan8700_read(LAN8700_BCR); - if (bcr & LAN8700_BCR_POWER_DOWN) { - bcr &= ~LAN8700_BCR_POWER_DOWN; - lan8700_write(LAN8700_BCR, bcr); - } + uint32_t bcr = lan8700_read(LAN8700_BCR); + if (bcr & LAN8700_BCR_POWER_DOWN) { + bcr &= ~LAN8700_BCR_POWER_DOWN; + lan8700_write(LAN8700_BCR, bcr); + } - /* Soft reset to load strap defaults */ - lan8700_write(LAN8700_BCR, LAN8700_BCR_RESET); - lan8700_wait_reset_clear(); + /* Soft reset to load strap defaults */ + lan8700_write(LAN8700_BCR, LAN8700_BCR_RESET); + lan8700_wait_reset_clear(); - /* Start auto-negotiation (or force 100FD for debug) */ - bcr = lan8700_read(LAN8700_BCR); - bcr |= LAN8700_BCR_AUTONEG | LAN8700_BCR_RESTART_AUTONEG; - lan8700_write(LAN8700_BCR, bcr); + /* Start auto-negotiation (or force 100FD for debug) */ + bcr = lan8700_read(LAN8700_BCR); + bcr |= LAN8700_BCR_AUTONEG | LAN8700_BCR_RESTART_AUTONEG; + lan8700_write(LAN8700_BCR, bcr); } static phy_link_state_t lan8700_get_link_state(void) { - uint32_t bsr = lan8700_read(LAN8700_BSR); - /* BSR is latch-low: read twice to clear latched bits */ - bsr = lan8700_read(LAN8700_BSR); + uint32_t bsr = lan8700_read(LAN8700_BSR); + /* BSR is latch-low: read twice to clear latched bits */ + bsr = lan8700_read(LAN8700_BSR); - if ((bsr & LAN8700_BSR_LINK_STATUS) == 0U) { - return PHY_LINK_DOWN; - } - - uint32_t bcr = lan8700_read(LAN8700_BCR); - if ((bcr & LAN8700_BCR_AUTONEG) == 0U) { - const uint32_t speed_100 = (bcr & LAN8700_BCR_SPEED_SELECT); - const uint32_t full = (bcr & LAN8700_BCR_DUPLEX_MODE); - if (speed_100) { - return full ? PHY_100_FULL : PHY_100_HALF; + if ((bsr & LAN8700_BSR_LINK_STATUS) == 0U) { + return PHY_LINK_DOWN; + } + + uint32_t bcr = lan8700_read(LAN8700_BCR); + if ((bcr & LAN8700_BCR_AUTONEG) == 0U) { + const uint32_t speed_100 = (bcr & LAN8700_BCR_SPEED_SELECT); + const uint32_t full = (bcr & LAN8700_BCR_DUPLEX_MODE); + if (speed_100) { + return full ? PHY_100_FULL : PHY_100_HALF; + } + return full ? PHY_10_FULL : PHY_10_HALF; + } + + if ((bsr & LAN8700_BSR_AUTONEG_COMPLETE) == 0U) { + return PHY_LINK_DOWN; + } + + uint32_t anar = lan8700_read(LAN8700_ANAR); + uint32_t anlpar = lan8700_read(LAN8700_ANLPAR); + uint32_t common = anar & anlpar; + + if (common & LAN8700_ANAR_100FD) { + return PHY_100_FULL; + } + if (common & LAN8700_ANAR_100HD) { + return PHY_100_HALF; + } + if (common & LAN8700_ANAR_10FD) { + return PHY_10_FULL; + } + if (common & LAN8700_ANAR_10HD) { + return PHY_10_HALF; } - return full ? PHY_10_FULL : PHY_10_HALF; - } - if ((bsr & LAN8700_BSR_AUTONEG_COMPLETE) == 0U) { return PHY_LINK_DOWN; - } - - uint32_t anar = lan8700_read(LAN8700_ANAR); - uint32_t anlpar = lan8700_read(LAN8700_ANLPAR); - uint32_t common = anar & anlpar; - - if (common & LAN8700_ANAR_100FD) { - return PHY_100_FULL; - } - if (common & LAN8700_ANAR_100HD) { - return PHY_100_HALF; - } - if (common & LAN8700_ANAR_10FD) { - return PHY_10_FULL; - } - if (common & LAN8700_ANAR_10HD) { - return PHY_10_HALF; - } - - return PHY_LINK_DOWN; } const phy_driver_t phy_lan8700 = { diff --git a/Src/HALAL/Services/Communication/Ethernet/PHY/lan8742.c b/Src/HALAL/Services/Communication/Ethernet/PHY/lan8742.c index 40a6f0458..71375e888 100644 --- a/Src/HALAL/Services/Communication/Ethernet/PHY/lan8742.c +++ b/Src/HALAL/Services/Communication/Ethernet/PHY/lan8742.c @@ -10,10 +10,8 @@ extern ETH_HandleTypeDef heth; /* Bus IO provided by ethernetif.c */ extern int32_t ETH_PHY_IO_Init(void); extern int32_t ETH_PHY_IO_DeInit(void); -extern int32_t ETH_PHY_IO_WriteReg(uint32_t DevAddr, uint32_t RegAddr, - uint32_t RegVal); -extern int32_t ETH_PHY_IO_ReadReg(uint32_t DevAddr, uint32_t RegAddr, - uint32_t *pRegVal); +extern int32_t ETH_PHY_IO_WriteReg(uint32_t DevAddr, uint32_t RegAddr, uint32_t RegVal); +extern int32_t ETH_PHY_IO_ReadReg(uint32_t DevAddr, uint32_t RegAddr, uint32_t* pRegVal); extern int32_t ETH_PHY_IO_GetTick(void); static lan8742_Object_t lan; @@ -27,70 +25,70 @@ static lan8742_IOCtx_t ctx = { }; static void lan_init(void) { - uint32_t id1 = 0, id2 = 0; - - HAL_ETH_SetMDIOClockRange(&heth); - LAN8742_RegisterBusIO(&lan, &ctx); - - if (lan.IO.Init) { - lan.IO.Init(); - } - - lan.Is_Initialized = 0; - lan.DevAddr = LAN8742_PHY_ADDRESS; - - /* Prefer configured address; fall back to MDIO scan if not responsive */ - if (lan.IO.ReadReg(lan.DevAddr, LAN8742_PHYI1R, &id1) < 0 || - lan.IO.ReadReg(lan.DevAddr, LAN8742_PHYI2R, &id2) < 0 || - (id1 == 0x0000U) || (id1 == 0xFFFFU)) { - for (uint32_t addr = 0; addr <= 31; ++addr) { - if (lan.IO.ReadReg(addr, LAN8742_PHYI1R, &id1) < 0) { - continue; - } - if (lan.IO.ReadReg(addr, LAN8742_PHYI2R, &id2) < 0) { - continue; - } - if ((id1 != 0x0000U) && (id1 != 0xFFFFU)) { - lan.DevAddr = addr; - break; - } + uint32_t id1 = 0, id2 = 0; + + HAL_ETH_SetMDIOClockRange(&heth); + LAN8742_RegisterBusIO(&lan, &ctx); + + if (lan.IO.Init) { + lan.IO.Init(); } - } - if ((id1 != 0x0000U) && (id1 != 0xFFFFU)) { - lan.Is_Initialized = 1; - } + lan.Is_Initialized = 0; + lan.DevAddr = LAN8742_PHY_ADDRESS; + + /* Prefer configured address; fall back to MDIO scan if not responsive */ + if (lan.IO.ReadReg(lan.DevAddr, LAN8742_PHYI1R, &id1) < 0 || + lan.IO.ReadReg(lan.DevAddr, LAN8742_PHYI2R, &id2) < 0 || (id1 == 0x0000U) || + (id1 == 0xFFFFU)) { + for (uint32_t addr = 0; addr <= 31; ++addr) { + if (lan.IO.ReadReg(addr, LAN8742_PHYI1R, &id1) < 0) { + continue; + } + if (lan.IO.ReadReg(addr, LAN8742_PHYI2R, &id2) < 0) { + continue; + } + if ((id1 != 0x0000U) && (id1 != 0xFFFFU)) { + lan.DevAddr = addr; + break; + } + } + } - if (lan.Is_Initialized) { - LAN8742_DisablePowerDownMode(&lan); - LAN8742_StartAutoNego(&lan); + if ((id1 != 0x0000U) && (id1 != 0xFFFFU)) { + lan.Is_Initialized = 1; + } - uint32_t bcr = 0; - if (lan.IO.ReadReg(lan.DevAddr, LAN8742_BCR, &bcr) >= 0) { - bcr |= (LAN8742_BCR_AUTONEGO_EN | LAN8742_BCR_RESTART_AUTONEGO); - lan.IO.WriteReg(lan.DevAddr, LAN8742_BCR, bcr); + if (lan.Is_Initialized) { + LAN8742_DisablePowerDownMode(&lan); + LAN8742_StartAutoNego(&lan); + + uint32_t bcr = 0; + if (lan.IO.ReadReg(lan.DevAddr, LAN8742_BCR, &bcr) >= 0) { + bcr |= (LAN8742_BCR_AUTONEGO_EN | LAN8742_BCR_RESTART_AUTONEGO); + lan.IO.WriteReg(lan.DevAddr, LAN8742_BCR, bcr); + } } - } } static phy_link_state_t lan_get_link_state(void) { - if (!lan.Is_Initialized) { - return PHY_LINK_DOWN; - } - int32_t st = LAN8742_GetLinkState(&lan); - - switch (st) { - case LAN8742_STATUS_100MBITS_FULLDUPLEX: - return PHY_100_FULL; - case LAN8742_STATUS_100MBITS_HALFDUPLEX: - return PHY_100_HALF; - case LAN8742_STATUS_10MBITS_FULLDUPLEX: - return PHY_10_FULL; - case LAN8742_STATUS_10MBITS_HALFDUPLEX: - return PHY_10_HALF; - default: - return PHY_LINK_DOWN; - } + if (!lan.Is_Initialized) { + return PHY_LINK_DOWN; + } + int32_t st = LAN8742_GetLinkState(&lan); + + switch (st) { + case LAN8742_STATUS_100MBITS_FULLDUPLEX: + return PHY_100_FULL; + case LAN8742_STATUS_100MBITS_HALFDUPLEX: + return PHY_100_HALF; + case LAN8742_STATUS_10MBITS_FULLDUPLEX: + return PHY_10_FULL; + case LAN8742_STATUS_10MBITS_HALFDUPLEX: + return PHY_10_HALF; + default: + return PHY_LINK_DOWN; + } } const phy_driver_t phy_lan8742 = { diff --git a/Src/HALAL/Services/Communication/Ethernet/PHY/phy_select.c b/Src/HALAL/Services/Communication/Ethernet/PHY/phy_select.c index 435cf1307..7a93c26c6 100644 --- a/Src/HALAL/Services/Communication/Ethernet/PHY/phy_select.c +++ b/Src/HALAL/Services/Communication/Ethernet/PHY/phy_select.c @@ -2,15 +2,15 @@ #if defined(USE_PHY_KSZ8041) extern const phy_driver_t phy_ksz8041; -const phy_driver_t *phy_driver = &phy_ksz8041; +const phy_driver_t* phy_driver = &phy_ksz8041; #elif defined(USE_PHY_LAN8742) extern const phy_driver_t phy_lan8742; -const phy_driver_t *phy_driver = &phy_lan8742; +const phy_driver_t* phy_driver = &phy_lan8742; #elif defined(USE_PHY_LAN8700) extern const phy_driver_t phy_lan8700; -const phy_driver_t *phy_driver = &phy_lan8700; +const phy_driver_t* phy_driver = &phy_lan8700; #else #error "No PHY selected" diff --git a/Src/HALAL/Services/Communication/FDCAN/FDCAN.cpp b/Src/HALAL/Services/Communication/FDCAN/FDCAN.cpp index bdafa32be..d351bc7fe 100644 --- a/Src/HALAL/Services/Communication/FDCAN/FDCAN.cpp +++ b/Src/HALAL/Services/Communication/FDCAN/FDCAN.cpp @@ -13,108 +13,129 @@ uint16_t FDCAN::id_counter = 0; unordered_map FDCAN::registered_fdcan = {}; -unordered_map FDCAN::dlc_to_len = {{DLC::BYTES_0, 0}, {DLC::BYTES_1, 1}, {DLC::BYTES_2, 2}, {DLC::BYTES_3, 3}, {DLC::BYTES_4, 4}, - {DLC::BYTES_5, 5}, {DLC::BYTES_6, 6}, {DLC::BYTES_7, 7}, {DLC::BYTES_8, 8}, {DLC::BYTES_12, 12}, - {DLC::BYTES_16, 16}, {DLC::BYTES_20, 20}, {DLC::BYTES_24, 24}, {DLC::BYTES_32, 32}, {DLC::BYTES_48, 48}, - {DLC::BYTES_64, 64} - }; -unordered_map FDCAN::handle_to_id{}; -unordered_map FDCAN::instance_to_id{}; -FDCAN::Packet packet{.rx_data = array{},.data_length = FDCAN::BYTES_64}; - - -void FDCAN::start(){ - for( std::pair inst: FDCAN::registered_fdcan){ - uint8_t id = inst.first; - FDCAN::Instance* instance = inst.second; - FDCAN::init(instance); - - instance->rx_queue = queue(); - instance->tx_data = vector(); - - if(HAL_FDCAN_Start(instance->hfdcan) != HAL_OK){ - ErrorHandler("Error during FDCAN %d initialization.", instance->fdcan_number); - } - - if(HAL_FDCAN_ActivateNotification(instance->hfdcan, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0) != HAL_OK){ - ErrorHandler("Error activating FDCAN %d notifications.", instance->fdcan_number); - } - - instance->start = true; - - FDCAN::registered_fdcan[id] = instance; - FDCAN::instance_to_id[instance] = id; - } +unordered_map FDCAN::dlc_to_len = { + {DLC::BYTES_0, 0}, + {DLC::BYTES_1, 1}, + {DLC::BYTES_2, 2}, + {DLC::BYTES_3, 3}, + {DLC::BYTES_4, 4}, + {DLC::BYTES_5, 5}, + {DLC::BYTES_6, 6}, + {DLC::BYTES_7, 7}, + {DLC::BYTES_8, 8}, + {DLC::BYTES_12, 12}, + {DLC::BYTES_16, 16}, + {DLC::BYTES_20, 20}, + {DLC::BYTES_24, 24}, + {DLC::BYTES_32, 32}, + {DLC::BYTES_48, 48}, + {DLC::BYTES_64, 64} +}; +unordered_map FDCAN::handle_to_id{}; +unordered_map FDCAN::instance_to_id{}; +FDCAN::Packet packet{.rx_data = array{}, .data_length = FDCAN::BYTES_64}; + +void FDCAN::start() { + for (std::pair inst : FDCAN::registered_fdcan) { + uint8_t id = inst.first; + FDCAN::Instance* instance = inst.second; + FDCAN::init(instance); + + instance->rx_queue = queue(); + instance->tx_data = vector(); + + if (HAL_FDCAN_Start(instance->hfdcan) != HAL_OK) { + ErrorHandler("Error during FDCAN %d initialization.", instance->fdcan_number); + } + + if (HAL_FDCAN_ActivateNotification(instance->hfdcan, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0) != + HAL_OK) { + ErrorHandler("Error activating FDCAN %d notifications.", instance->fdcan_number); + } + + instance->start = true; + + FDCAN::registered_fdcan[id] = instance; + FDCAN::instance_to_id[instance] = id; + } } -bool FDCAN::transmit(uint8_t id, uint32_t message_id, const char* data, FDCAN::DLC dlc){ - if (not FDCAN::registered_fdcan.contains(id)) { - ErrorHandler("There is no registered FDCAN with id: %d.", id); - return false; - } +bool FDCAN::transmit(uint8_t id, uint32_t message_id, const char* data, FDCAN::DLC dlc) { + if (not FDCAN::registered_fdcan.contains(id)) { + ErrorHandler("There is no registered FDCAN with id: %d.", id); + return false; + } - FDCAN::Instance* instance = registered_fdcan[id]; + FDCAN::Instance* instance = registered_fdcan[id]; - if (not instance->start) { - ErrorHandler("The FDCAN %d is not initialized.", instance->fdcan_number); - return false; - } + if (not instance->start) { + ErrorHandler("The FDCAN %d is not initialized.", instance->fdcan_number); + return false; + } - instance->tx_header.Identifier = message_id; + instance->tx_header.Identifier = message_id; - if (dlc != FDCAN::DLC::DEFAULT) { - instance->tx_header.DataLength = dlc; - } + if (dlc != FDCAN::DLC::DEFAULT) { + instance->tx_header.DataLength = dlc; + } - HAL_StatusTypeDef error = HAL_FDCAN_AddMessageToTxFifoQ(instance->hfdcan, &instance->tx_header, (uint8_t*)data); + HAL_StatusTypeDef error = + HAL_FDCAN_AddMessageToTxFifoQ(instance->hfdcan, &instance->tx_header, (uint8_t*)data); - if (error != HAL_OK) { - ErrorHandler("Error sending message with id: 0x%x by FDCAN %d", message_id, instance->fdcan_number); - return false; - } + if (error != HAL_OK) { + ErrorHandler( + "Error sending message with id: 0x%x by FDCAN %d", + message_id, + instance->fdcan_number + ); + return false; + } - return true; + return true; } -void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs){} - -bool FDCAN::read(uint8_t id, FDCAN::Packet* data){ - if (not FDCAN::registered_fdcan.contains(id)) { - ErrorHandler("There is no FDCAN registered with id: %d.", id); - return false; - } - - if(!FDCAN::received_test(id)) { - return false; - } - FDCAN_RxHeaderTypeDef header_buffer = FDCAN_RxHeaderTypeDef(); - HAL_FDCAN_GetRxMessage(FDCAN::registered_fdcan.at(id)->hfdcan, FDCAN::registered_fdcan.at(id)->rx_location, &header_buffer, data->rx_data.data()); - if(data->identifier == FDCAN::ID::FAULT_ID){ - ErrorHandler("FAULT PROPAGATED via CAN"); - } - data->identifier = header_buffer.Identifier; - data->data_length = static_cast(header_buffer.DataLength); - - return true; +void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef* hfdcan, uint32_t RxFifo0ITs) {} + +bool FDCAN::read(uint8_t id, FDCAN::Packet* data) { + if (not FDCAN::registered_fdcan.contains(id)) { + ErrorHandler("There is no FDCAN registered with id: %d.", id); + return false; + } + + if (!FDCAN::received_test(id)) { + return false; + } + FDCAN_RxHeaderTypeDef header_buffer = FDCAN_RxHeaderTypeDef(); + HAL_FDCAN_GetRxMessage( + FDCAN::registered_fdcan.at(id)->hfdcan, + FDCAN::registered_fdcan.at(id)->rx_location, + &header_buffer, + data->rx_data.data() + ); + if (data->identifier == FDCAN::ID::FAULT_ID) { + ErrorHandler("FAULT PROPAGATED via CAN"); + } + data->identifier = header_buffer.Identifier; + data->data_length = static_cast(header_buffer.DataLength); + + return true; } -bool FDCAN::received_test(uint8_t id){ - if (not FDCAN::registered_fdcan.contains(id)) { - ErrorHandler("FDCAN with id %u not registered", id); - return false; - } +bool FDCAN::received_test(uint8_t id) { + if (not FDCAN::registered_fdcan.contains(id)) { + ErrorHandler("FDCAN with id %u not registered", id); + return false; + } - return !((FDCAN::registered_fdcan.at(id)->hfdcan->Instance->RXF0S & FDCAN_RXF0S_F0FL) == 0U); + return !((FDCAN::registered_fdcan.at(id)->hfdcan->Instance->RXF0S & FDCAN_RXF0S_F0FL) == 0U); } -void FDCAN::init(FDCAN::Instance* fdcan){ - FDCAN_HandleTypeDef* handle = fdcan->hfdcan; - handle_to_id[handle] = instance_to_id[fdcan]; - - if (HAL_FDCAN_Init(handle) != HAL_OK) - { - ErrorHandler("Error during FDCAN %d init.", fdcan->fdcan_number); - } +void FDCAN::init(FDCAN::Instance* fdcan) { + FDCAN_HandleTypeDef* handle = fdcan->hfdcan; + handle_to_id[handle] = instance_to_id[fdcan]; + if (HAL_FDCAN_Init(handle) != HAL_OK) { + ErrorHandler("Error during FDCAN %d init.", fdcan->fdcan_number); + } } #endif diff --git a/Src/HALAL/Services/Communication/I2C/I2C.cpp b/Src/HALAL/Services/Communication/I2C/I2C.cpp index 1af2a9784..6b52a919a 100644 --- a/Src/HALAL/Services/Communication/I2C/I2C.cpp +++ b/Src/HALAL/Services/Communication/I2C/I2C.cpp @@ -12,264 +12,285 @@ unordered_map I2C::active_i2c; uint16_t I2C::id_counter = 0; -uint8_t I2C::inscribe(I2C::Peripheral &i2c, uint8_t address) { - if (!I2C::available_i2cs.contains(i2c)) { - ErrorHandler("The I2C %d is not available on the runes files", (uint16_t)i2c); - return 0; - } +uint8_t I2C::inscribe(I2C::Peripheral& i2c, uint8_t address) { + if (!I2C::available_i2cs.contains(i2c)) { + ErrorHandler("The I2C %d is not available on the runes files", (uint16_t)i2c); + return 0; + } - I2C::Instance *i2c_instance = I2C::available_i2cs[i2c]; + I2C::Instance* i2c_instance = I2C::available_i2cs[i2c]; - Pin::inscribe(i2c_instance->SCL, ALTERNATIVE); - Pin::inscribe(i2c_instance->SDA, ALTERNATIVE); + Pin::inscribe(i2c_instance->SCL, ALTERNATIVE); + Pin::inscribe(i2c_instance->SDA, ALTERNATIVE); - DMA::inscribe_stream(i2c_instance->RX_DMA); - DMA::inscribe_stream(i2c_instance->TX_DMA); + DMA::inscribe_stream(i2c_instance->RX_DMA); + DMA::inscribe_stream(i2c_instance->TX_DMA); - uint8_t id = I2C::id_counter++; + uint8_t id = I2C::id_counter++; - I2C::active_i2c[id] = i2c_instance; - i2c_instance->address = address; + I2C::active_i2c[id] = i2c_instance; + i2c_instance->address = address; - return id; + return id; } -uint8_t I2C::inscribe(I2C::Peripheral &i2c) { - return I2C::inscribe(i2c, 0); -} +uint8_t I2C::inscribe(I2C::Peripheral& i2c) { return I2C::inscribe(i2c, 0); } void I2C::start() { - for (auto iter : I2C::active_i2c) { - I2C::init(iter.second); - } + for (auto iter : I2C::active_i2c) { + I2C::init(iter.second); + } } -bool I2C::transmit_next_packet(uint8_t id, I2CPacket &packet) { - if (!I2C::active_i2c.contains(id)) { - ErrorHandler("I2C id not found on transmit packet \n\r"); - return false; - } - - I2C::Instance *i2c = I2C::active_i2c[id]; - - if (i2c->hi2c->State == HAL_I2C_STATE_BUSY_TX) { - ErrorHandler("I2C Transmit buffer busy!\n\r"); - return false; - } - - if (hdma_i2c2_tx.State != 0x01U) { - } - - if (HAL_I2C_Master_Transmit_DMA(i2c->hi2c, packet.get_id()<<1, - packet.get_data(), packet.get_size()) != HAL_OK) { - ErrorHandler("I2C Error during memory read DMA!\n\r"); - return false; - } - return true; +bool I2C::transmit_next_packet(uint8_t id, I2CPacket& packet) { + if (!I2C::active_i2c.contains(id)) { + ErrorHandler("I2C id not found on transmit packet \n\r"); + return false; + } + + I2C::Instance* i2c = I2C::active_i2c[id]; + + if (i2c->hi2c->State == HAL_I2C_STATE_BUSY_TX) { + ErrorHandler("I2C Transmit buffer busy!\n\r"); + return false; + } + + if (hdma_i2c2_tx.State != 0x01U) { + } + + if (HAL_I2C_Master_Transmit_DMA( + i2c->hi2c, + packet.get_id() << 1, + packet.get_data(), + packet.get_size() + ) != HAL_OK) { + ErrorHandler("I2C Error during memory read DMA!\n\r"); + return false; + } + return true; } -bool I2C::transmit_next_packet_polling(uint8_t id, I2CPacket &packet) { - if (!I2C::active_i2c.contains(id)) { - ErrorHandler("I2C id not found on transmit packet \n\r"); - return false; - } - - I2C::Instance *i2c = I2C::active_i2c[id]; - - if (i2c->hi2c->State == HAL_I2C_STATE_BUSY_TX) { - ErrorHandler("I2C Transmit buffer busy!\n\r"); - return false; - } - - if (HAL_I2C_Master_Transmit(i2c->hi2c, packet.get_id()<<1, packet.get_data(), - packet.get_size(), 50) != HAL_OK) { - //ErrorHandler("Error during I2C transmission \n\r"); - return false; - } - return true; +bool I2C::transmit_next_packet_polling(uint8_t id, I2CPacket& packet) { + if (!I2C::active_i2c.contains(id)) { + ErrorHandler("I2C id not found on transmit packet \n\r"); + return false; + } + + I2C::Instance* i2c = I2C::active_i2c[id]; + + if (i2c->hi2c->State == HAL_I2C_STATE_BUSY_TX) { + ErrorHandler("I2C Transmit buffer busy!\n\r"); + return false; + } + + if (HAL_I2C_Master_Transmit( + i2c->hi2c, + packet.get_id() << 1, + packet.get_data(), + packet.get_size(), + 50 + ) != HAL_OK) { + // ErrorHandler("Error during I2C transmission \n\r"); + return false; + } + return true; } -bool I2C::receive_next_packet(uint8_t id, I2CPacket &packet) { - if (!I2C::active_i2c.contains(id)) { - ErrorHandler("I2C id not found on receive packet \n\r"); - return false; - } - - I2C::Instance *i2c = I2C::active_i2c[id]; - - if (i2c->hi2c->State == HAL_I2C_STATE_BUSY_RX) { - ErrorHandler("I2C Receive buffer busy!\n\r"); - return false; - } - - *packet.get_data() = 0; - - if (HAL_I2C_Master_Receive_DMA(i2c->hi2c, packet.get_id()<<1, - packet.get_data(), packet.get_size()) != HAL_OK) { - ErrorHandler("I2C Error during memory write DMA!\n\r"); - return false; - } - - i2c->is_receive_ready = false; - return true; -} - -bool I2C::receive_next_packet_polling(uint8_t id, I2CPacket &packet) { - if (!I2C::active_i2c.contains(id)) { - ErrorHandler("I2C id not found on receive packet \n\r"); - return false; - } - - I2C::Instance *i2c = I2C::active_i2c[id]; - - if (i2c->hi2c->State == HAL_I2C_STATE_BUSY_RX) { - ErrorHandler("I2C Receive buffer busy!\n\r"); - return false; - } - - *packet.get_data() = 0; - - if (HAL_I2C_Master_Receive(i2c->hi2c, packet.get_id()<<1, packet.get_data(), - packet.get_size(), 50) != HAL_OK) { - //ErrorHandler("I2C Error during receive!\n\r"); - return false; - } - - i2c->is_receive_ready = false; - return true; +bool I2C::receive_next_packet(uint8_t id, I2CPacket& packet) { + if (!I2C::active_i2c.contains(id)) { + ErrorHandler("I2C id not found on receive packet \n\r"); + return false; + } + + I2C::Instance* i2c = I2C::active_i2c[id]; + + if (i2c->hi2c->State == HAL_I2C_STATE_BUSY_RX) { + ErrorHandler("I2C Receive buffer busy!\n\r"); + return false; + } + + *packet.get_data() = 0; + + if (HAL_I2C_Master_Receive_DMA( + i2c->hi2c, + packet.get_id() << 1, + packet.get_data(), + packet.get_size() + ) != HAL_OK) { + ErrorHandler("I2C Error during memory write DMA!\n\r"); + return false; + } + + i2c->is_receive_ready = false; + return true; } -void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef *hi2c) { - auto result = find_if(I2C::active_i2c.begin(), I2C::active_i2c.end(), - [&](auto i2c) { - return (i2c.second->hi2c == hi2c); - }); - - if (result != I2C::active_i2c.end()) { - (*result).second->is_receive_ready = true; - } else { - //TODO: Warning: Data receive form an unknown SPI - } +bool I2C::receive_next_packet_polling(uint8_t id, I2CPacket& packet) { + if (!I2C::active_i2c.contains(id)) { + ErrorHandler("I2C id not found on receive packet \n\r"); + return false; + } + + I2C::Instance* i2c = I2C::active_i2c[id]; + + if (i2c->hi2c->State == HAL_I2C_STATE_BUSY_RX) { + ErrorHandler("I2C Receive buffer busy!\n\r"); + return false; + } + + *packet.get_data() = 0; + + if (HAL_I2C_Master_Receive( + i2c->hi2c, + packet.get_id() << 1, + packet.get_data(), + packet.get_size(), + 50 + ) != HAL_OK) { + // ErrorHandler("I2C Error during receive!\n\r"); + return false; + } + + i2c->is_receive_ready = false; + return true; } -bool I2C::read_from(uint8_t id, I2CPacket &packet, uint16_t mem_addr, - uint16_t mem_size) { - if (!I2C::active_i2c.contains(id)) { - ErrorHandler("I2C id not found on read \n\r"); - return false; - } - - I2C::Instance *i2c = I2C::active_i2c[id]; - - *packet.get_data() = 0; - - if (i2c->hi2c->State == HAL_I2C_STATE_BUSY_TX) { - ErrorHandler("I2C Transmit buffer busy!\n\r"); - return false; - } +void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef* hi2c) { + auto result = find_if(I2C::active_i2c.begin(), I2C::active_i2c.end(), [&](auto i2c) { + return (i2c.second->hi2c == hi2c); + }); - if (HAL_I2C_Mem_Read_DMA(i2c->hi2c, packet.get_id()<<1, mem_addr, 1, - packet.get_data(), packet.get_size())) { - ErrorHandler("I2C Error during memory read DMA!\n\r"); - return false; - } - - i2c->is_receive_ready = false; - return true; + if (result != I2C::active_i2c.end()) { + (*result).second->is_receive_ready = true; + } else { + // TODO: Warning: Data receive form an unknown SPI + } } -bool I2C::write_to(uint8_t id, I2CPacket &packet, uint16_t mem_addr, - uint16_t mem_size) { - if (!I2C::active_i2c.contains(id)) { - ErrorHandler("I2C id not found on write \n\r"); - return false; - } - - I2C::Instance *i2c = I2C::active_i2c[id]; - - if (i2c->hi2c->State == HAL_I2C_STATE_BUSY_RX) { - ErrorHandler("I2C Receive buffer busy!\n\r"); - return false; - } - - if (HAL_I2C_Mem_Write_DMA(i2c->hi2c, packet.get_id()<<1, mem_addr, mem_size, - packet.get_data(), packet.get_size())) { - ErrorHandler("I2C Error during memory write DMA!\n\r"); - return false; - } - - return true; +bool I2C::read_from(uint8_t id, I2CPacket& packet, uint16_t mem_addr, uint16_t mem_size) { + if (!I2C::active_i2c.contains(id)) { + ErrorHandler("I2C id not found on read \n\r"); + return false; + } + + I2C::Instance* i2c = I2C::active_i2c[id]; + + *packet.get_data() = 0; + + if (i2c->hi2c->State == HAL_I2C_STATE_BUSY_TX) { + ErrorHandler("I2C Transmit buffer busy!\n\r"); + return false; + } + + if (HAL_I2C_Mem_Read_DMA( + i2c->hi2c, + packet.get_id() << 1, + mem_addr, + 1, + packet.get_data(), + packet.get_size() + )) { + ErrorHandler("I2C Error during memory read DMA!\n\r"); + return false; + } + + i2c->is_receive_ready = false; + return true; +} +bool I2C::write_to(uint8_t id, I2CPacket& packet, uint16_t mem_addr, uint16_t mem_size) { + if (!I2C::active_i2c.contains(id)) { + ErrorHandler("I2C id not found on write \n\r"); + return false; + } + + I2C::Instance* i2c = I2C::active_i2c[id]; + + if (i2c->hi2c->State == HAL_I2C_STATE_BUSY_RX) { + ErrorHandler("I2C Receive buffer busy!\n\r"); + return false; + } + + if (HAL_I2C_Mem_Write_DMA( + i2c->hi2c, + packet.get_id() << 1, + mem_addr, + mem_size, + packet.get_data(), + packet.get_size() + )) { + ErrorHandler("I2C Error during memory write DMA!\n\r"); + return false; + } + + return true; } bool I2C::has_next_packet(uint8_t id) { - if (!I2C::active_i2c.contains(id)) { - ErrorHandler("I2C id not found on next packet check \n\r"); - return false; - } + if (!I2C::active_i2c.contains(id)) { + ErrorHandler("I2C id not found on next packet check \n\r"); + return false; + } - I2C::Instance *i2c = I2C::active_i2c[id]; + I2C::Instance* i2c = I2C::active_i2c[id]; - return i2c->is_receive_ready; + return i2c->is_receive_ready; } bool I2C::is_busy(uint8_t id) { - if (!I2C::active_i2c.contains(id)) { - ErrorHandler("I2C id not found on is busy check \n\r"); - return false; - } - I2C::Instance *i2c = I2C::active_i2c[id]; + if (!I2C::active_i2c.contains(id)) { + ErrorHandler("I2C id not found on is busy check \n\r"); + return false; + } + I2C::Instance* i2c = I2C::active_i2c[id]; - if (i2c->hi2c->State == HAL_I2C_STATE_BUSY_TX) { - return true; - } + if (i2c->hi2c->State == HAL_I2C_STATE_BUSY_TX) { + return true; + } - return false; + return false; } void I2C::reset(uint8_t id) { - I2C::Instance *i2c = I2C::active_i2c[id]; - HAL_I2C_DeInit(i2c->hi2c); - HAL_I2C_Init(i2c->hi2c); + I2C::Instance* i2c = I2C::active_i2c[id]; + HAL_I2C_DeInit(i2c->hi2c); + HAL_I2C_Init(i2c->hi2c); } -void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hspi) { - //TODO: Fault, I2C error - return; +void HAL_I2C_ErrorCallback(I2C_HandleTypeDef* hspi) { + // TODO: Fault, I2C error + return; } -void I2C::init(I2C::Instance *i2c) { - if (i2c->is_initialized) { - return; - } - if (!available_speed_frequencies.contains(i2c->speed_frequency_kHz)) { - ErrorHandler( - "Error initializing, the frequency of the I2C is not available"); - return; - } - i2c->hi2c->Instance = i2c->instance; - i2c->hi2c->Init.Timing = - available_speed_frequencies[i2c->speed_frequency_kHz]; - i2c->hi2c->Init.OwnAddress1 = (i2c->address) << 1; - i2c->hi2c->Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; - i2c->hi2c->Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; - i2c->hi2c->Init.OwnAddress2 = 0; - i2c->hi2c->Init.OwnAddress2Masks = I2C_OA2_NOMASK; - i2c->hi2c->Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; - i2c->hi2c->Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; - - if (HAL_I2C_Init(i2c->hi2c) != HAL_OK) { - ErrorHandler("Error configurating I2C"); - } - - if (HAL_I2CEx_ConfigAnalogFilter(i2c->hi2c, I2C_ANALOGFILTER_ENABLE) - != HAL_OK) { - ErrorHandler("Error configurating Analog Filter of the I2C"); - } - - if (HAL_I2CEx_ConfigDigitalFilter(i2c->hi2c, 0) != HAL_OK) { - ErrorHandler("Error configurating Digital Filter of the I2C"); - } - i2c->is_initialized = true; +void I2C::init(I2C::Instance* i2c) { + if (i2c->is_initialized) { + return; + } + if (!available_speed_frequencies.contains(i2c->speed_frequency_kHz)) { + ErrorHandler("Error initializing, the frequency of the I2C is not available"); + return; + } + i2c->hi2c->Instance = i2c->instance; + i2c->hi2c->Init.Timing = available_speed_frequencies[i2c->speed_frequency_kHz]; + i2c->hi2c->Init.OwnAddress1 = (i2c->address) << 1; + i2c->hi2c->Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; + i2c->hi2c->Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; + i2c->hi2c->Init.OwnAddress2 = 0; + i2c->hi2c->Init.OwnAddress2Masks = I2C_OA2_NOMASK; + i2c->hi2c->Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; + i2c->hi2c->Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; + + if (HAL_I2C_Init(i2c->hi2c) != HAL_OK) { + ErrorHandler("Error configurating I2C"); + } + + if (HAL_I2CEx_ConfigAnalogFilter(i2c->hi2c, I2C_ANALOGFILTER_ENABLE) != HAL_OK) { + ErrorHandler("Error configurating Analog Filter of the I2C"); + } + + if (HAL_I2CEx_ConfigDigitalFilter(i2c->hi2c, 0) != HAL_OK) { + ErrorHandler("Error configurating Digital Filter of the I2C"); + } + i2c->is_initialized = true; } #endif \ No newline at end of file diff --git a/Src/HALAL/Services/Communication/SNTP/SNTP.cpp b/Src/HALAL/Services/Communication/SNTP/SNTP.cpp index f5465d18e..132e872fc 100644 --- a/Src/HALAL/Services/Communication/SNTP/SNTP.cpp +++ b/Src/HALAL/Services/Communication/SNTP/SNTP.cpp @@ -8,61 +8,82 @@ #include "HALAL/Services/Communication/SNTP/SNTP.hpp" #define SUBSECONDS_PER_SECOND 32767 -#define TRANSFORMATION_FACTOR (SUBSECONDS_PER_SECOND/999999.0) +#define TRANSFORMATION_FACTOR (SUBSECONDS_PER_SECOND / 999999.0) #define TARGET_IP "192.168.0.9" -void SNTP::sntp_update(uint8_t address_head, uint8_t address_second, uint8_t address_third, uint8_t address_last){ -// sntp_setoperatingmode(SNTP_OPMODE_POLL); -// ip4_addr_t* address; -// IP_ADDR4(address,address_head,address_second,address_third,address_last); -// sntp_setserver(0,address); -// sntp_init(); +void SNTP::sntp_update( + uint8_t address_head, + uint8_t address_second, + uint8_t address_third, + uint8_t address_last +) { + // sntp_setoperatingmode(SNTP_OPMODE_POLL); + // ip4_addr_t* address; + // IP_ADDR4(address,address_head,address_second,address_third,address_last); + // sntp_setserver(0,address); + // sntp_init(); } void SNTP::sntp_update(string ip) { -// sntp_setoperatingmode(SNTP_OPMODE_POLL); -// IPV4 target(ip); -// sntp_setserver(0,&target.address); -// sntp_init(); + // sntp_setoperatingmode(SNTP_OPMODE_POLL); + // IPV4 target(ip); + // sntp_setserver(0,&target.address); + // sntp_init(); } -void SNTP::sntp_update(){ -// sntp_setoperatingmode(SNTP_OPMODE_POLL); -// IPV4 target(TARGET_IP); -// sntp_setserver(0,&target.address); -// sntp_init(); +void SNTP::sntp_update() { + // sntp_setoperatingmode(SNTP_OPMODE_POLL); + // IPV4 target(TARGET_IP); + // sntp_setserver(0,&target.address); + // sntp_init(); } -void set_rtc(uint16_t counter, uint8_t second, uint8_t minute, uint8_t hour, uint8_t day, uint8_t month, uint16_t year){ - Global_RTC::set_rtc_data(counter, second, minute, hour, day, month, year); +void set_rtc( + uint16_t counter, + uint8_t second, + uint8_t minute, + uint8_t hour, + uint8_t day, + uint8_t month, + uint16_t year +) { + Global_RTC::set_rtc_data(counter, second, minute, hour, day, month, year); } -u32_t get_rtc_s(){ - RTCData rtc_time = Global_RTC::get_rtc_timestamp(); - time_t nowtime = 0; - struct tm *nowtm; - nowtm = gmtime(&nowtime); - nowtm->tm_year = rtc_time.year -1900; - nowtm->tm_mon = rtc_time.month - 1; - nowtm->tm_mday = rtc_time.day; - nowtm->tm_hour = rtc_time.hour; - nowtm->tm_min = rtc_time.minute; - nowtm->tm_sec = rtc_time.second; - u32_t sec = mktime(nowtm); - return sec; +u32_t get_rtc_s() { + RTCData rtc_time = Global_RTC::get_rtc_timestamp(); + time_t nowtime = 0; + struct tm* nowtm; + nowtm = gmtime(&nowtime); + nowtm->tm_year = rtc_time.year - 1900; + nowtm->tm_mon = rtc_time.month - 1; + nowtm->tm_mday = rtc_time.day; + nowtm->tm_hour = rtc_time.hour; + nowtm->tm_min = rtc_time.minute; + nowtm->tm_sec = rtc_time.second; + u32_t sec = mktime(nowtm); + return sec; } -u32_t get_rtc_us(){ - RTCData rtc_time = Global_RTC::get_rtc_timestamp(); - return rtc_time.counter/TRANSFORMATION_FACTOR; +u32_t get_rtc_us() { + RTCData rtc_time = Global_RTC::get_rtc_timestamp(); + return rtc_time.counter / TRANSFORMATION_FACTOR; } void set_time(uint32_t sec, uint32_t us) { - struct timeval tv; - tv.tv_sec = sec; - tv.tv_usec = us; - time_t nowtime = sec; - struct tm *nowtm = localtime(&nowtime); - uint32_t subsecond = (uint32_t)(TRANSFORMATION_FACTOR * tv.tv_usec); - set_rtc(subsecond, nowtm->tm_sec, nowtm->tm_min, nowtm->tm_hour, nowtm->tm_mday, 1+nowtm->tm_mon, 1900+(nowtm->tm_year)); + struct timeval tv; + tv.tv_sec = sec; + tv.tv_usec = us; + time_t nowtime = sec; + struct tm* nowtm = localtime(&nowtime); + uint32_t subsecond = (uint32_t)(TRANSFORMATION_FACTOR * tv.tv_usec); + set_rtc( + subsecond, + nowtm->tm_sec, + nowtm->tm_min, + nowtm->tm_hour, + nowtm->tm_mday, + 1 + nowtm->tm_mon, + 1900 + (nowtm->tm_year) + ); } diff --git a/Src/HALAL/Services/Communication/SPI/SPI.cpp b/Src/HALAL/Services/Communication/SPI/SPI.cpp index 975474a83..7ac5c2865 100644 --- a/Src/HALAL/Services/Communication/SPI/SPI.cpp +++ b/Src/HALAL/Services/Communication/SPI/SPI.cpp @@ -22,9 +22,7 @@ uint16_t SPI::id_counter = 0; uint8_t SPI::inscribe(SPI::Peripheral& spi) { if (!SPI::available_spi.contains(spi)) { - ErrorHandler( - " The SPI peripheral %d is already used or does not exists.", - (uint16_t)spi); + ErrorHandler(" The SPI peripheral %d is already used or does not exists.", (uint16_t)spi); return 0; } @@ -37,14 +35,12 @@ uint8_t SPI::inscribe(SPI::Peripheral& spi) { spi_instance->MOSI->alternative_function = AF5; spi_instance->MISO->alternative_function = AF5; - spi_instance->SPIOrderID = - (uint16_t*)MPUManager::allocate_non_cached_memory(32); - spi_instance->available_end = - (uint16_t*)MPUManager::allocate_non_cached_memory(32); - spi_instance->rx_buffer = (uint8_t*)MPUManager::allocate_non_cached_memory( - SPI_MAXIMUM_PACKET_SIZE_BYTES); - spi_instance->tx_buffer = (uint8_t*)MPUManager::allocate_non_cached_memory( - SPI_MAXIMUM_PACKET_SIZE_BYTES); + spi_instance->SPIOrderID = (uint16_t*)MPUManager::allocate_non_cached_memory(32); + spi_instance->available_end = (uint16_t*)MPUManager::allocate_non_cached_memory(32); + spi_instance->rx_buffer = + (uint8_t*)MPUManager::allocate_non_cached_memory(SPI_MAXIMUM_PACKET_SIZE_BYTES); + spi_instance->tx_buffer = + (uint8_t*)MPUManager::allocate_non_cached_memory(SPI_MAXIMUM_PACKET_SIZE_BYTES); Pin::inscribe(*spi_instance->SCK, ALTERNATIVE); Pin::inscribe(*spi_instance->MOSI, ALTERNATIVE); @@ -84,8 +80,11 @@ void SPI::assign_RS(uint8_t id, Pin& RSPin) { void SPI::start() { for (auto iter : SPI::registered_spi) { SPI::init(iter.second); - HAL_GPIO_WritePin(iter.second->SS->port, iter.second->SS->gpio_pin, - (GPIO_PinState)PinState::ON); + HAL_GPIO_WritePin( + iter.second->SS->port, + iter.second->SS->gpio_pin, + (GPIO_PinState)PinState::ON + ); } } @@ -106,23 +105,23 @@ bool SPI::transmit(uint8_t id, span data) { SPI::Instance* spi = SPI::registered_spi[id]; turn_off_chip_select(spi); - HAL_StatusTypeDef errorcode = - HAL_SPI_Transmit(spi->hspi, data.data(), data.size(), 10); + HAL_StatusTypeDef errorcode = HAL_SPI_Transmit(spi->hspi, data.data(), data.size(), 10); turn_on_chip_select(spi); switch (errorcode) { - case HAL_OK: - return true; - break; - case HAL_BUSY: - return false; - break; - default: - ErrorHandler( - "Error while transmiting and receiving with spi DMA. Errorcode " - "%u", - (uint8_t)errorcode); - return false; - break; + case HAL_OK: + return true; + break; + case HAL_BUSY: + return false; + break; + default: + ErrorHandler( + "Error while transmiting and receiving with spi DMA. Errorcode " + "%u", + (uint8_t)errorcode + ); + return false; + break; } } @@ -134,23 +133,23 @@ bool SPI::transmit_DMA(uint8_t id, span data) { SPI::Instance* spi = SPI::registered_spi[id]; - HAL_StatusTypeDef errorcode = - HAL_SPI_Transmit_DMA(spi->hspi, data.data(), data.size()); + HAL_StatusTypeDef errorcode = HAL_SPI_Transmit_DMA(spi->hspi, data.data(), data.size()); turn_on_chip_select(spi); switch (errorcode) { - case HAL_OK: - return true; - break; - case HAL_BUSY: - return false; - break; - default: - ErrorHandler( - "Error while transmiting and receiving with spi DMA. Errorcode " - "%u", - (uint8_t)errorcode); - return false; - break; + case HAL_OK: + return true; + break; + case HAL_BUSY: + return false; + break; + default: + ErrorHandler( + "Error while transmiting and receiving with spi DMA. Errorcode " + "%u", + (uint8_t)errorcode + ); + return false; + break; } } @@ -162,23 +161,23 @@ bool SPI::receive(uint8_t id, span data) { SPI::Instance* spi = SPI::registered_spi[id]; turn_off_chip_select(spi); - HAL_StatusTypeDef errorcode = - HAL_SPI_Receive(spi->hspi, data.data(), data.size(), 10); + HAL_StatusTypeDef errorcode = HAL_SPI_Receive(spi->hspi, data.data(), data.size(), 10); turn_on_chip_select(spi); switch (errorcode) { - case HAL_OK: - return true; - break; - case HAL_BUSY: - return false; - break; - default: - ErrorHandler( - "Error while transmiting and receiving with spi DMA. Errorcode " - "%u", - (uint8_t)errorcode); - return false; - break; + case HAL_OK: + return true; + break; + case HAL_BUSY: + return false; + break; + default: + ErrorHandler( + "Error while transmiting and receiving with spi DMA. Errorcode " + "%u", + (uint8_t)errorcode + ); + return false; + break; } } bool SPI::receive_DMA(uint8_t id, span data) { @@ -189,27 +188,26 @@ bool SPI::receive_DMA(uint8_t id, span data) { SPI::Instance* spi = SPI::registered_spi[id]; - HAL_StatusTypeDef errorcode = - HAL_SPI_Receive_DMA(spi->hspi, data.data(), data.size()); + HAL_StatusTypeDef errorcode = HAL_SPI_Receive_DMA(spi->hspi, data.data(), data.size()); switch (errorcode) { - case HAL_OK: - return true; - break; - case HAL_BUSY: - return false; - break; - default: - ErrorHandler( - "Error while transmiting and receiving with spi DMA. Errorcode " - "%u", - (uint8_t)errorcode); - return false; - break; + case HAL_OK: + return true; + break; + case HAL_BUSY: + return false; + break; + default: + ErrorHandler( + "Error while transmiting and receiving with spi DMA. Errorcode " + "%u", + (uint8_t)errorcode + ); + return false; + break; } } -bool SPI::transmit_and_receive(uint8_t id, span command_data, - span receive_data) { +bool SPI::transmit_and_receive(uint8_t id, span command_data, span receive_data) { if (!SPI::registered_spi.contains(id)) { ErrorHandler("No SPI registered with id %u", id); return false; @@ -217,13 +215,12 @@ bool SPI::transmit_and_receive(uint8_t id, span command_data, SPI::Instance* spi = SPI::registered_spi[id]; turn_off_chip_select(spi); - HAL_StatusTypeDef errorcode = HAL_SPI_Transmit( - spi->hspi, command_data.data(), command_data.size(), 10); + HAL_StatusTypeDef errorcode = + HAL_SPI_Transmit(spi->hspi, command_data.data(), command_data.size(), 10); if (errorcode != HAL_OK) { return false; } - if (HAL_SPI_Receive(spi->hspi, receive_data.data(), receive_data.size(), - 10) != HAL_OK) { + if (HAL_SPI_Receive(spi->hspi, receive_data.data(), receive_data.size(), 10) != HAL_OK) { turn_on_chip_select(spi); ErrorHandler("Error during receive in %s", spi->name.c_str()); return false; @@ -231,8 +228,11 @@ bool SPI::transmit_and_receive(uint8_t id, span command_data, turn_on_chip_select(spi); return true; } -bool SPI::transmit_and_receive_DMA(uint8_t id, span command_data, - span receive_data) { +bool SPI::transmit_and_receive_DMA( + uint8_t id, + span command_data, + span receive_data +) { if (!SPI::registered_spi.contains(id)) { ErrorHandler("No SPI registered with id %u", id); return false; @@ -240,23 +240,27 @@ bool SPI::transmit_and_receive_DMA(uint8_t id, span command_data, SPI::Instance* spi = SPI::registered_spi[id]; - HAL_StatusTypeDef errorcode = - HAL_SPI_TransmitReceive_DMA(spi->hspi, command_data.data(), - receive_data.data(), command_data.size()); + HAL_StatusTypeDef errorcode = HAL_SPI_TransmitReceive_DMA( + spi->hspi, + command_data.data(), + receive_data.data(), + command_data.size() + ); switch (errorcode) { - case HAL_OK: - return true; - break; - case HAL_BUSY: - return false; - break; - default: - ErrorHandler( - "Error while transmiting and receiving with spi DMA. Errorcode " - "%u", - (uint8_t)errorcode); - return false; - break; + case HAL_OK: + return true; + break; + case HAL_BUSY: + return false; + break; + default: + ErrorHandler( + "Error while transmiting and receiving with spi DMA. Errorcode " + "%u", + (uint8_t)errorcode + ); + return false; + break; } } @@ -344,8 +348,7 @@ void SPI::Order_update() { // slave again if it has the correct Order ID ready if (known_slave_ready(spi)) { master_check_available_end(spi); - } else if (Time::get_global_tick() - spi->last_end_check > - MASTER_SPI_CHECK_DELAY) { + } else if (Time::get_global_tick() - spi->last_end_check > MASTER_SPI_CHECK_DELAY) { master_check_available_end(spi); spi->last_end_check = Time::get_global_tick(); } @@ -359,13 +362,11 @@ void SPI::Order_update() { void SPI::master_check_available_end(SPI::Instance* spi) { SPI::turn_off_chip_select(spi); - spi_communicate_order_data(spi, (uint8_t*)spi->SPIOrderID, - (uint8_t*)spi->available_end, 2); + spi_communicate_order_data(spi, (uint8_t*)spi->SPIOrderID, (uint8_t*)spi->available_end, 2); } void SPI::slave_check_packet_ID(SPI::Instance* spi) { - spi_communicate_order_data(spi, (uint8_t*)spi->available_end, - (uint8_t*)spi->SPIOrderID, 2); + spi_communicate_order_data(spi, (uint8_t*)spi->available_end, (uint8_t*)spi->SPIOrderID, 2); } void SPI::chip_select_on(uint8_t id) { @@ -414,13 +415,10 @@ void SPI::init(SPI::Instance* spi) { spi->hspi->Init.NSSPMode = SPI_NSS_PULSE_ENABLE; spi->hspi->Init.NSSPolarity = spi->nss_polarity; spi->hspi->Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA; - spi->hspi->Init.TxCRCInitializationPattern = - SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN; - spi->hspi->Init.RxCRCInitializationPattern = - SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN; + spi->hspi->Init.TxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN; + spi->hspi->Init.RxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN; spi->hspi->Init.MasterSSIdleness = SPI_MASTER_SS_IDLENESS_00CYCLE; - spi->hspi->Init.MasterInterDataIdleness = - SPI_MASTER_INTERDATA_IDLENESS_00CYCLE; + spi->hspi->Init.MasterInterDataIdleness = SPI_MASTER_INTERDATA_IDLENESS_00CYCLE; spi->hspi->Init.MasterReceiverAutoSusp = SPI_MASTER_RX_AUTOSUSP_DISABLE; spi->hspi->Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_DISABLE; spi->hspi->Init.IOSwap = SPI_IO_SWAP_DISABLE; @@ -558,21 +556,21 @@ void SPI::init(SPI::Instance* spi) { // // } // // } -void SPI::spi_communicate_order_data(SPI::Instance* spi, uint8_t* value_to_send, - uint8_t* value_to_receive, - uint16_t size_to_send) { - HAL_SPI_TransmitReceive_DMA(spi->hspi, value_to_send, value_to_receive, - size_to_send); +void SPI::spi_communicate_order_data( + SPI::Instance* spi, + uint8_t* value_to_send, + uint8_t* value_to_receive, + uint16_t size_to_send +) { + HAL_SPI_TransmitReceive_DMA(spi->hspi, value_to_send, value_to_receive, size_to_send); } void SPI::turn_on_chip_select(SPI::Instance* spi) { - HAL_GPIO_WritePin(spi->SS->port, spi->SS->gpio_pin, - (GPIO_PinState)PinState::ON); + HAL_GPIO_WritePin(spi->SS->port, spi->SS->gpio_pin, (GPIO_PinState)PinState::ON); } void SPI::turn_off_chip_select(SPI::Instance* spi) { - HAL_GPIO_WritePin(spi->SS->port, spi->SS->gpio_pin, - (GPIO_PinState)PinState::OFF); + HAL_GPIO_WritePin(spi->SS->port, spi->SS->gpio_pin, (GPIO_PinState)PinState::OFF); } void SPI::mark_slave_ready(SPI::Instance* spi) { diff --git a/Src/HALAL/Services/Communication/UART/UART.cpp b/Src/HALAL/Services/Communication/UART/UART.cpp index 7a354e166..ba613e899 100644 --- a/Src/HALAL/Services/Communication/UART/UART.cpp +++ b/Src/HALAL/Services/Communication/UART/UART.cpp @@ -8,18 +8,18 @@ #ifdef HAL_UART_MODULE_ENABLED -unordered_map UART::registered_uart = {}; +unordered_map UART::registered_uart = {}; uint16_t UART::id_counter = 0; -uint8_t UART::inscribe(UART::Peripheral& uart){ - if ( !UART::available_uarts.contains(uart)){ - ErrorHandler(" The UART peripheral %d is already used or does not exists.", (uint16_t)uart); +uint8_t UART::inscribe(UART::Peripheral& uart) { + if (!UART::available_uarts.contains(uart)) { + ErrorHandler(" The UART peripheral %d is already used or does not exists.", (uint16_t)uart); - return 0; - } + return 0; + } - UART::Instance* uart_instance = UART::available_uarts[uart]; + UART::Instance* uart_instance = UART::available_uarts[uart]; Pin::inscribe(uart_instance->TX, ALTERNATIVE); Pin::inscribe(uart_instance->RX, ALTERNATIVE); @@ -31,66 +31,65 @@ uint8_t UART::inscribe(UART::Peripheral& uart){ return id; } -void UART::start(){ - for(auto iter : UART::registered_uart){ - UART::init(iter.second); - } +void UART::start() { + for (auto iter : UART::registered_uart) { + UART::init(iter.second); + } } -bool UART::transmit(uint8_t id, uint8_t data){ - array arr {data}; +bool UART::transmit(uint8_t id, uint8_t data) { + array arr{data}; return transmit(id, arr); } -bool UART::transmit(uint8_t id, span data){ +bool UART::transmit(uint8_t id, span data) { if (not UART::registered_uart.contains(id)) - return false; //TODO: Error handler + return false; // TODO: Error handler UART_HandleTypeDef* handle = get_handle(id); - if((handle->ErrorCode & TXBUSYMASK) == 1) - return false; + if ((handle->ErrorCode & TXBUSYMASK) == 1) + return false; - if (HAL_UART_Transmit_DMA(handle, data.data(), data.size()) != HAL_OK){ - return false; //TODO: Warning, Error during transmision + if (HAL_UART_Transmit_DMA(handle, data.data(), data.size()) != HAL_OK) { + return false; // TODO: Warning, Error during transmision } return true; } -bool UART::transmit_polling(uint8_t id, uint8_t data){ - array arr {data}; +bool UART::transmit_polling(uint8_t id, uint8_t data) { + array arr{data}; return transmit_polling(id, arr); } -bool UART::transmit_polling(uint8_t id, span data){ +bool UART::transmit_polling(uint8_t id, span data) { if (not UART::registered_uart.contains(id)) - return false; //TODO: Error handler + return false; // TODO: Error handler UART_HandleTypeDef* handle = get_handle(id); - if((handle->ErrorCode & TXBUSYMASK) == 1) - return false; + if ((handle->ErrorCode & TXBUSYMASK) == 1) + return false; - if (HAL_UART_Transmit(handle, data.data(), data.size(), 10) != HAL_OK){ - return false; //TODO: Warning, Error during transmision + if (HAL_UART_Transmit(handle, data.data(), data.size(), 10) != HAL_OK) { + return false; // TODO: Warning, Error during transmision } return true; } -bool UART::receive(uint8_t id, span data){ +bool UART::receive(uint8_t id, span data) { if (not UART::registered_uart.contains(id)) - return false; //TODO: Error handler + return false; // TODO: Error handler UART::Instance* uart = UART::registered_uart[id]; - if(((uart->huart->RxState & RXBUSYMASK) >> 1) == 1) - return false; - + if (((uart->huart->RxState & RXBUSYMASK) >> 1) == 1) + return false; if (HAL_UART_Receive_DMA(uart->huart, data.data(), data.size()) != HAL_OK) { - return false; //TODO: Warning, Error during receive + return false; // TODO: Warning, Error during receive } uart->receive_ready = false; @@ -98,134 +97,131 @@ bool UART::receive(uint8_t id, span data){ return true; } -bool UART::receive_polling(uint8_t id, span data){ +bool UART::receive_polling(uint8_t id, span data) { if (not UART::registered_uart.contains(id)) - return false; //TODO: Error handler + return false; // TODO: Error handler UART_HandleTypeDef* handle = get_handle(id); - if(((handle->RxState & RXBUSYMASK) >> 1) == 1) - return false; - + if (((handle->RxState & RXBUSYMASK) >> 1) == 1) + return false; if (HAL_UART_Receive(handle, data.data(), data.size(), 10) != HAL_OK) { - return false; //TODO: Warning, Error during receive + return false; // TODO: Warning, Error during receive } return true; } -void HAL_UART_RxCpltCallback(UART_HandleTypeDef * huart){ - auto result = find_if(UART::registered_uart.begin(), UART::registered_uart.end(), [&](auto uart){return uart.second->huart == huart;}); +void HAL_UART_RxCpltCallback(UART_HandleTypeDef* huart) { + auto result = + find_if(UART::registered_uart.begin(), UART::registered_uart.end(), [&](auto uart) { + return uart.second->huart == huart; + }); if (result != UART::registered_uart.end()) { (*result).second->receive_ready = true; - }else{ - __NOP();//TODO: Warning: Data received form an unknown UART + } else { + __NOP(); // TODO: Warning: Data received form an unknown UART } } -bool UART::has_next_packet(uint8_t id){ +bool UART::has_next_packet(uint8_t id) { if (not UART::registered_uart.contains(id)) - return false; //TODO: Error handler + return false; // TODO: Error handler UART::Instance* uart = UART::registered_uart[id]; return uart->receive_ready; } -bool UART::is_busy(uint8_t id){ +bool UART::is_busy(uint8_t id) { if (not UART::registered_uart.contains(id)) - return false; //TODO: Error handler + return false; // TODO: Error handler UART_HandleTypeDef* handle = get_handle(id); - if((handle->ErrorCode & TXBUSYMASK) == 1) - return false; + if ((handle->ErrorCode & TXBUSYMASK) == 1) + return false; return false; } -bool UART::set_up_printf(UART::Peripheral& uart){ - if (printf_ready) { - return false; - } +bool UART::set_up_printf(UART::Peripheral& uart) { + if (printf_ready) { + return false; + } - UART::printf_uart = UART::inscribe(uart); - setvbuf(stdout, NULL, _IONBF, 0); - setvbuf(stderr, NULL, _IONBF, 0); + UART::printf_uart = UART::inscribe(uart); + setvbuf(stdout, NULL, _IONBF, 0); + setvbuf(stderr, NULL, _IONBF, 0); - UART::printf_ready = true; + UART::printf_ready = true; - return UART::printf_ready; + return UART::printf_ready; } void UART::print_by_uart(char* ptr, int len) { - if (!UART::printf_ready) { - return; - } + if (!UART::printf_ready) { + return; + } - vector data(ptr, ptr+len); + vector data(ptr, ptr + len); - UART::transmit_polling(UART::printf_uart, data); + UART::transmit_polling(UART::printf_uart, data); } -void HAL_UART_ErrorCallback(UART_HandleTypeDef *uart){ - //TODO: Fault, UART error +void HAL_UART_ErrorCallback(UART_HandleTypeDef* uart) { + // TODO: Fault, UART error return; } -void UART::init(UART::Instance* uart){ +void UART::init(UART::Instance* uart) { - if (uart->initialized) { - return; - } - - UART_HandleTypeDef* handle = uart->huart; - handle->Instance = uart->instance; - handle->Init.BaudRate = uart->baud_rate; - handle->Init.WordLength = uart->word_length; - handle->Init.StopBits = UART_STOPBITS_1; - handle->Init.Parity = UART_PARITY_NONE; - handle->Init.Mode = UART_MODE_TX_RX; - handle->Init.HwFlowCtl = UART_HWCONTROL_NONE; - handle->Init.OverSampling = UART_OVERSAMPLING_16; - handle->Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE; - handle->Init.ClockPrescaler = UART_PRESCALER_DIV1; - handle->AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT; + if (uart->initialized) { + return; + } - if (HAL_UART_Init(handle) != HAL_OK){ - //TODO: Error Handler - } + UART_HandleTypeDef* handle = uart->huart; + handle->Instance = uart->instance; + handle->Init.BaudRate = uart->baud_rate; + handle->Init.WordLength = uart->word_length; + handle->Init.StopBits = UART_STOPBITS_1; + handle->Init.Parity = UART_PARITY_NONE; + handle->Init.Mode = UART_MODE_TX_RX; + handle->Init.HwFlowCtl = UART_HWCONTROL_NONE; + handle->Init.OverSampling = UART_OVERSAMPLING_16; + handle->Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE; + handle->Init.ClockPrescaler = UART_PRESCALER_DIV1; + handle->AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT; + + if (HAL_UART_Init(handle) != HAL_OK) { + // TODO: Error Handler + } - if (HAL_UARTEx_SetTxFifoThreshold(handle, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK){ - //TODO: Error Handler - } + if (HAL_UARTEx_SetTxFifoThreshold(handle, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK) { + // TODO: Error Handler + } - if (HAL_UARTEx_SetRxFifoThreshold(handle, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK){ - //TODO: Error Handler - } + if (HAL_UARTEx_SetRxFifoThreshold(handle, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK) { + // TODO: Error Handler + } - if (HAL_UARTEx_DisableFifoMode(handle) != HAL_OK){ - //TODO: Error Handler - } + if (HAL_UARTEx_DisableFifoMode(handle) != HAL_OK) { + // TODO: Error Handler + } - uart->initialized = true; + uart->initialized = true; } -UART_HandleTypeDef* UART::get_handle(uint8_t id) { - return registered_uart[id]->huart; -} +UART_HandleTypeDef* UART::get_handle(uint8_t id) { return registered_uart[id]->huart; } #ifdef __cplusplus extern "C" { #endif - - #ifdef __cplusplus } #endif #endif - diff --git a/Src/HALAL/Services/DigitalInputService/DigitalInputService.cpp b/Src/HALAL/Services/DigitalInputService/DigitalInputService.cpp index 6f5b33c65..b003ddbd7 100644 --- a/Src/HALAL/Services/DigitalInputService/DigitalInputService.cpp +++ b/Src/HALAL/Services/DigitalInputService/DigitalInputService.cpp @@ -8,20 +8,20 @@ #include "HALAL/Services/DigitalInputService/DigitalInputService.hpp" uint8_t DigitalInput::id_counter = 0; -map DigitalInput::service_ids = {}; +map DigitalInput::service_ids = {}; -uint8_t DigitalInput::inscribe(Pin& pin){ - Pin::inscribe(pin, INPUT); - DigitalInput::service_ids[id_counter] = pin; - return id_counter++; +uint8_t DigitalInput::inscribe(Pin& pin) { + Pin::inscribe(pin, INPUT); + DigitalInput::service_ids[id_counter] = pin; + return id_counter++; } -PinState DigitalInput::read_pin_state(uint8_t id){ - if (not DigitalInput::service_ids.contains(id)){ - ErrorHandler("ID %d is not registered as a DigitalInput", id); - return PinState::OFF; - } +PinState DigitalInput::read_pin_state(uint8_t id) { + if (not DigitalInput::service_ids.contains(id)) { + ErrorHandler("ID %d is not registered as a DigitalInput", id); + return PinState::OFF; + } - Pin pin = DigitalInput::service_ids[id]; - return (PinState)HAL_GPIO_ReadPin(pin.port, pin.gpio_pin); + Pin pin = DigitalInput::service_ids[id]; + return (PinState)HAL_GPIO_ReadPin(pin.port, pin.gpio_pin); } diff --git a/Src/HALAL/Services/EXTI/EXTI.cpp b/Src/HALAL/Services/EXTI/EXTI.cpp index e0d1536b4..11a38f73b 100644 --- a/Src/HALAL/Services/EXTI/EXTI.cpp +++ b/Src/HALAL/Services/EXTI/EXTI.cpp @@ -22,12 +22,12 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { } } -uint8_t ExternalInterrupt::inscribe(Pin& pin, function&& action, - TRIGGER trigger) { +uint8_t ExternalInterrupt::inscribe(Pin& pin, function&& action, TRIGGER trigger) { if (not instances.contains(pin.gpio_pin)) { ErrorHandler( " The pin %s is already used or isn t available for EXTI usage", - pin.to_string().c_str()); + pin.to_string().c_str() + ); return 0; } diff --git a/Src/HALAL/Services/Encoder/Encoder.cpp b/Src/HALAL/Services/Encoder/Encoder.cpp index 6d18f3c35..dd82c355e 100644 --- a/Src/HALAL/Services/Encoder/Encoder.cpp +++ b/Src/HALAL/Services/Encoder/Encoder.cpp @@ -19,7 +19,9 @@ uint8_t Encoder::inscribe(Pin& pin1, Pin& pin2) { ErrorHandler( " The pin %s and the pin %s are already used or aren't " "configurated for encoder usage", - pin1.to_string().c_str(), pin2.to_string().c_str()); + pin1.to_string().c_str(), + pin2.to_string().c_str() + ); return 0; } @@ -47,14 +49,12 @@ void Encoder::turn_on(uint8_t id) { TimerPeripheral* timer = pin_timer_map[registered_encoder[id]]; if (HAL_TIM_Encoder_GetState(timer->handle) == HAL_TIM_STATE_RESET) { - ErrorHandler("Unable to get state from encoder in timer %s", - timer->name.c_str()); + ErrorHandler("Unable to get state from encoder in timer %s", timer->name.c_str()); return; } if (HAL_TIM_Encoder_Start(timer->handle, TIM_CHANNEL_ALL) != HAL_OK) { - ErrorHandler("Unable to start encoder in timer %s", - timer->name.c_str()); + ErrorHandler("Unable to start encoder in timer %s", timer->name.c_str()); return; } @@ -111,8 +111,7 @@ void Encoder::init(TimerPeripheral* encoder) { TIM_Encoder_InitTypeDef sConfig = {0}; TIM_MasterConfigTypeDef sMasterConfig = {0}; - encoder->handle->Instance = - TimerPeripheral::handle_to_timer[encoder->handle]; + encoder->handle->Instance = TimerPeripheral::handle_to_timer[encoder->handle]; encoder->handle->Init.Prescaler = encoder->init_data.prescaler; encoder->handle->Init.CounterMode = TIM_COUNTERMODE_UP; encoder->handle->Init.Period = encoder->init_data.period; @@ -128,27 +127,23 @@ void Encoder::init(TimerPeripheral* encoder) { sConfig.IC2Prescaler = TIM_ICPSC_DIV1; sConfig.IC2Filter = 0; if (HAL_TIM_Encoder_Init(encoder->handle, &sConfig) != HAL_OK) { - ErrorHandler("Unable to init encoder in timer %s", - encoder->name.c_str()); + ErrorHandler("Unable to init encoder in timer %s", encoder->name.c_str()); } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; - if (HAL_TIMEx_MasterConfigSynchronization(encoder->handle, - &sMasterConfig) != HAL_OK) { + if (HAL_TIMEx_MasterConfigSynchronization(encoder->handle, &sMasterConfig) != HAL_OK) { ErrorHandler( "Unable to config master synchronization in encoder in timer %s", - encoder->name.c_str()); + encoder->name.c_str() + ); } } -int64_t Encoder::get_delta_clock(uint64_t clock_time, - uint64_t last_clock_time) { +int64_t Encoder::get_delta_clock(uint64_t clock_time, uint64_t last_clock_time) { int64_t delta_clock = clock_time - last_clock_time; - if (clock_time < last_clock_time) { // overflow handle - delta_clock = - clock_time + - CLOCK_MAX_VALUE * NANO_SECOND / HAL_RCC_GetPCLK1Freq() * 2 - - last_clock_time; + if (clock_time < last_clock_time) { // overflow handle + delta_clock = clock_time + CLOCK_MAX_VALUE * NANO_SECOND / HAL_RCC_GetPCLK1Freq() * 2 - + last_clock_time; } return delta_clock; } diff --git a/Src/HALAL/Services/FMAC/FMAC.cpp b/Src/HALAL/Services/FMAC/FMAC.cpp index 1076a69b0..f72c61af5 100644 --- a/Src/HALAL/Services/FMAC/FMAC.cpp +++ b/Src/HALAL/Services/FMAC/FMAC.cpp @@ -10,161 +10,187 @@ MultiplierAccelerator::FMACMemoryLayout MultiplierAccelerator::MemoryLayout; MultiplierAccelerator::FMACProcessInstance MultiplierAccelerator::Process; -void MultiplierAccelerator::IIR_software_in_software_out_inscribe(uint16_t input_coefficient_array_size, int16_t* input_coefficient_array, uint16_t feedback_coefficient_array_size, int16_t* feedback_coefficient_array){ - if(input_coefficient_array_size > 63 || feedback_coefficient_array_size > 63){ - ErrorHandler("Error while configurating IIR FMAC, no coefficient can be greater than 63"); - return; - } - - MemoryLayout.FFiltersSize = input_coefficient_array_size; - MemoryLayout.FFilterCoeffs = input_coefficient_array; - MemoryLayout.FeedbackFilterSize = feedback_coefficient_array_size; - MemoryLayout.FeedbackFilterCoeffs = feedback_coefficient_array; - MemoryLayout.FInSize = 127 - MemoryLayout.FFiltersSize; - MemoryLayout.FInValues = new int16_t[MemoryLayout.FInSize]; - MemoryLayout.FeedbackInSize = 127 - MemoryLayout.FeedbackFilterSize; - MemoryLayout.FeedbackInValues = new int16_t[MemoryLayout.FeedbackInSize]; - MemoryLayout.OutSize = MemoryLayout.FeedbackInSize; - MemoryLayout.OutValues = new int16_t[MemoryLayout.OutSize]; - Instance.mode = IIR; - - MultiplierAccelerator::inscribe(); +void MultiplierAccelerator::IIR_software_in_software_out_inscribe( + uint16_t input_coefficient_array_size, + int16_t* input_coefficient_array, + uint16_t feedback_coefficient_array_size, + int16_t* feedback_coefficient_array +) { + if (input_coefficient_array_size > 63 || feedback_coefficient_array_size > 63) { + ErrorHandler("Error while configurating IIR FMAC, no coefficient can be greater than 63"); + return; + } + + MemoryLayout.FFiltersSize = input_coefficient_array_size; + MemoryLayout.FFilterCoeffs = input_coefficient_array; + MemoryLayout.FeedbackFilterSize = feedback_coefficient_array_size; + MemoryLayout.FeedbackFilterCoeffs = feedback_coefficient_array; + MemoryLayout.FInSize = 127 - MemoryLayout.FFiltersSize; + MemoryLayout.FInValues = new int16_t[MemoryLayout.FInSize]; + MemoryLayout.FeedbackInSize = 127 - MemoryLayout.FeedbackFilterSize; + MemoryLayout.FeedbackInValues = new int16_t[MemoryLayout.FeedbackInSize]; + MemoryLayout.OutSize = MemoryLayout.FeedbackInSize; + MemoryLayout.OutValues = new int16_t[MemoryLayout.OutSize]; + Instance.mode = IIR; + + MultiplierAccelerator::inscribe(); } -void MultiplierAccelerator::inscribe(){ - DMA::inscribe_stream(Instance.dma_preload); - DMA::inscribe_stream(Instance.dma_read); - DMA::inscribe_stream(Instance.dma_write); +void MultiplierAccelerator::inscribe() { + DMA::inscribe_stream(Instance.dma_preload); + DMA::inscribe_stream(Instance.dma_read); + DMA::inscribe_stream(Instance.dma_write); } -void MultiplierAccelerator::start(){ - if(Instance.mode != MultiplierAccelerator::None){ - Instance.hfmac->Instance = FMAC; - if(HAL_FMAC_Init(Instance.hfmac)!= HAL_OK){ - ErrorHandler("Error while initialising the FMAC"); - } - - FMAC_FilterConfigTypeDef sFmacConfig; - - if(Instance.mode == IIR){ - sFmacConfig.CoeffBaseAddress = 0; - sFmacConfig.CoeffBufferSize = MemoryLayout.FFiltersSize + MemoryLayout.FeedbackFilterSize; - sFmacConfig.InputBaseAddress = MemoryLayout.FFiltersSize + MemoryLayout.FeedbackFilterSize; - sFmacConfig.InputBufferSize = MemoryLayout.FInSize; - sFmacConfig.InputThreshold = FMAC_THRESHOLD_1; - sFmacConfig.OutputBaseAddress = MemoryLayout.FFiltersSize + MemoryLayout.FeedbackFilterSize + MemoryLayout.FInSize; - sFmacConfig.OutputBufferSize = MemoryLayout.FeedbackInSize; - sFmacConfig.OutputThreshold = FMAC_THRESHOLD_1; - sFmacConfig.pCoeffA = MemoryLayout.FeedbackFilterCoeffs; - sFmacConfig.CoeffASize = MemoryLayout.FeedbackFilterSize; - sFmacConfig.pCoeffB = MemoryLayout.FFilterCoeffs; - sFmacConfig.CoeffBSize = MemoryLayout.FFiltersSize; - sFmacConfig.Filter = FMAC_FUNC_IIR_DIRECT_FORM_1; - sFmacConfig.InputAccess = FMAC_BUFFER_ACCESS_DMA; - sFmacConfig.OutputAccess = FMAC_BUFFER_ACCESS_DMA; - sFmacConfig.Clip = FMAC_CLIP_DISABLED; - sFmacConfig.P = MemoryLayout.FFiltersSize; - sFmacConfig.Q = MemoryLayout.FeedbackFilterSize; - sFmacConfig.R = 0; - - if (HAL_FMAC_FilterConfig(Instance.hfmac, &sFmacConfig) != HAL_OK) - {ErrorHandler("Error while configurating the FMAC");} - - }else{ - - } - - } +void MultiplierAccelerator::start() { + if (Instance.mode != MultiplierAccelerator::None) { + Instance.hfmac->Instance = FMAC; + if (HAL_FMAC_Init(Instance.hfmac) != HAL_OK) { + ErrorHandler("Error while initialising the FMAC"); + } + + FMAC_FilterConfigTypeDef sFmacConfig; + + if (Instance.mode == IIR) { + sFmacConfig.CoeffBaseAddress = 0; + sFmacConfig.CoeffBufferSize = + MemoryLayout.FFiltersSize + MemoryLayout.FeedbackFilterSize; + sFmacConfig.InputBaseAddress = + MemoryLayout.FFiltersSize + MemoryLayout.FeedbackFilterSize; + sFmacConfig.InputBufferSize = MemoryLayout.FInSize; + sFmacConfig.InputThreshold = FMAC_THRESHOLD_1; + sFmacConfig.OutputBaseAddress = + MemoryLayout.FFiltersSize + MemoryLayout.FeedbackFilterSize + MemoryLayout.FInSize; + sFmacConfig.OutputBufferSize = MemoryLayout.FeedbackInSize; + sFmacConfig.OutputThreshold = FMAC_THRESHOLD_1; + sFmacConfig.pCoeffA = MemoryLayout.FeedbackFilterCoeffs; + sFmacConfig.CoeffASize = MemoryLayout.FeedbackFilterSize; + sFmacConfig.pCoeffB = MemoryLayout.FFilterCoeffs; + sFmacConfig.CoeffBSize = MemoryLayout.FFiltersSize; + sFmacConfig.Filter = FMAC_FUNC_IIR_DIRECT_FORM_1; + sFmacConfig.InputAccess = FMAC_BUFFER_ACCESS_DMA; + sFmacConfig.OutputAccess = FMAC_BUFFER_ACCESS_DMA; + sFmacConfig.Clip = FMAC_CLIP_DISABLED; + sFmacConfig.P = MemoryLayout.FFiltersSize; + sFmacConfig.Q = MemoryLayout.FeedbackFilterSize; + sFmacConfig.R = 0; + + if (HAL_FMAC_FilterConfig(Instance.hfmac, &sFmacConfig) != HAL_OK) { + ErrorHandler("Error while configurating the FMAC"); + } + + } else { + } + } } -void MultiplierAccelerator::software_preload(int16_t* preload_data, uint8_t amount_to_preload, int16_t* preload_feedback_data, uint8_t amount_to_feedback_preload){ +void MultiplierAccelerator::software_preload( + int16_t* preload_data, + uint8_t amount_to_preload, + int16_t* preload_feedback_data, + uint8_t amount_to_feedback_preload +) { #if FMAC_ERROR_CHECK != 0 - if(amount_to_preload > MemoryLayout.FInSize || amount_to_preload > MemoryLayout.FeedbackInSize){ - ErrorHandler("Error while preloading data, cannot preload more data than the structure can hold"); - } - - while(HAL_FMAC_GetState(Instance.hfmac) != HAL_FMAC_STATE_READY){ - - } + if (amount_to_preload > MemoryLayout.FInSize || + amount_to_preload > MemoryLayout.FeedbackInSize) { + ErrorHandler( + "Error while preloading data, cannot preload more data than the structure can hold" + ); + } + + while (HAL_FMAC_GetState(Instance.hfmac) != HAL_FMAC_STATE_READY) { + } #endif - if (HAL_FMAC_FilterPreload(Instance.hfmac, preload_data, amount_to_preload, preload_feedback_data, amount_to_feedback_preload) != HAL_OK){ - ErrorHandler("Unexpected error on preload of data"); - } - + if (HAL_FMAC_FilterPreload( + Instance.hfmac, + preload_data, + amount_to_preload, + preload_feedback_data, + amount_to_feedback_preload + ) != HAL_OK) { + ErrorHandler("Unexpected error on preload of data"); + } } -void MultiplierAccelerator::software_run(int16_t* calculated_data, uint16_t* output_size){ - Process.running_output_data = calculated_data; - Process.output_data_size = *output_size; - Process.state = MultiplierAccelerator::WAITING_DATA; - if(HAL_FMAC_FilterStart(Instance.hfmac, calculated_data, output_size) != HAL_OK){ - ErrorHandler("Error while starting FMAC"); - } +void MultiplierAccelerator::software_run(int16_t* calculated_data, uint16_t* output_size) { + Process.running_output_data = calculated_data; + Process.output_data_size = *output_size; + Process.state = MultiplierAccelerator::WAITING_DATA; + if (HAL_FMAC_FilterStart(Instance.hfmac, calculated_data, output_size) != HAL_OK) { + ErrorHandler("Error while starting FMAC"); + } } -void MultiplierAccelerator::software_load_input(int16_t* input_data, uint16_t* input_size){ - Process.input_data = input_data; - Process.input_data_size = *input_size; - if(HAL_FMAC_AppendFilterData(Instance.hfmac, input_data, input_size) != HAL_OK){ - ErrorHandler("Error while loading data into the FMAC"); - } - Process.state = MultiplierAccelerator::RUNNING; -} - - -void MultiplierAccelerator::software_load_repeat(int16_t* input_data, uint16_t* input_size){ - Process.state = MultiplierAccelerator::RUNNING; - - Process.running_output_data = Process.output_data; - if(HAL_FMAC_ConfigFilterOutputBuffer(Instance.hfmac, Process.running_output_data, &Process.output_data_size) != HAL_OK){ - ErrorHandler("Error while preparing buffer for the FMAC"); - } - - Process.input_data = input_data; - Process.input_data_size = *input_size; - if(HAL_FMAC_AppendFilterData(Instance.hfmac, input_data, input_size) != HAL_OK){ - ErrorHandler("Error while loading data into the FMAC"); - } +void MultiplierAccelerator::software_load_input(int16_t* input_data, uint16_t* input_size) { + Process.input_data = input_data; + Process.input_data_size = *input_size; + if (HAL_FMAC_AppendFilterData(Instance.hfmac, input_data, input_size) != HAL_OK) { + ErrorHandler("Error while loading data into the FMAC"); + } + Process.state = MultiplierAccelerator::RUNNING; } -void MultiplierAccelerator::software_load_full(int16_t* input_data, uint16_t* input_size, int16_t* output_data, uint16_t* output_size){ - Process.state = MultiplierAccelerator::RUNNING; - - Process.running_output_data = output_data; - Process.output_data_size = *output_size; - if(HAL_FMAC_ConfigFilterOutputBuffer(Instance.hfmac, output_data, output_size) != HAL_OK){ - ErrorHandler("Error while preparing buffer for the FMAC"); - } - - Process.input_data = input_data; - Process.input_data_size = *input_size; - if(HAL_FMAC_AppendFilterData(Instance.hfmac, input_data, input_size) != HAL_OK){ - ErrorHandler("Error while loading data into the FMAC"); - } +void MultiplierAccelerator::software_load_repeat(int16_t* input_data, uint16_t* input_size) { + Process.state = MultiplierAccelerator::RUNNING; + + Process.running_output_data = Process.output_data; + if (HAL_FMAC_ConfigFilterOutputBuffer( + Instance.hfmac, + Process.running_output_data, + &Process.output_data_size + ) != HAL_OK) { + ErrorHandler("Error while preparing buffer for the FMAC"); + } + + Process.input_data = input_data; + Process.input_data_size = *input_size; + if (HAL_FMAC_AppendFilterData(Instance.hfmac, input_data, input_size) != HAL_OK) { + ErrorHandler("Error while loading data into the FMAC"); + } } -void MultiplierAccelerator::software_stop(){ - if(HAL_FMAC_FilterStop(Instance.hfmac) != HAL_OK){ - ErrorHandler("Error while stopping FMAC"); - } +void MultiplierAccelerator::software_load_full( + int16_t* input_data, + uint16_t* input_size, + int16_t* output_data, + uint16_t* output_size +) { + Process.state = MultiplierAccelerator::RUNNING; + + Process.running_output_data = output_data; + Process.output_data_size = *output_size; + if (HAL_FMAC_ConfigFilterOutputBuffer(Instance.hfmac, output_data, output_size) != HAL_OK) { + ErrorHandler("Error while preparing buffer for the FMAC"); + } + + Process.input_data = input_data; + Process.input_data_size = *input_size; + if (HAL_FMAC_AppendFilterData(Instance.hfmac, input_data, input_size) != HAL_OK) { + ErrorHandler("Error while loading data into the FMAC"); + } } -bool MultiplierAccelerator::is_ready(){ - return Instance.hfmac->State == HAL_FMAC_STATE_READY && Process.state != MultiplierAccelerator::RUNNING; +void MultiplierAccelerator::software_stop() { + if (HAL_FMAC_FilterStop(Instance.hfmac) != HAL_OK) { + ErrorHandler("Error while stopping FMAC"); + } } -void HAL_FMAC_HalfOutputDataReadyCallback(FMAC_HandleTypeDef *hfmac){} - - -void HAL_FMAC_OutputDataReadyCallback(FMAC_HandleTypeDef *hfmac){ - SCB_CleanInvalidateDCache_by_Addr( (uint32_t*)MultiplierAccelerator::Process.running_output_data, sizeof(MultiplierAccelerator::Process.output_data)); - MultiplierAccelerator::Process.output_data = MultiplierAccelerator::Process.running_output_data; - MultiplierAccelerator::Process.state = MultiplierAccelerator::WAITING_DATA; +bool MultiplierAccelerator::is_ready() { + return Instance.hfmac->State == HAL_FMAC_STATE_READY && + Process.state != MultiplierAccelerator::RUNNING; } +void HAL_FMAC_HalfOutputDataReadyCallback(FMAC_HandleTypeDef* hfmac) {} -void HAL_FMAC_ErrorCallback(FMAC_HandleTypeDef *hfmac){ - ErrorHandler("Error while running FMAC"); +void HAL_FMAC_OutputDataReadyCallback(FMAC_HandleTypeDef* hfmac) { + SCB_CleanInvalidateDCache_by_Addr( + (uint32_t*)MultiplierAccelerator::Process.running_output_data, + sizeof(MultiplierAccelerator::Process.output_data) + ); + MultiplierAccelerator::Process.output_data = MultiplierAccelerator::Process.running_output_data; + MultiplierAccelerator::Process.state = MultiplierAccelerator::WAITING_DATA; } +void HAL_FMAC_ErrorCallback(FMAC_HandleTypeDef* hfmac) { ErrorHandler("Error while running FMAC"); } diff --git a/Src/HALAL/Services/Flash/Flash.cpp b/Src/HALAL/Services/Flash/Flash.cpp index 5df93a3cc..ee6edde97 100644 --- a/Src/HALAL/Services/Flash/Flash.cpp +++ b/Src/HALAL/Services/Flash/Flash.cpp @@ -7,169 +7,157 @@ #include "HALAL/Services/Flash/Flash.hpp" -void Flash::read(uint32_t source_addr, uint32_t* result, uint32_t number_of_words){ - if (source_addr < FLASH_START_ADDRESS || source_addr > FLASH_END_ADDRESS) { - ErrorHandler("Address out of memory when trying to read flash memory."); - return; - } - - HAL_FLASH_Unlock(); - uint32_t i; - for (i = 0; i < number_of_words; i++) { - *result = *(__IO uint32_t *)(source_addr); - source_addr += 4; - result++; - } - HAL_FLASH_Lock(); +void Flash::read(uint32_t source_addr, uint32_t* result, uint32_t number_of_words) { + if (source_addr < FLASH_START_ADDRESS || source_addr > FLASH_END_ADDRESS) { + ErrorHandler("Address out of memory when trying to read flash memory."); + return; + } + + HAL_FLASH_Unlock(); + uint32_t i; + for (i = 0; i < number_of_words; i++) { + *result = *(__IO uint32_t*)(source_addr); + source_addr += 4; + result++; + } + HAL_FLASH_Lock(); } -//TODO: Estaria muy bien optimizar el uso de ram en la escritura de múltiples sectores -bool Flash::write(uint32_t * source, uint32_t dest_addr, uint32_t number_of_words){ - if (dest_addr < FLASH_SECTOR0_START_ADDRESS || dest_addr > FLASH_END_ADDRESS) { - ErrorHandler("Address out of memory when trying to write flash memory."); - return false; - } - - uint32_t start_relative_position_in_words; - uint32_t end_relative_position_in_words; - uint32_t buff_pos, source_pos; - uint32_t index = 0; - - uint32_t start_sector = Flash::get_sector(dest_addr); - uint32_t start_sector_addr = Flash::get_sector_starting_address(start_sector); - uint32_t end_address = dest_addr + ((number_of_words * 4) - 4); - uint32_t end_sector = Flash::get_sector(end_address); - uint8_t number_of_sectors = end_sector - start_sector + 1; - std::unique_ptr buffer = std::make_unique_for_overwrite(SECTOR_SIZE_IN_WORDS * number_of_sectors); - start_relative_position_in_words = (dest_addr - start_sector_addr) / 4 ; - end_relative_position_in_words = start_relative_position_in_words + number_of_words - 1; - source_pos = 0; - - Flash::read(start_sector_addr, buffer.get(), SECTOR_SIZE_IN_WORDS * number_of_sectors); - for (buff_pos = start_relative_position_in_words; buff_pos <= end_relative_position_in_words && source_pos < number_of_words; ++buff_pos ) { - buffer[buff_pos] = source[source_pos]; - source_pos++; - } - - if (not Flash::erase(start_sector, end_sector)) { - //TODO: Exception handle (Error while erasing for writing, aborting...) - return false; - } - - HAL_FLASH_Unlock(); - while(index < (uint32_t)(SECTOR_SIZE_IN_WORDS * number_of_sectors) ){ - if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_FLASHWORD, start_sector_addr, (uint32_t)&buffer[index]) == HAL_OK) { - start_sector_addr += 4 * FLASHWORD; - index += FLASHWORD; - }else{ - HAL_FLASH_Lock(); - //TODO: Exception handle (Error while writing, aborting...) - return false; - } - } - - HAL_FLASH_Lock(); - return true; +// TODO: Estaria muy bien optimizar el uso de ram en la escritura de múltiples sectores +bool Flash::write(uint32_t* source, uint32_t dest_addr, uint32_t number_of_words) { + if (dest_addr < FLASH_SECTOR0_START_ADDRESS || dest_addr > FLASH_END_ADDRESS) { + ErrorHandler("Address out of memory when trying to write flash memory."); + return false; + } + + uint32_t start_relative_position_in_words; + uint32_t end_relative_position_in_words; + uint32_t buff_pos, source_pos; + uint32_t index = 0; + + uint32_t start_sector = Flash::get_sector(dest_addr); + uint32_t start_sector_addr = Flash::get_sector_starting_address(start_sector); + uint32_t end_address = dest_addr + ((number_of_words * 4) - 4); + uint32_t end_sector = Flash::get_sector(end_address); + uint8_t number_of_sectors = end_sector - start_sector + 1; + std::unique_ptr buffer = + std::make_unique_for_overwrite(SECTOR_SIZE_IN_WORDS * number_of_sectors); + start_relative_position_in_words = (dest_addr - start_sector_addr) / 4; + end_relative_position_in_words = start_relative_position_in_words + number_of_words - 1; + source_pos = 0; + + Flash::read(start_sector_addr, buffer.get(), SECTOR_SIZE_IN_WORDS * number_of_sectors); + for (buff_pos = start_relative_position_in_words; + buff_pos <= end_relative_position_in_words && source_pos < number_of_words; + ++buff_pos) { + buffer[buff_pos] = source[source_pos]; + source_pos++; + } + + if (not Flash::erase(start_sector, end_sector)) { + // TODO: Exception handle (Error while erasing for writing, aborting...) + return false; + } + + HAL_FLASH_Unlock(); + while (index < (uint32_t)(SECTOR_SIZE_IN_WORDS * number_of_sectors)) { + if (HAL_FLASH_Program( + FLASH_TYPEPROGRAM_FLASHWORD, + start_sector_addr, + (uint32_t)&buffer[index] + ) == HAL_OK) { + start_sector_addr += 4 * FLASHWORD; + index += FLASHWORD; + } else { + HAL_FLASH_Lock(); + // TODO: Exception handle (Error while writing, aborting...) + return false; + } + } + + HAL_FLASH_Lock(); + return true; } -bool Flash::erase(uint32_t startSector, uint32_t endSector){ - static FLASH_EraseInitTypeDef EraseInitStruct; - uint32_t sectorError; +bool Flash::erase(uint32_t startSector, uint32_t endSector) { + static FLASH_EraseInitTypeDef EraseInitStruct; + uint32_t sectorError; - EraseInitStruct.TypeErase = FLASH_TYPEERASE_SECTORS; - EraseInitStruct.VoltageRange = FLASH_VOLTAGE_RANGE_3; - EraseInitStruct.Sector = startSector; - EraseInitStruct.Banks = FLASH_BANK_1; - EraseInitStruct.NbSectors = (endSector - startSector) + 1; + EraseInitStruct.TypeErase = FLASH_TYPEERASE_SECTORS; + EraseInitStruct.VoltageRange = FLASH_VOLTAGE_RANGE_3; + EraseInitStruct.Sector = startSector; + EraseInitStruct.Banks = FLASH_BANK_1; + EraseInitStruct.NbSectors = (endSector - startSector) + 1; - HAL_FLASH_Unlock(); + HAL_FLASH_Unlock(); - if (HAL_FLASHEx_Erase(&EraseInitStruct, §orError) != HAL_OK) - { - //TODO: Handle the exception(Error while erasing, aborting...) - HAL_FLASH_Lock(); - return false; - } + if (HAL_FLASHEx_Erase(&EraseInitStruct, §orError) != HAL_OK) { + // TODO: Handle the exception(Error while erasing, aborting...) + HAL_FLASH_Lock(); + return false; + } - HAL_FLASH_Lock(); + HAL_FLASH_Lock(); - return true; + return true; } -uint32_t Flash::get_sector(uint32_t Address) -{ - uint32_t sector = FLASH_SECTOR_0; - - /* BANK 1 */ - if((Address >= 0x08000000) && (Address < 0x08020000)) - { - sector = FLASH_SECTOR_0; - } - else if((Address >= 0x08020000) && (Address < 0x08040000)) - { - sector = FLASH_SECTOR_1; - } - else if((Address >= 0x08040000) && (Address < 0x08060000)) - { - sector = FLASH_SECTOR_2; - } - else if((Address >= 0x08060000) && (Address < 0x08080000)) - { - sector = FLASH_SECTOR_3; - } - else if((Address >= 0x08080000) && (Address < 0x080A0000)) - { - sector = FLASH_SECTOR_4; - } - else if((Address >= 0x080A0000) && (Address < 0x080C0000)) - { - sector = FLASH_SECTOR_5; - } - else if((Address >= 0x080C0000) && (Address < 0x080E0000)) - { - sector = FLASH_SECTOR_6; - } - else if((Address >= 0x080E0000) && (Address < 0x08100000)) - { - sector = FLASH_SECTOR_7; - } - - return sector; +uint32_t Flash::get_sector(uint32_t Address) { + uint32_t sector = FLASH_SECTOR_0; + + /* BANK 1 */ + if ((Address >= 0x08000000) && (Address < 0x08020000)) { + sector = FLASH_SECTOR_0; + } else if ((Address >= 0x08020000) && (Address < 0x08040000)) { + sector = FLASH_SECTOR_1; + } else if ((Address >= 0x08040000) && (Address < 0x08060000)) { + sector = FLASH_SECTOR_2; + } else if ((Address >= 0x08060000) && (Address < 0x08080000)) { + sector = FLASH_SECTOR_3; + } else if ((Address >= 0x08080000) && (Address < 0x080A0000)) { + sector = FLASH_SECTOR_4; + } else if ((Address >= 0x080A0000) && (Address < 0x080C0000)) { + sector = FLASH_SECTOR_5; + } else if ((Address >= 0x080C0000) && (Address < 0x080E0000)) { + sector = FLASH_SECTOR_6; + } else if ((Address >= 0x080E0000) && (Address < 0x08100000)) { + sector = FLASH_SECTOR_7; + } + + return sector; } -uint32_t Flash::get_sector_starting_address(uint32_t sector){ - uint32_t address; - switch (sector) { - case FLASH_SECTOR_0: - address = FLASH_SECTOR0_START_ADDRESS; - break; - case FLASH_SECTOR_1: - address = FLASH_SECTOR1_START_ADDRESS; - break; - case FLASH_SECTOR_2: - address = FLASH_SECTOR2_START_ADDRESS; - break; - case FLASH_SECTOR_3: - address = FLASH_SECTOR3_START_ADDRESS; - break; - case FLASH_SECTOR_4: - address = FLASH_SECTOR4_START_ADDRESS; - break; - case FLASH_SECTOR_5: - address = FLASH_SECTOR5_START_ADDRESS; - break; - case FLASH_SECTOR_6: - address = FLASH_SECTOR6_START_ADDRESS; - break; - case FLASH_SECTOR_7: - address = FLASH_SECTOR7_START_ADDRESS; - break; - default: - address = FLASH_SECTOR0_START_ADDRESS; - break; - } - - return address; +uint32_t Flash::get_sector_starting_address(uint32_t sector) { + uint32_t address; + switch (sector) { + case FLASH_SECTOR_0: + address = FLASH_SECTOR0_START_ADDRESS; + break; + case FLASH_SECTOR_1: + address = FLASH_SECTOR1_START_ADDRESS; + break; + case FLASH_SECTOR_2: + address = FLASH_SECTOR2_START_ADDRESS; + break; + case FLASH_SECTOR_3: + address = FLASH_SECTOR3_START_ADDRESS; + break; + case FLASH_SECTOR_4: + address = FLASH_SECTOR4_START_ADDRESS; + break; + case FLASH_SECTOR_5: + address = FLASH_SECTOR5_START_ADDRESS; + break; + case FLASH_SECTOR_6: + address = FLASH_SECTOR6_START_ADDRESS; + break; + case FLASH_SECTOR_7: + address = FLASH_SECTOR7_START_ADDRESS; + break; + default: + address = FLASH_SECTOR0_START_ADDRESS; + break; + } + + return address; } - - diff --git a/Src/HALAL/Services/Flash/FlashTests/Flash_Test.cpp b/Src/HALAL/Services/Flash/FlashTests/Flash_Test.cpp index 0917bf42a..60bf1503f 100644 --- a/Src/HALAL/Services/Flash/FlashTests/Flash_Test.cpp +++ b/Src/HALAL/Services/Flash/FlashTests/Flash_Test.cpp @@ -6,159 +6,157 @@ */ #include "HALAL/Services/Flash/FlashTests/Flash_Test.hpp" -#define NUMERIC_PI 3.1415 -#define NUMERIC_E 2.7183 -#define SQUARE_ROOT_OF_TWO 1.4142 +#define NUMERIC_PI 3.1415 +#define NUMERIC_E 2.7183 +#define SQUARE_ROOT_OF_TWO 1.4142 -namespace FlashTest{ +namespace FlashTest { - void float_2_bytes(uint8_t * ftoa_bytes_temp, float float_variable){ - union { - float a; - uint8_t bytes[4]; - } uni; +void float_2_bytes(uint8_t* ftoa_bytes_temp, float float_variable) { + union { + float a; + uint8_t bytes[4]; + } uni; - uni.a = float_variable; + uni.a = float_variable; - for (uint8_t i = 0; i < 4; i++) { - ftoa_bytes_temp[i] = uni.bytes[i]; - } - } - - float bytes_2_float(uint8_t * ftoa_bytes_temp){ - union { - float a; - uint8_t bytes[4]; - } uni; - - for (uint8_t i = 0; i < 4; i++) { - uni.bytes[i] = ftoa_bytes_temp[i]; - } + for (uint8_t i = 0; i < 4; i++) { + ftoa_bytes_temp[i] = uni.bytes[i]; + } +} - float float_variable = uni.a; - return float_variable; - } +float bytes_2_float(uint8_t* ftoa_bytes_temp) { + union { + float a; + uint8_t bytes[4]; + } uni; - bool test1_writing_1_float(){ - uint8_t float_byte_array[4]; - uint32_t addr = FLASH_SECTOR4_START_ADDRESS; + for (uint8_t i = 0; i < 4; i++) { + uni.bytes[i] = ftoa_bytes_temp[i]; + } - float_2_bytes(float_byte_array, NUMERIC_PI); - return Flash::write((uint32_t *)float_byte_array, addr,(uint32_t) 1); - } + float float_variable = uni.a; + return float_variable; +} - bool test2_reading_1_float(){ - uint8_t float_byte_array[4]; +bool test1_writing_1_float() { + uint8_t float_byte_array[4]; + uint32_t addr = FLASH_SECTOR4_START_ADDRESS; - Flash::read(FLASH_SECTOR4_START_ADDRESS, (uint32_t *)float_byte_array, 1); + float_2_bytes(float_byte_array, NUMERIC_PI); + return Flash::write((uint32_t*)float_byte_array, addr, (uint32_t)1); +} - float result = bytes_2_float(float_byte_array); +bool test2_reading_1_float() { + uint8_t float_byte_array[4]; - return result * 10000 == NUMERIC_PI * 10000; - } + Flash::read(FLASH_SECTOR4_START_ADDRESS, (uint32_t*)float_byte_array, 1); - bool test3_writing_multiple_float(){ - uint8_t float_byte_array[4]; - uint32_t addr = FLASH_SECTOR4_START_ADDRESS; - uint32_t offset1 = 0x04; - uint32_t offset2 = 0x20; + float result = bytes_2_float(float_byte_array); - float_2_bytes(float_byte_array, NUMERIC_PI); - Flash::write((uint32_t *)float_byte_array, addr,(uint32_t) 1); + return result * 10000 == NUMERIC_PI * 10000; +} - float_2_bytes(float_byte_array, NUMERIC_E); - Flash::write((uint32_t *)float_byte_array, addr + offset1,(uint32_t) 1); +bool test3_writing_multiple_float() { + uint8_t float_byte_array[4]; + uint32_t addr = FLASH_SECTOR4_START_ADDRESS; + uint32_t offset1 = 0x04; + uint32_t offset2 = 0x20; - float_2_bytes(float_byte_array, SQUARE_ROOT_OF_TWO); - Flash::write((uint32_t *)float_byte_array, addr + offset2,(uint32_t) 1); + float_2_bytes(float_byte_array, NUMERIC_PI); + Flash::write((uint32_t*)float_byte_array, addr, (uint32_t)1); - Flash::read(addr, (uint32_t *)float_byte_array, 1); - float first_result = bytes_2_float(float_byte_array); - bool first_write = first_result * 10000 == NUMERIC_PI * 10000; + float_2_bytes(float_byte_array, NUMERIC_E); + Flash::write((uint32_t*)float_byte_array, addr + offset1, (uint32_t)1); - Flash::read(addr + offset1, (uint32_t *)float_byte_array, 1); - float second_result = bytes_2_float(float_byte_array); - bool second_write = second_result * 100000 == NUMERIC_E * 100000; + float_2_bytes(float_byte_array, SQUARE_ROOT_OF_TWO); + Flash::write((uint32_t*)float_byte_array, addr + offset2, (uint32_t)1); - Flash::read(addr + offset2, (uint32_t *)float_byte_array, 1); - float third_result = bytes_2_float(float_byte_array); - bool third_write = third_result * 100000 == SQUARE_ROOT_OF_TWO * 100000; + Flash::read(addr, (uint32_t*)float_byte_array, 1); + float first_result = bytes_2_float(float_byte_array); + bool first_write = first_result * 10000 == NUMERIC_PI * 10000; - return first_write && second_write && third_write; - } + Flash::read(addr + offset1, (uint32_t*)float_byte_array, 1); + float second_result = bytes_2_float(float_byte_array); + bool second_write = second_result * 100000 == NUMERIC_E * 100000; - bool test4_wr_in_sector_border(){ - uint32_t init_data[] = {1024, 2048, 4096, 8192}; - uint32_t data_result[4]; - uint32_t addr = FLASH_SECTOR5_START_ADDRESS - 0x08; + Flash::read(addr + offset2, (uint32_t*)float_byte_array, 1); + float third_result = bytes_2_float(float_byte_array); + bool third_write = third_result * 100000 == SQUARE_ROOT_OF_TWO * 100000; - Flash::write(&init_data[0], addr,(uint32_t) 4); - Flash::read(addr, &data_result[0], 4); + return first_write && second_write && third_write; +} - for(int i = 0; i < 4; i++){ - if (init_data[i] != data_result[i]) { - return false; - } - } +bool test4_wr_in_sector_border() { + uint32_t init_data[] = {1024, 2048, 4096, 8192}; + uint32_t data_result[4]; + uint32_t addr = FLASH_SECTOR5_START_ADDRESS - 0x08; - return true; - } + Flash::write(&init_data[0], addr, (uint32_t)4); + Flash::read(addr, &data_result[0], 4); - bool test5_write_time_1_word(){ - uint32_t data = 0xAABBCCDD; - uint32_t addr_data = FLASH_SECTOR6_START_ADDRESS - 0x04; - uint32_t addr_result = FLASH_SECTOR4_START_ADDRESS; - uint16_t offset = 0x10; - bool result; + for (int i = 0; i < 4; i++) { + if (init_data[i] != data_result[i]) { + return false; + } + } - uint32_t init = HAL_GetTick(); - result = Flash::write(&data, addr_data,(uint32_t) 1); - uint32_t end = HAL_GetTick() - init; + return true; +} +bool test5_write_time_1_word() { + uint32_t data = 0xAABBCCDD; + uint32_t addr_data = FLASH_SECTOR6_START_ADDRESS - 0x04; + uint32_t addr_result = FLASH_SECTOR4_START_ADDRESS; + uint16_t offset = 0x10; + bool result; - return result && Flash::write(&end, (uint32_t)(addr_result + offset), 1); + uint32_t init = HAL_GetTick(); + result = Flash::write(&data, addr_data, (uint32_t)1); + uint32_t end = HAL_GetTick() - init; - } + return result && Flash::write(&end, (uint32_t)(addr_result + offset), 1); +} - bool test6_write_time_1000_word(){ - uint32_t data[1000]; - uint32_t addr_data = FLASH_SECTOR6_START_ADDRESS; - uint32_t addr_result = FLASH_SECTOR4_START_ADDRESS; - uint16_t offset = 0x14; - bool result; +bool test6_write_time_1000_word() { + uint32_t data[1000]; + uint32_t addr_data = FLASH_SECTOR6_START_ADDRESS; + uint32_t addr_result = FLASH_SECTOR4_START_ADDRESS; + uint16_t offset = 0x14; + bool result; - for(int i = 0; i < 1000; i++){ - data[i] = i + 1; - } + for (int i = 0; i < 1000; i++) { + data[i] = i + 1; + } - uint32_t init = HAL_GetTick(); - result = Flash::write(&data[0], addr_data,(uint32_t) 1000); - uint32_t end = HAL_GetTick() - init; + uint32_t init = HAL_GetTick(); + result = Flash::write(&data[0], addr_data, (uint32_t)1000); + uint32_t end = HAL_GetTick() - init; - return result && Flash::write(&end, addr_result + offset, 1); - } + return result && Flash::write(&end, addr_result + offset, 1); +} - bool test7_read_time_1000_word(uint32_t* result){ - uint32_t addr_result = FLASH_SECTOR4_START_ADDRESS; - uint16_t offset = 0x18; +bool test7_read_time_1000_word(uint32_t* result) { + uint32_t addr_result = FLASH_SECTOR4_START_ADDRESS; + uint16_t offset = 0x18; - uint32_t init = HAL_GetTick(); - Flash::read(addr_result, result, 1000); - uint32_t end = HAL_GetTick() - init; + uint32_t init = HAL_GetTick(); + Flash::read(addr_result, result, 1000); + uint32_t end = HAL_GetTick() - init; - return Flash::write(&end, addr_result + offset, 1); - } + return Flash::write(&end, addr_result + offset, 1); +} - bool test8_erase_time_1_sector(){ - uint32_t addr_result = FLASH_SECTOR4_START_ADDRESS; - uint32_t sector = FLASH_SECTOR_7; - uint16_t offset = 0x1C; - bool result; +bool test8_erase_time_1_sector() { + uint32_t addr_result = FLASH_SECTOR4_START_ADDRESS; + uint32_t sector = FLASH_SECTOR_7; + uint16_t offset = 0x1C; + bool result; - uint32_t init = HAL_GetTick(); - result = Flash::erase(sector, sector); - uint32_t end = HAL_GetTick() - init; + uint32_t init = HAL_GetTick(); + result = Flash::erase(sector, sector); + uint32_t end = HAL_GetTick() - init; - return result && Flash::write(&end, addr_result + offset, 1); - } + return result && Flash::write(&end, addr_result + offset, 1); } +} // namespace FlashTest diff --git a/Src/HALAL/Services/InfoWarning/InfoWarning.cpp b/Src/HALAL/Services/InfoWarning/InfoWarning.cpp index dc36e81fa..d60c06988 100644 --- a/Src/HALAL/Services/InfoWarning/InfoWarning.cpp +++ b/Src/HALAL/Services/InfoWarning/InfoWarning.cpp @@ -13,49 +13,46 @@ string InfoWarning::func = "Error-No-Func-Found"; string InfoWarning::file = "Error-No-File-Found"; bool InfoWarning::warning_triggered = false; -void InfoWarning::SetMetaData(int line, const char * func, const char * file){ - InfoWarning::line = to_string(line); - InfoWarning::func = string(func); - InfoWarning::file = string(file); +void InfoWarning::SetMetaData(int line, const char* func, const char* file) { + InfoWarning::line = to_string(line); + InfoWarning::func = string(func); + InfoWarning::file = string(file); } -void InfoWarning::InfoWarningTrigger(string format, ... ){ - if (InfoWarning::warning_triggered) { - return; - } +void InfoWarning::InfoWarningTrigger(string format, ...) { + if (InfoWarning::warning_triggered) { + return; + } - InfoWarning::warning_triggered = true; + InfoWarning::warning_triggered = true; - if (format.length() != 0) { - description = ""; - } + if (format.length() != 0) { + description = ""; + } - va_list arguments; - va_start(arguments, format); - va_list arg_copy; - va_copy(arg_copy, arguments); + va_list arguments; + va_start(arguments, format); + va_list arg_copy; + va_copy(arg_copy, arguments); - const int32_t size = vsnprintf(nullptr, 0, format.c_str(), arguments) + 1; - const unique_ptr buffer = make_unique(size); - va_end(arguments); + const int32_t size = vsnprintf(nullptr, 0, format.c_str(), arguments) + 1; + const unique_ptr buffer = make_unique(size); + va_end(arguments); - vsnprintf(buffer.get(), size, format.c_str(), arg_copy); - va_end(arg_copy); + vsnprintf(buffer.get(), size, format.c_str(), arg_copy); + va_end(arg_copy); - description += string(buffer.get(), buffer.get() + size - 1) + " | Line: " + InfoWarning::line - + " Function: '" + InfoWarning::func + "' File: " + InfoWarning::file ; + description += string(buffer.get(), buffer.get() + size - 1) + " | Line: " + InfoWarning::line + + " Function: '" + InfoWarning::func + "' File: " + InfoWarning::file; #ifdef HAL_TIM_MODULE_ENABLED - description += " | TimeStamp: " + to_string(Time::get_global_tick()); + description += " | TimeStamp: " + to_string(Time::get_global_tick()); #endif - } -void InfoWarning::InfoWarningUpdate(){ - if (!InfoWarning::warning_triggered) { - return; - } - printf("Warning: %s%s", InfoWarning::description.c_str(), endl); - +void InfoWarning::InfoWarningUpdate() { + if (!InfoWarning::warning_triggered) { + return; + } + printf("Warning: %s%s", InfoWarning::description.c_str(), endl); } - diff --git a/Src/HALAL/Services/InputCapture/InputCapture.cpp b/Src/HALAL/Services/InputCapture/InputCapture.cpp index b7caa6885..293119de9 100644 --- a/Src/HALAL/Services/InputCapture/InputCapture.cpp +++ b/Src/HALAL/Services/InputCapture/InputCapture.cpp @@ -10,126 +10,138 @@ uint8_t InputCapture::id_counter = 0; map InputCapture::active_instances = {}; static map channel_dict = { - {HAL_TIM_ACTIVE_CHANNEL_1, TIM_CHANNEL_1}, - {HAL_TIM_ACTIVE_CHANNEL_2, TIM_CHANNEL_2}, - {HAL_TIM_ACTIVE_CHANNEL_3, TIM_CHANNEL_3}, - {HAL_TIM_ACTIVE_CHANNEL_4, TIM_CHANNEL_4}, - {HAL_TIM_ACTIVE_CHANNEL_5, TIM_CHANNEL_5}, - {HAL_TIM_ACTIVE_CHANNEL_6, TIM_CHANNEL_6} + {HAL_TIM_ACTIVE_CHANNEL_1, TIM_CHANNEL_1}, + {HAL_TIM_ACTIVE_CHANNEL_2, TIM_CHANNEL_2}, + {HAL_TIM_ACTIVE_CHANNEL_3, TIM_CHANNEL_3}, + {HAL_TIM_ACTIVE_CHANNEL_4, TIM_CHANNEL_4}, + {HAL_TIM_ACTIVE_CHANNEL_5, TIM_CHANNEL_5}, + {HAL_TIM_ACTIVE_CHANNEL_6, TIM_CHANNEL_6} }; -InputCapture::Instance::Instance(Pin& pin, TimerPeripheral* peripheral, uint32_t channel_rising, uint32_t channel_falling) : - pin(pin), - peripheral(peripheral), - channel_rising(channel_rising), - channel_falling(channel_falling) - { - frequency = 0; - duty_cycle = 0; - - } - -uint8_t InputCapture::inscribe(Pin& pin){ - if (not available_instances.contains(pin) || pin.mode != OperationMode::NOT_USED) { - ErrorHandler(" The pin %s is already used or isn t available for InputCapture usage", pin.to_string().c_str()); - return 0; - } - - Pin::inscribe(pin, TIMER_ALTERNATE_FUNCTION); - - Instance data = available_instances[pin]; - id_counter++; - active_instances[id_counter] = data; - active_instances[id_counter].id = id_counter; - - vector>& channels = active_instances[id_counter].peripheral->init_data.input_capture_channels; - uint32_t channel_rising = active_instances[id_counter].channel_rising; - uint32_t channel_falling = active_instances[id_counter].channel_falling; - channels.push_back({channel_rising, channel_falling}); - return id_counter; +InputCapture::Instance::Instance( + Pin& pin, + TimerPeripheral* peripheral, + uint32_t channel_rising, + uint32_t channel_falling +) + : pin(pin), peripheral(peripheral), channel_rising(channel_rising), + channel_falling(channel_falling) { + frequency = 0; + duty_cycle = 0; } -void InputCapture::turn_on(uint8_t id){ - if (not active_instances.contains(id)) { - ErrorHandler("ID %d is not registered as an active_instance", id); - return; - } - Instance instance = active_instances[id]; - - - if (HAL_TIM_IC_Start_IT(instance.peripheral->handle, instance.channel_rising) != HAL_OK) { - ErrorHandler("Unable to start the %s Input Capture measurement in interrupt mode", instance.peripheral->name.c_str()); - } - - if (HAL_TIM_IC_Start(instance.peripheral->handle, instance.channel_falling) != HAL_OK) { - ErrorHandler("Unable to start the %s Input Capture measurement", instance.peripheral->name.c_str()); - } - +uint8_t InputCapture::inscribe(Pin& pin) { + if (not available_instances.contains(pin) || pin.mode != OperationMode::NOT_USED) { + ErrorHandler( + " The pin %s is already used or isn t available for InputCapture usage", + pin.to_string().c_str() + ); + return 0; + } + + Pin::inscribe(pin, TIMER_ALTERNATE_FUNCTION); + + Instance data = available_instances[pin]; + id_counter++; + active_instances[id_counter] = data; + active_instances[id_counter].id = id_counter; + + vector>& channels = + active_instances[id_counter].peripheral->init_data.input_capture_channels; + uint32_t channel_rising = active_instances[id_counter].channel_rising; + uint32_t channel_falling = active_instances[id_counter].channel_falling; + channels.push_back({channel_rising, channel_falling}); + return id_counter; } -void InputCapture::turn_off(uint8_t id){ - if (not active_instances.contains(id)) { - ErrorHandler("ID %d is not registered as an active_instance", id); - return; - } - Instance instance = active_instances[id]; - if (HAL_TIM_IC_Stop_IT(instance.peripheral->handle, instance.channel_rising) != HAL_OK) { - ErrorHandler("Unable to stop the %s Input Capture measurement in interrupt mode", instance.peripheral->name.c_str()); - } - - if (HAL_TIM_IC_Stop(instance.peripheral->handle, instance.channel_falling) != HAL_OK) { - ErrorHandler("Unable to stop the %s Input Capture measurement", instance.peripheral->name.c_str()); - } +void InputCapture::turn_on(uint8_t id) { + if (not active_instances.contains(id)) { + ErrorHandler("ID %d is not registered as an active_instance", id); + return; + } + Instance instance = active_instances[id]; + + if (HAL_TIM_IC_Start_IT(instance.peripheral->handle, instance.channel_rising) != HAL_OK) { + ErrorHandler( + "Unable to start the %s Input Capture measurement in interrupt mode", + instance.peripheral->name.c_str() + ); + } + + if (HAL_TIM_IC_Start(instance.peripheral->handle, instance.channel_falling) != HAL_OK) { + ErrorHandler( + "Unable to start the %s Input Capture measurement", + instance.peripheral->name.c_str() + ); + } +} +void InputCapture::turn_off(uint8_t id) { + if (not active_instances.contains(id)) { + ErrorHandler("ID %d is not registered as an active_instance", id); + return; + } + Instance instance = active_instances[id]; + if (HAL_TIM_IC_Stop_IT(instance.peripheral->handle, instance.channel_rising) != HAL_OK) { + ErrorHandler( + "Unable to stop the %s Input Capture measurement in interrupt mode", + instance.peripheral->name.c_str() + ); + } + + if (HAL_TIM_IC_Stop(instance.peripheral->handle, instance.channel_falling) != HAL_OK) { + ErrorHandler( + "Unable to stop the %s Input Capture measurement", + instance.peripheral->name.c_str() + ); + } } uint32_t InputCapture::read_frequency(uint8_t id) { - if (not active_instances.contains(id)) { - ErrorHandler("ID %d is not registered as an active_instance", id); - return 0; - } - Instance instance = active_instances[id]; - return instance.frequency; + if (not active_instances.contains(id)) { + ErrorHandler("ID %d is not registered as an active_instance", id); + return 0; + } + Instance instance = active_instances[id]; + return instance.frequency; } uint8_t InputCapture::read_duty_cycle(uint8_t id) { - if (not active_instances.contains(id)) { - ErrorHandler("ID %d is not registered as an active_instance", id); - return 0; - } - Instance instance = active_instances[id]; - return instance.duty_cycle; + if (not active_instances.contains(id)) { + ErrorHandler("ID %d is not registered as an active_instance", id); + return 0; + } + Instance instance = active_instances[id]; + return instance.duty_cycle; } InputCapture::Instance InputCapture::find_instance_by_channel(uint32_t channel) { - for (auto id_instance: active_instances) { - uint32_t& ch_rising = id_instance.second.channel_rising; - uint32_t& ch_falling = id_instance.second.channel_falling; + for (auto id_instance : active_instances) { + uint32_t& ch_rising = id_instance.second.channel_rising; + uint32_t& ch_falling = id_instance.second.channel_falling; - if(ch_rising == channel || ch_falling == channel) { - return id_instance.second; - } - } + if (ch_rising == channel || ch_falling == channel) { + return id_instance.second; + } + } - ErrorHandler("Channel %d is not a registered channel", channel); - return Instance(); + ErrorHandler("Channel %d is not a registered channel", channel); + return Instance(); } -void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) -{ - htim->Instance->CNT = 0; - uint32_t& active_channel = channel_dict[htim->Channel]; - InputCapture::Instance instance = InputCapture::find_instance_by_channel(active_channel); - - uint32_t rising_value = HAL_TIM_ReadCapturedValue(htim, instance.channel_rising); - if (rising_value != 0) { - float ref_clock = HAL_RCC_GetPCLK1Freq()*2 / (instance.peripheral->handle->Init.Prescaler+1); - float falling_value = HAL_TIM_ReadCapturedValue(htim, instance.channel_falling); - - InputCapture::active_instances[instance.id].frequency = round(ref_clock / rising_value); - InputCapture::active_instances[instance.id].duty_cycle = round((falling_value * 100) / rising_value); - } - +void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef* htim) { + htim->Instance->CNT = 0; + uint32_t& active_channel = channel_dict[htim->Channel]; + InputCapture::Instance instance = InputCapture::find_instance_by_channel(active_channel); + + uint32_t rising_value = HAL_TIM_ReadCapturedValue(htim, instance.channel_rising); + if (rising_value != 0) { + float ref_clock = + HAL_RCC_GetPCLK1Freq() * 2 / (instance.peripheral->handle->Init.Prescaler + 1); + float falling_value = HAL_TIM_ReadCapturedValue(htim, instance.channel_falling); + + InputCapture::active_instances[instance.id].frequency = round(ref_clock / rising_value); + InputCapture::active_instances[instance.id].duty_cycle = + round((falling_value * 100) / rising_value); + } } - - diff --git a/Src/HALAL/Services/PWM/DualCenterPWM/DualCenterPWM.cpp b/Src/HALAL/Services/PWM/DualCenterPWM/DualCenterPWM.cpp index dfeeedd3c..aa6310b81 100644 --- a/Src/HALAL/Services/PWM/DualCenterPWM/DualCenterPWM.cpp +++ b/Src/HALAL/Services/PWM/DualCenterPWM/DualCenterPWM.cpp @@ -4,11 +4,12 @@ DualCenterPWM::DualCenterPWM(Pin& pin, Pin& pin_negated) { if (not TimerPeripheral::available_dual_pwms.contains({pin, pin_negated})) { ErrorHandler( "Pins %s and %s are not registered as an available Dual PWM", - pin.to_string(), pin_negated.to_string()); + pin.to_string(), + pin_negated.to_string() + ); } - TimerPeripheral& timer = - TimerPeripheral::available_dual_pwms.at({pin, pin_negated}).first; + TimerPeripheral& timer = TimerPeripheral::available_dual_pwms.at({pin, pin_negated}).first; TimerPeripheral::PWMData& pwm_data = TimerPeripheral::available_dual_pwms.at({pin, pin_negated}).second; @@ -18,7 +19,9 @@ DualCenterPWM::DualCenterPWM(Pin& pin, Pin& pin_negated) { if (pwm_data.mode != TimerPeripheral::PWM_MODE::CENTER_ALIGNED) { ErrorHandler( "Pins %s and %s are not registered as a CENTER ALIGNED PWM", - pin.to_string(), pin_negated.to_string()); + pin.to_string(), + pin_negated.to_string() + ); } Pin::inscribe(pin, TIMER_ALTERNATE_FUNCTION); @@ -31,7 +34,6 @@ DualCenterPWM::DualCenterPWM(Pin& pin, Pin& pin_negated) { void DualCenterPWM::set_frequency(uint32_t freq_in_hz) { this->frequency = freq_in_hz; TIM_TypeDef& timer = *peripheral->handle->Instance; - timer.ARR = - (HAL_RCC_GetPCLK1Freq() * 2 / (timer.PSC + 1)) / (frequency * 2); + timer.ARR = (HAL_RCC_GetPCLK1Freq() * 2 / (timer.PSC + 1)) / (frequency * 2); set_duty_cycle(duty_cycle); } \ No newline at end of file diff --git a/Src/HALAL/Services/PWM/DualPWM/DualPWM.cpp b/Src/HALAL/Services/PWM/DualPWM/DualPWM.cpp index 5bcd96d51..86225b141 100644 --- a/Src/HALAL/Services/PWM/DualPWM/DualPWM.cpp +++ b/Src/HALAL/Services/PWM/DualPWM/DualPWM.cpp @@ -10,113 +10,114 @@ #include "stm32h7xx_hal_def.h" DualPWM::DualPWM(Pin& pin, Pin& pin_negated) { - if (not TimerPeripheral::available_dual_pwms.contains({pin, pin_negated})) { - ErrorHandler("Pins %s and %s are not registered as an available Dual PWM", pin.to_string(), pin_negated.to_string()); - } - - TimerPeripheral& timer = TimerPeripheral::available_dual_pwms.at({pin, pin_negated}).first; - TimerPeripheral::PWMData& pwm_data = TimerPeripheral::available_dual_pwms.at({pin, pin_negated}).second; - - peripheral = &timer; - channel = pwm_data.channel; - - if (pwm_data.mode != TimerPeripheral::PWM_MODE::NORMAL) { - ErrorHandler("Pins %s and %s are not registered as a DUAL PWM", pin.to_string(), pin_negated.to_string()); - } - - Pin::inscribe(pin, TIMER_ALTERNATE_FUNCTION); - Pin::inscribe(pin_negated, TIMER_ALTERNATE_FUNCTION); - timer.init_data.pwm_channels.push_back(pwm_data); - - duty_cycle = 0; + if (not TimerPeripheral::available_dual_pwms.contains({pin, pin_negated})) { + ErrorHandler( + "Pins %s and %s are not registered as an available Dual PWM", + pin.to_string(), + pin_negated.to_string() + ); + } + + TimerPeripheral& timer = TimerPeripheral::available_dual_pwms.at({pin, pin_negated}).first; + TimerPeripheral::PWMData& pwm_data = + TimerPeripheral::available_dual_pwms.at({pin, pin_negated}).second; + + peripheral = &timer; + channel = pwm_data.channel; + + if (pwm_data.mode != TimerPeripheral::PWM_MODE::NORMAL) { + ErrorHandler( + "Pins %s and %s are not registered as a DUAL PWM", + pin.to_string(), + pin_negated.to_string() + ); + } + + Pin::inscribe(pin, TIMER_ALTERNATE_FUNCTION); + Pin::inscribe(pin_negated, TIMER_ALTERNATE_FUNCTION); + timer.init_data.pwm_channels.push_back(pwm_data); + + duty_cycle = 0; } void DualPWM::turn_on() { - if (HAL_TIM_PWM_Start(peripheral->handle, channel) != HAL_OK) { - ErrorHandler("Dual PWM positive channel did not start correctly", 0); - } + if (HAL_TIM_PWM_Start(peripheral->handle, channel) != HAL_OK) { + ErrorHandler("Dual PWM positive channel did not start correctly", 0); + } - if (HAL_TIMEx_PWMN_Start(peripheral->handle, channel) != HAL_OK) { - ErrorHandler("Dual PWM negative channel did not start correctly", 0); - } + if (HAL_TIMEx_PWMN_Start(peripheral->handle, channel) != HAL_OK) { + ErrorHandler("Dual PWM negative channel did not start correctly", 0); + } } void DualPWM::turn_off() { - if (HAL_TIM_PWM_Stop(peripheral->handle, channel) != HAL_OK) { - ErrorHandler("Dual PWM positive channel did not stop correctly", 0); - } + if (HAL_TIM_PWM_Stop(peripheral->handle, channel) != HAL_OK) { + ErrorHandler("Dual PWM positive channel did not stop correctly", 0); + } - if (HAL_TIMEx_PWMN_Stop(peripheral->handle, channel) != HAL_OK) { - ErrorHandler("Dual PWM negative channel did not stop correctly", 0); - } + if (HAL_TIMEx_PWMN_Stop(peripheral->handle, channel) != HAL_OK) { + ErrorHandler("Dual PWM negative channel did not stop correctly", 0); + } } void DualPWM::turn_on_positive() { - if (HAL_TIM_PWM_Start(peripheral->handle, channel) != HAL_OK) { - ErrorHandler("Dual PWM positive channel did not start correctly", 0); - } + if (HAL_TIM_PWM_Start(peripheral->handle, channel) != HAL_OK) { + ErrorHandler("Dual PWM positive channel did not start correctly", 0); + } } void DualPWM::turn_on_negated() { - if (HAL_TIMEx_PWMN_Start(peripheral->handle, channel) != HAL_OK) { - ErrorHandler("Dual PWM negative channel did not start correctly", 0); - } + if (HAL_TIMEx_PWMN_Start(peripheral->handle, channel) != HAL_OK) { + ErrorHandler("Dual PWM negative channel did not start correctly", 0); + } } void DualPWM::turn_off_positive() { - if (HAL_TIM_PWM_Stop(peripheral->handle, channel) != HAL_OK) { - ErrorHandler("Dual PWM positive channel did not stop correctly", 0); - } + if (HAL_TIM_PWM_Stop(peripheral->handle, channel) != HAL_OK) { + ErrorHandler("Dual PWM positive channel did not stop correctly", 0); + } } void DualPWM::turn_off_negated() { - if (HAL_TIMEx_PWMN_Stop(peripheral->handle, channel) != HAL_OK) { - ErrorHandler("Dual PWM negative channel did not stop correctly", 0); - } + if (HAL_TIMEx_PWMN_Stop(peripheral->handle, channel) != HAL_OK) { + ErrorHandler("Dual PWM negative channel did not stop correctly", 0); + } } -void DualPWM::set_duty_cycle(float duty_cycle){ - uint16_t raw_duty = __HAL_TIM_GET_AUTORELOAD(peripheral->handle) / 100.0 * duty_cycle; - __HAL_TIM_SET_COMPARE(peripheral->handle, channel, raw_duty); - this->duty_cycle = duty_cycle; +void DualPWM::set_duty_cycle(float duty_cycle) { + uint16_t raw_duty = __HAL_TIM_GET_AUTORELOAD(peripheral->handle) / 100.0 * duty_cycle; + __HAL_TIM_SET_COMPARE(peripheral->handle, channel, raw_duty); + this->duty_cycle = duty_cycle; } -void DualPWM::set_frequency(uint32_t freq_in_hz){ - this->frequency = freq_in_hz; - TIM_TypeDef& timer = *peripheral->handle->Instance; - timer.ARR = (HAL_RCC_GetPCLK1Freq()*2 / (timer.PSC+1)) / frequency; - set_duty_cycle(duty_cycle); +void DualPWM::set_frequency(uint32_t freq_in_hz) { + this->frequency = freq_in_hz; + TIM_TypeDef& timer = *peripheral->handle->Instance; + timer.ARR = (HAL_RCC_GetPCLK1Freq() * 2 / (timer.PSC + 1)) / frequency; + set_duty_cycle(duty_cycle); } -uint32_t DualPWM::get_frequency()const{ - return frequency; +uint32_t DualPWM::get_frequency() const { return frequency; } +float DualPWM::get_duty_cycle() const { return duty_cycle; } +void DualPWM::set_dead_time(std::chrono::nanoseconds dead_time_ns) { + TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; + + // per https://hasanyavuz.ozderya.net/?p=437 + auto time = dead_time_ns.count(); + + if (time <= 127 * clock_period_ns) { + sBreakDeadTimeConfig.DeadTime = time / clock_period_ns; + } else if (time > 127 * clock_period_ns && time <= 2 * clock_period_ns * 127) { + sBreakDeadTimeConfig.DeadTime = time / (2 * clock_period_ns) - 64 + 128; + } else if (time > 2 * clock_period_ns * 127 && time <= 8 * clock_period_ns * 127) { + sBreakDeadTimeConfig.DeadTime = time / (8 * clock_period_ns) - 32 + 192; + } else if (time > 8 * clock_period_ns * 127 && time <= 16 * clock_period_ns * 127) { + sBreakDeadTimeConfig.DeadTime = time / (16 * clock_period_ns) - 32 + 224; + } else { + ErrorHandler("Invalid dead time configuration"); + } + // sBreakDeadTimeConfig.LockLevel = 0; + // sBreakDeadTimeConfig.BreakState = 1; + HAL_TIMEx_ConfigBreakDeadTime(peripheral->handle, &sBreakDeadTimeConfig); + peripheral->handle->Instance->BDTR |= TIM_BDTR_MOE; + return; } -float DualPWM::get_duty_cycle()const{ - return duty_cycle; -} -void DualPWM::set_dead_time(std::chrono::nanoseconds dead_time_ns) -{ - TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; - - // per https://hasanyavuz.ozderya.net/?p=437 - auto time = dead_time_ns.count(); - - if(time <= 127 * clock_period_ns){ - sBreakDeadTimeConfig.DeadTime = time/clock_period_ns; - }else if (time >127 * clock_period_ns && time <= 2 * clock_period_ns * 127) - { - sBreakDeadTimeConfig.DeadTime = time /(2 * clock_period_ns) - 64 + 128; - }else if(time > 2 * clock_period_ns * 127 && time <= 8 * clock_period_ns * 127){ - sBreakDeadTimeConfig.DeadTime = time/(8 * clock_period_ns) -32 + 192; - }else if(time > 8 * clock_period_ns * 127 && time <=16 * clock_period_ns*127){ - sBreakDeadTimeConfig.DeadTime = time/(16 * clock_period_ns) -32 + 224; - }else{ - ErrorHandler("Invalid dead time configuration"); - } - //sBreakDeadTimeConfig.LockLevel = 0; - //sBreakDeadTimeConfig.BreakState = 1; - HAL_TIMEx_ConfigBreakDeadTime(peripheral->handle,&sBreakDeadTimeConfig); - peripheral->handle->Instance->BDTR |= TIM_BDTR_MOE; - return; - -} - diff --git a/Src/HALAL/Services/PWM/DualPhasedPWM/DualPhasedPWM.cpp b/Src/HALAL/Services/PWM/DualPhasedPWM/DualPhasedPWM.cpp index 667dfd391..4da250d02 100644 --- a/Src/HALAL/Services/PWM/DualPhasedPWM/DualPhasedPWM.cpp +++ b/Src/HALAL/Services/PWM/DualPhasedPWM/DualPhasedPWM.cpp @@ -8,93 +8,101 @@ #include "HALAL/Services/PWM/DualPhasedPWM/DualPhasedPWM.hpp" DualPhasedPWM::DualPhasedPWM(Pin& pin, Pin& pin_negated) { - if (not TimerPeripheral::available_dual_pwms.contains({pin, pin_negated})) { - ErrorHandler("Pins %s and %s are not registered as an available Dual PHASED PWM", pin.to_string(), pin_negated.to_string()); - } - - TimerPeripheral& timer = TimerPeripheral::available_dual_pwms.at({pin, pin_negated}).first; - TimerPeripheral::PWMData& pwm_data = TimerPeripheral::available_dual_pwms.at({pin, pin_negated}).second; - - peripheral = &timer; - channel = pwm_data.channel; - - if (pwm_data.mode != TimerPeripheral::PWM_MODE::PHASED) { - ErrorHandler("Pins %s and %s are not registered as a DUAL PHASED PWM", pin.to_string(), pin_negated.to_string()); - } - - Pin::inscribe(pin, TIMER_ALTERNATE_FUNCTION); - Pin::inscribe(pin_negated, TIMER_ALTERNATE_FUNCTION); - timer.init_data.pwm_channels.push_back(pwm_data); - - duty_cycle = 0; - raw_phase = 0; + if (not TimerPeripheral::available_dual_pwms.contains({pin, pin_negated})) { + ErrorHandler( + "Pins %s and %s are not registered as an available Dual PHASED PWM", + pin.to_string(), + pin_negated.to_string() + ); + } + + TimerPeripheral& timer = TimerPeripheral::available_dual_pwms.at({pin, pin_negated}).first; + TimerPeripheral::PWMData& pwm_data = + TimerPeripheral::available_dual_pwms.at({pin, pin_negated}).second; + + peripheral = &timer; + channel = pwm_data.channel; + + if (pwm_data.mode != TimerPeripheral::PWM_MODE::PHASED) { + ErrorHandler( + "Pins %s and %s are not registered as a DUAL PHASED PWM", + pin.to_string(), + pin_negated.to_string() + ); + } + + Pin::inscribe(pin, TIMER_ALTERNATE_FUNCTION); + Pin::inscribe(pin_negated, TIMER_ALTERNATE_FUNCTION); + timer.init_data.pwm_channels.push_back(pwm_data); + + duty_cycle = 0; + raw_phase = 0; } - -void DualPhasedPWM::set_duty_cycle(float duty_cycle){ - this->duty_cycle = duty_cycle; - if(raw_phase > 100.0){ - duty_cycle = 100.0 - duty_cycle; - raw_phase = raw_phase - 100.0; - __STLIB_TIM_SET_MODE(peripheral->handle, channel, STLIB_TIMER_CCMR_PWM_MODE_1) - }else{ - __STLIB_TIM_SET_MODE(peripheral->handle, channel, STLIB_TIMER_CCMR_PWM_MODE_2) - } - uint32_t arr = peripheral->handle->Instance->ARR; - float start_high = arr*(50.0 - duty_cycle)/50.0; - float end_high = arr*(100.0 - duty_cycle)/50.0 + 1; - if(start_high < 0){start_high = 0;} - if(end_high > arr){end_high = arr;} - float max_range = duty_cycle > 50.0 ? 100 - duty_cycle : duty_cycle; - start_high = start_high + arr * max_range * raw_phase / 5000.0; - end_high = end_high - arr * max_range * raw_phase / 5000.0; - - - peripheral->handle->Instance->CR2 &= ~TIM_CR2_CCPC; - peripheral->handle->Instance->CR2 &= ~TIM_CR2_CCUS; - - __HAL_TIM_SET_COMPARE(peripheral->handle, channel, start_high); - - if (channel % 8 == 0) { - __HAL_TIM_SET_COMPARE(peripheral->handle, channel + 4, end_high); - peripheral->handle->Instance->EGR |= TIM_EGR_UG; - peripheral->handle->Instance->CR2 |= TIM_CR2_CCPC; - peripheral->handle->Instance->CR2 |= TIM_CR2_CCUS; - TIM_CCxChannelCmd(peripheral->handle->Instance, channel, TIM_CCx_ENABLE); - TIM_CCxChannelCmd(peripheral->handle->Instance, channel+4, TIM_CCx_ENABLE); - __HAL_TIM_MOE_ENABLE(peripheral->handle); - } else { - __HAL_TIM_SET_COMPARE(peripheral->handle, channel - 4, end_high); - peripheral->handle->Instance->EGR |= TIM_EGR_UG; - peripheral->handle->Instance->CR2 |= TIM_CR2_CCPC; - peripheral->handle->Instance->CR2 |= TIM_CR2_CCUS; - TIM_CCxChannelCmd(peripheral->handle->Instance, channel, TIM_CCx_ENABLE); - TIM_CCxChannelCmd(peripheral->handle->Instance, channel-4, TIM_CCx_ENABLE); - __HAL_TIM_MOE_ENABLE(peripheral->handle); - } +void DualPhasedPWM::set_duty_cycle(float duty_cycle) { + this->duty_cycle = duty_cycle; + if (raw_phase > 100.0) { + duty_cycle = 100.0 - duty_cycle; + raw_phase = raw_phase - 100.0; + __STLIB_TIM_SET_MODE(peripheral->handle, channel, STLIB_TIMER_CCMR_PWM_MODE_1) + } else { + __STLIB_TIM_SET_MODE(peripheral->handle, channel, STLIB_TIMER_CCMR_PWM_MODE_2) + } + uint32_t arr = peripheral->handle->Instance->ARR; + float start_high = arr * (50.0 - duty_cycle) / 50.0; + float end_high = arr * (100.0 - duty_cycle) / 50.0 + 1; + if (start_high < 0) { + start_high = 0; + } + if (end_high > arr) { + end_high = arr; + } + float max_range = duty_cycle > 50.0 ? 100 - duty_cycle : duty_cycle; + start_high = start_high + arr * max_range * raw_phase / 5000.0; + end_high = end_high - arr * max_range * raw_phase / 5000.0; + + peripheral->handle->Instance->CR2 &= ~TIM_CR2_CCPC; + peripheral->handle->Instance->CR2 &= ~TIM_CR2_CCUS; + + __HAL_TIM_SET_COMPARE(peripheral->handle, channel, start_high); + + if (channel % 8 == 0) { + __HAL_TIM_SET_COMPARE(peripheral->handle, channel + 4, end_high); + peripheral->handle->Instance->EGR |= TIM_EGR_UG; + peripheral->handle->Instance->CR2 |= TIM_CR2_CCPC; + peripheral->handle->Instance->CR2 |= TIM_CR2_CCUS; + TIM_CCxChannelCmd(peripheral->handle->Instance, channel, TIM_CCx_ENABLE); + TIM_CCxChannelCmd(peripheral->handle->Instance, channel + 4, TIM_CCx_ENABLE); + __HAL_TIM_MOE_ENABLE(peripheral->handle); + } else { + __HAL_TIM_SET_COMPARE(peripheral->handle, channel - 4, end_high); + peripheral->handle->Instance->EGR |= TIM_EGR_UG; + peripheral->handle->Instance->CR2 |= TIM_CR2_CCPC; + peripheral->handle->Instance->CR2 |= TIM_CR2_CCUS; + TIM_CCxChannelCmd(peripheral->handle->Instance, channel, TIM_CCx_ENABLE); + TIM_CCxChannelCmd(peripheral->handle->Instance, channel - 4, TIM_CCx_ENABLE); + __HAL_TIM_MOE_ENABLE(peripheral->handle); + } } - -void DualPhasedPWM::set_frequency(uint32_t freq_in_hz){ - this->frequency = freq_in_hz; - TIM_TypeDef& timer = *peripheral->handle->Instance; - timer.ARR = (HAL_RCC_GetPCLK1Freq()*2 / (timer.PSC+1)) / 2 / frequency; - set_duty_cycle(duty_cycle); +void DualPhasedPWM::set_frequency(uint32_t freq_in_hz) { + this->frequency = freq_in_hz; + TIM_TypeDef& timer = *peripheral->handle->Instance; + timer.ARR = (HAL_RCC_GetPCLK1Freq() * 2 / (timer.PSC + 1)) / 2 / frequency; + set_duty_cycle(duty_cycle); } void DualPhasedPWM::set_phase(float phase_in_deg) { - if(duty_cycle == 50.0){ - this->raw_phase = phase_in_deg*(200/360); - }else{ - //TODO - } - set_duty_cycle(duty_cycle); + if (duty_cycle == 50.0) { + this->raw_phase = phase_in_deg * (200 / 360); + } else { + // TODO + } + set_duty_cycle(duty_cycle); } void DualPhasedPWM::set_raw_phase(float raw_phase) { - this->raw_phase = raw_phase; - set_duty_cycle(duty_cycle); + this->raw_phase = raw_phase; + set_duty_cycle(duty_cycle); } -float DualPhasedPWM::get_phase()const{ - return raw_phase*360/200; -} +float DualPhasedPWM::get_phase() const { return raw_phase * 360 / 200; } diff --git a/Src/HALAL/Services/PWM/PWM/PWM.cpp b/Src/HALAL/Services/PWM/PWM/PWM.cpp index ed9c95588..6874578b8 100644 --- a/Src/HALAL/Services/PWM/PWM/PWM.cpp +++ b/Src/HALAL/Services/PWM/PWM/PWM.cpp @@ -10,107 +10,102 @@ #include "stm32h7xx_hal_def.h" PWM::PWM(Pin& pin) { - if (not TimerPeripheral::available_pwm.contains(pin)) { + if (not TimerPeripheral::available_pwm.contains(pin)) { - ErrorHandler("Pin %s is not registered as an available PWM", pin.to_string()); - return; - } + ErrorHandler("Pin %s is not registered as an available PWM", pin.to_string()); + return; + } - TimerPeripheral& timer = TimerPeripheral::available_pwm.at(pin).first; - TimerPeripheral::PWMData& pwm_data = TimerPeripheral::available_pwm.at(pin).second; + TimerPeripheral& timer = TimerPeripheral::available_pwm.at(pin).first; + TimerPeripheral::PWMData& pwm_data = TimerPeripheral::available_pwm.at(pin).second; if (pwm_data.mode == TimerPeripheral::PWM_MODE::CENTER_ALIGNED) { - is_center_aligned = true; - } - if(pwm_data.mode == TimerPeripheral::PWM_MODE::PHASED){ - ErrorHandler("Pin %s is registered as Phased being a single PWM", pin.to_string()); - } + is_center_aligned = true; + } + if (pwm_data.mode == TimerPeripheral::PWM_MODE::PHASED) { + ErrorHandler("Pin %s is registered as Phased being a single PWM", pin.to_string()); + } - peripheral = &timer; - channel = pwm_data.channel; + peripheral = &timer; + channel = pwm_data.channel; - Pin::inscribe(pin, TIMER_ALTERNATE_FUNCTION); - timer.init_data.pwm_channels.push_back(pwm_data); + Pin::inscribe(pin, TIMER_ALTERNATE_FUNCTION); + timer.init_data.pwm_channels.push_back(pwm_data); - duty_cycle = 0; - is_initialized = true; + duty_cycle = 0; + is_initialized = true; } void PWM::turn_on() { - if(not is_initialized){ - ErrorHandler("PWM was not initialized"); - } - if(is_on) return; - if(HAL_TIM_PWM_Start(peripheral->handle, channel) != HAL_OK) { - ErrorHandler("PWM did not turn on correctly", 0); - } - is_on = true; + if (not is_initialized) { + ErrorHandler("PWM was not initialized"); + } + if (is_on) + return; + if (HAL_TIM_PWM_Start(peripheral->handle, channel) != HAL_OK) { + ErrorHandler("PWM did not turn on correctly", 0); + } + is_on = true; } void PWM::turn_off() { - if(not is_on) return; - if(HAL_TIM_PWM_Stop(peripheral->handle, channel) != HAL_OK) { - ErrorHandler("PWM did not stop correctly", 0); - } - is_on = false; + if (not is_on) + return; + if (HAL_TIM_PWM_Stop(peripheral->handle, channel) != HAL_OK) { + ErrorHandler("PWM did not stop correctly", 0); + } + is_on = false; } void PWM::set_duty_cycle(float duty_cycle) { - uint16_t raw_duty = __HAL_TIM_GET_AUTORELOAD(peripheral->handle) / 100.0 * duty_cycle; - __HAL_TIM_SET_COMPARE(peripheral->handle, channel, raw_duty); - this->duty_cycle = duty_cycle; + uint16_t raw_duty = __HAL_TIM_GET_AUTORELOAD(peripheral->handle) / 100.0 * duty_cycle; + __HAL_TIM_SET_COMPARE(peripheral->handle, channel, raw_duty); + this->duty_cycle = duty_cycle; } void PWM::set_frequency(uint32_t frequency) { - if(is_center_aligned){ - frequency = 2*frequency; - } - this->frequency = frequency; - TIM_TypeDef& timer = *peripheral->handle->Instance; + if (is_center_aligned) { + frequency = 2 * frequency; + } + this->frequency = frequency; + TIM_TypeDef& timer = *peripheral->handle->Instance; timer.ARR = (HAL_RCC_GetPCLK1Freq() * 2 / (timer.PSC + 1)) / frequency; - set_duty_cycle(duty_cycle); + set_duty_cycle(duty_cycle); } -uint32_t PWM::get_frequency() { - return frequency; -} - -float PWM::get_duty_cycle(){ - return duty_cycle; -} -void PWM::set_dead_time(std::chrono::nanoseconds dead_time_ns) -{ - TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; - - // per https://hasanyavuz.ozderya.net/?p=437 - auto time = dead_time_ns.count(); - - if(time <= 127 * clock_period_ns){ - sBreakDeadTimeConfig.DeadTime = time/clock_period_ns; - }else if (time >127 * clock_period_ns && time <= 2 * clock_period_ns * 127) - { - sBreakDeadTimeConfig.DeadTime = time /(2 * clock_period_ns) - 64 + 128; - }else if(time > 2 * clock_period_ns * 127 && time <= 8 * clock_period_ns * 127){ - sBreakDeadTimeConfig.DeadTime = time/(8 * clock_period_ns) -32 + 192; - }else if(time > 8 * clock_period_ns * 127 && time <=16 * clock_period_ns*127){ - sBreakDeadTimeConfig.DeadTime = time/(16 * clock_period_ns) -32 + 224; - }else{ - ErrorHandler("Invalid dead time configuration"); - } - - sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; - sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; - sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; - sBreakDeadTimeConfig.BreakState = TIM_BREAK_ENABLE; - sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; - sBreakDeadTimeConfig.BreakFilter = 0; - sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE; - sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH; - sBreakDeadTimeConfig.Break2Filter = 0; - sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; - HAL_TIMEx_ConfigBreakDeadTime(peripheral->handle,&sBreakDeadTimeConfig); - peripheral->handle->Instance->BDTR |= TIM_BDTR_MOE; - - return; - +uint32_t PWM::get_frequency() { return frequency; } + +float PWM::get_duty_cycle() { return duty_cycle; } +void PWM::set_dead_time(std::chrono::nanoseconds dead_time_ns) { + TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0}; + + // per https://hasanyavuz.ozderya.net/?p=437 + auto time = dead_time_ns.count(); + + if (time <= 127 * clock_period_ns) { + sBreakDeadTimeConfig.DeadTime = time / clock_period_ns; + } else if (time > 127 * clock_period_ns && time <= 2 * clock_period_ns * 127) { + sBreakDeadTimeConfig.DeadTime = time / (2 * clock_period_ns) - 64 + 128; + } else if (time > 2 * clock_period_ns * 127 && time <= 8 * clock_period_ns * 127) { + sBreakDeadTimeConfig.DeadTime = time / (8 * clock_period_ns) - 32 + 192; + } else if (time > 8 * clock_period_ns * 127 && time <= 16 * clock_period_ns * 127) { + sBreakDeadTimeConfig.DeadTime = time / (16 * clock_period_ns) - 32 + 224; + } else { + ErrorHandler("Invalid dead time configuration"); + } + + sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; + sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; + sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; + sBreakDeadTimeConfig.BreakState = TIM_BREAK_ENABLE; + sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; + sBreakDeadTimeConfig.BreakFilter = 0; + sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE; + sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH; + sBreakDeadTimeConfig.Break2Filter = 0; + sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; + HAL_TIMEx_ConfigBreakDeadTime(peripheral->handle, &sBreakDeadTimeConfig); + peripheral->handle->Instance->BDTR |= TIM_BDTR_MOE; + + return; } diff --git a/Src/HALAL/Services/PWM/PhasedPWM/PhasedPWM.cpp b/Src/HALAL/Services/PWM/PhasedPWM/PhasedPWM.cpp index 4ea66b9a6..62cbb914d 100644 --- a/Src/HALAL/Services/PWM/PhasedPWM/PhasedPWM.cpp +++ b/Src/HALAL/Services/PWM/PhasedPWM/PhasedPWM.cpp @@ -8,118 +8,119 @@ #include "HALAL/Services/PWM/PhasedPWM/PhasedPWM.hpp" /** - * The function initializes a PhasedPWM object with a given pin and sets its duty cycle and phase to 0. - * + * The function initializes a PhasedPWM object with a given pin and sets its duty cycle and phase to + * 0. + * * @param pin The pin to which the PhasedPWM object is being attached. */ PhasedPWM::PhasedPWM(Pin& pin) { - if (not TimerPeripheral::available_pwm.contains(pin)) { - ErrorHandler("Pin %s is not registered as an available PWM", pin.to_string()); - return; - } + if (not TimerPeripheral::available_pwm.contains(pin)) { + ErrorHandler("Pin %s is not registered as an available PWM", pin.to_string()); + return; + } - TimerPeripheral& timer = TimerPeripheral::available_pwm.at(pin).first; - TimerPeripheral::PWMData& pwm_data = TimerPeripheral::available_pwm.at(pin).second; + TimerPeripheral& timer = TimerPeripheral::available_pwm.at(pin).first; + TimerPeripheral::PWMData& pwm_data = TimerPeripheral::available_pwm.at(pin).second; - if (pwm_data.mode != TimerPeripheral::PWM_MODE::PHASED) { - ErrorHandler("Pin %s is not registered as a PHASED PWM", pin.to_string()); - } + if (pwm_data.mode != TimerPeripheral::PWM_MODE::PHASED) { + ErrorHandler("Pin %s is not registered as a PHASED PWM", pin.to_string()); + } - peripheral = &timer; - channel = pwm_data.channel; + peripheral = &timer; + channel = pwm_data.channel; - Pin::inscribe(pin, TIMER_ALTERNATE_FUNCTION); - timer.init_data.pwm_channels.push_back(pwm_data); + Pin::inscribe(pin, TIMER_ALTERNATE_FUNCTION); + timer.init_data.pwm_channels.push_back(pwm_data); - duty_cycle = 0; - raw_phase = 0; - is_initialized = true; + duty_cycle = 0; + raw_phase = 0; + is_initialized = true; } /** - * This function sets the duty cycle of a PWM signal and calculates the raw duty and phase values based - * on the input duty cycle and phase. - * - * @param duty_cycle The duty cycle is a value between 0 and 100 that represents the percentage of time - * that the PWM signal is high compared to the total period of the signal. + * This function sets the duty cycle of a PWM signal and calculates the raw duty and phase values + * based on the input duty cycle and phase. + * + * @param duty_cycle The duty cycle is a value between 0 and 100 that represents the percentage of + * time that the PWM signal is high compared to the total period of the signal. */ void PhasedPWM::set_duty_cycle(float duty_cycle) { - this->duty_cycle = duty_cycle; - if(raw_phase > 100.0){ - duty_cycle = 100.0 - duty_cycle; - raw_phase = raw_phase - 100.0; - __STLIB_TIM_SET_MODE(peripheral->handle, channel, STLIB_TIMER_CCMR_PWM_MODE_1) - }else{ - __STLIB_TIM_SET_MODE(peripheral->handle, channel, STLIB_TIMER_CCMR_PWM_MODE_2) - } - uint32_t arr = peripheral->handle->Instance->ARR; - float start_high = arr*(50.0 - duty_cycle)/50.0; - float end_high = arr*(100.0 - duty_cycle)/50.0 + 1; - if(start_high < 0){start_high = 0;} - if(end_high > arr){end_high = arr;} - float max_range = duty_cycle > 50.0 ? 100 - duty_cycle : duty_cycle; - start_high = start_high + arr * max_range * raw_phase / 5000.0; - end_high = end_high - arr * max_range * raw_phase / 5000.0; - - - peripheral->handle->Instance->CR2 &= ~TIM_CR2_CCPC; - peripheral->handle->Instance->CR2 &= ~TIM_CR2_CCUS; - - __HAL_TIM_SET_COMPARE(peripheral->handle, channel, start_high); - - if (channel % 8 == 0) { - __HAL_TIM_SET_COMPARE(peripheral->handle, channel + 4, end_high); - peripheral->handle->Instance->EGR |= TIM_EGR_UG; - peripheral->handle->Instance->CR2 |= TIM_CR2_CCPC; - peripheral->handle->Instance->CR2 |= TIM_CR2_CCUS; - TIM_CCxChannelCmd(peripheral->handle->Instance, channel, TIM_CCx_ENABLE); - TIM_CCxChannelCmd(peripheral->handle->Instance, channel + 4, TIM_CCx_ENABLE); - __HAL_TIM_MOE_ENABLE(peripheral->handle); - } else { - __HAL_TIM_SET_COMPARE(peripheral->handle, channel - 4, end_high); - peripheral->handle->Instance->EGR |= TIM_EGR_UG; - peripheral->handle->Instance->CR2 |= TIM_CR2_CCPC; - peripheral->handle->Instance->CR2 |= TIM_CR2_CCUS; - TIM_CCxChannelCmd(peripheral->handle->Instance, channel, TIM_CCx_ENABLE); - TIM_CCxChannelCmd(peripheral->handle->Instance, channel - 4, TIM_CCx_ENABLE); - __HAL_TIM_MOE_ENABLE(peripheral->handle); - } + this->duty_cycle = duty_cycle; + if (raw_phase > 100.0) { + duty_cycle = 100.0 - duty_cycle; + raw_phase = raw_phase - 100.0; + __STLIB_TIM_SET_MODE(peripheral->handle, channel, STLIB_TIMER_CCMR_PWM_MODE_1) + } else { + __STLIB_TIM_SET_MODE(peripheral->handle, channel, STLIB_TIMER_CCMR_PWM_MODE_2) + } + uint32_t arr = peripheral->handle->Instance->ARR; + float start_high = arr * (50.0 - duty_cycle) / 50.0; + float end_high = arr * (100.0 - duty_cycle) / 50.0 + 1; + if (start_high < 0) { + start_high = 0; + } + if (end_high > arr) { + end_high = arr; + } + float max_range = duty_cycle > 50.0 ? 100 - duty_cycle : duty_cycle; + start_high = start_high + arr * max_range * raw_phase / 5000.0; + end_high = end_high - arr * max_range * raw_phase / 5000.0; + + peripheral->handle->Instance->CR2 &= ~TIM_CR2_CCPC; + peripheral->handle->Instance->CR2 &= ~TIM_CR2_CCUS; + + __HAL_TIM_SET_COMPARE(peripheral->handle, channel, start_high); + + if (channel % 8 == 0) { + __HAL_TIM_SET_COMPARE(peripheral->handle, channel + 4, end_high); + peripheral->handle->Instance->EGR |= TIM_EGR_UG; + peripheral->handle->Instance->CR2 |= TIM_CR2_CCPC; + peripheral->handle->Instance->CR2 |= TIM_CR2_CCUS; + TIM_CCxChannelCmd(peripheral->handle->Instance, channel, TIM_CCx_ENABLE); + TIM_CCxChannelCmd(peripheral->handle->Instance, channel + 4, TIM_CCx_ENABLE); + __HAL_TIM_MOE_ENABLE(peripheral->handle); + } else { + __HAL_TIM_SET_COMPARE(peripheral->handle, channel - 4, end_high); + peripheral->handle->Instance->EGR |= TIM_EGR_UG; + peripheral->handle->Instance->CR2 |= TIM_CR2_CCPC; + peripheral->handle->Instance->CR2 |= TIM_CR2_CCUS; + TIM_CCxChannelCmd(peripheral->handle->Instance, channel, TIM_CCx_ENABLE); + TIM_CCxChannelCmd(peripheral->handle->Instance, channel - 4, TIM_CCx_ENABLE); + __HAL_TIM_MOE_ENABLE(peripheral->handle); + } } /** * This function sets the frequency of a PWM signal using a timer in a microcontroller. - * + * * @param frequency The desired frequency of the PWM signal in Hertz (Hz). */ void PhasedPWM::set_frequency(uint32_t frequency) { - TIM_TypeDef& timer = *peripheral->handle->Instance; - timer.ARR = (HAL_RCC_GetPCLK1Freq()*2 / (timer.PSC+1)) / 2 / frequency; - this->frequency = frequency; - set_duty_cycle(duty_cycle); + TIM_TypeDef& timer = *peripheral->handle->Instance; + timer.ARR = (HAL_RCC_GetPCLK1Freq() * 2 / (timer.PSC + 1)) / 2 / frequency; + this->frequency = frequency; + set_duty_cycle(duty_cycle); } /** * This function sets the phase of a PhasedPWM object and updates the duty cycle accordingly. - * + * * @param phase The "phase" parameter is a floating-point value that represents the phase shift of a - * PWM signal. In other words, it determines the timing offset of the PWM waveform relative to its center. - * Only works with duty cycle = 50 for now. + * PWM signal. In other words, it determines the timing offset of the PWM waveform relative to its + * center. Only works with duty cycle = 50 for now. */ void PhasedPWM::set_phase(float phase_in_deg) { - if(duty_cycle == 50.0){ - this->raw_phase = phase_in_deg*(200.0/360.0); - }else{ - //TODO - } - set_duty_cycle(duty_cycle); + if (duty_cycle == 50.0) { + this->raw_phase = phase_in_deg * (200.0 / 360.0); + } else { + // TODO + } + set_duty_cycle(duty_cycle); } -void PhasedPWM::set_raw_phase(float raw_phase){ - this->raw_phase = raw_phase; - set_duty_cycle(duty_cycle); - +void PhasedPWM::set_raw_phase(float raw_phase) { + this->raw_phase = raw_phase; + set_duty_cycle(duty_cycle); } -float PhasedPWM::get_phase()const{ - return raw_phase*360/200; -} +float PhasedPWM::get_phase() const { return raw_phase * 360 / 200; } diff --git a/Src/HALAL/Services/Time/RTC.cpp b/Src/HALAL/Services/Time/RTC.cpp index 8e7ee65de..e4d82ff75 100644 --- a/Src/HALAL/Services/Time/RTC.cpp +++ b/Src/HALAL/Services/Time/RTC.cpp @@ -3,82 +3,84 @@ RTC_HandleTypeDef hrtc; RTCData Global_RTC::global_RTC; -void Global_RTC::start_rtc(){ - RTC_TimeTypeDef sTime = { 0 }; - RTC_DateTypeDef sDate = { 0 }; +void Global_RTC::start_rtc() { + RTC_TimeTypeDef sTime = {0}; + RTC_DateTypeDef sDate = {0}; - hrtc.Instance = RTC; - hrtc.Init.HourFormat = RTC_HOURFORMAT_24; - hrtc.Init.AsynchPrediv = 0; - hrtc.Init.SynchPrediv = 32767; - hrtc.Init.OutPut = RTC_OUTPUT_DISABLE; - hrtc.Init.OutPutPolarity = RTC_OUTPUT_POLARITY_HIGH; - hrtc.Init.OutPutType = RTC_OUTPUT_TYPE_OPENDRAIN; - hrtc.Init.OutPutRemap = RTC_OUTPUT_REMAP_NONE; + hrtc.Instance = RTC; + hrtc.Init.HourFormat = RTC_HOURFORMAT_24; + hrtc.Init.AsynchPrediv = 0; + hrtc.Init.SynchPrediv = 32767; + hrtc.Init.OutPut = RTC_OUTPUT_DISABLE; + hrtc.Init.OutPutPolarity = RTC_OUTPUT_POLARITY_HIGH; + hrtc.Init.OutPutType = RTC_OUTPUT_TYPE_OPENDRAIN; + hrtc.Init.OutPutRemap = RTC_OUTPUT_REMAP_NONE; - if (HAL_RTC_Init(&hrtc) != HAL_OK) { - ErrorHandler("Error on RTC Init"); - } - sTime.Hours = 0x0; - sTime.Minutes = 0x0; - sTime.Seconds = 0x0; - sTime.DayLightSaving = RTC_DAYLIGHTSAVING_NONE; - sTime.StoreOperation = RTC_STOREOPERATION_RESET; + if (HAL_RTC_Init(&hrtc) != HAL_OK) { + ErrorHandler("Error on RTC Init"); + } + sTime.Hours = 0x0; + sTime.Minutes = 0x0; + sTime.Seconds = 0x0; + sTime.DayLightSaving = RTC_DAYLIGHTSAVING_NONE; + sTime.StoreOperation = RTC_STOREOPERATION_RESET; - if (HAL_RTC_SetTime(&hrtc, &sTime, RTC_FORMAT_BIN) != HAL_OK) { - ErrorHandler("Error while setting time at RTC start"); - } + if (HAL_RTC_SetTime(&hrtc, &sTime, RTC_FORMAT_BIN) != HAL_OK) { + ErrorHandler("Error while setting time at RTC start"); + } - sDate.WeekDay = RTC_WEEKDAY_MONDAY; - sDate.Month = RTC_MONTH_JANUARY; - sDate.Date = 0x1; - sDate.Year = 23; + sDate.WeekDay = RTC_WEEKDAY_MONDAY; + sDate.Month = RTC_MONTH_JANUARY; + sDate.Date = 0x1; + sDate.Year = 23; - if (HAL_RTC_SetDate(&hrtc, &sDate, RTC_FORMAT_BIN) != HAL_OK) { - ErrorHandler("Error while setting date at RTC start"); - } + if (HAL_RTC_SetDate(&hrtc, &sDate, RTC_FORMAT_BIN) != HAL_OK) { + ErrorHandler("Error while setting date at RTC start"); + } } - -RTCData Global_RTC::get_rtc_timestamp(){ - RTCData ret; - RTC_TimeTypeDef gTime; - RTC_DateTypeDef gDate; - HAL_RTC_GetTime(&hrtc, &gTime, RTC_FORMAT_BIN); - HAL_RTC_GetDate(&hrtc, &gDate, RTC_FORMAT_BIN); - ret.counter = gTime.SecondFraction - gTime.SubSeconds; - ret.second = gTime.Seconds; - ret.minute = gTime.Minutes; - ret.hour = gTime.Hours; - ret.day = gDate.Date; - ret.month = gDate.Month; - ret.year = 2000 + gDate.Year; - return ret; +RTCData Global_RTC::get_rtc_timestamp() { + RTCData ret; + RTC_TimeTypeDef gTime; + RTC_DateTypeDef gDate; + HAL_RTC_GetTime(&hrtc, &gTime, RTC_FORMAT_BIN); + HAL_RTC_GetDate(&hrtc, &gDate, RTC_FORMAT_BIN); + ret.counter = gTime.SecondFraction - gTime.SubSeconds; + ret.second = gTime.Seconds; + ret.minute = gTime.Minutes; + ret.hour = gTime.Hours; + ret.day = gDate.Date; + ret.month = gDate.Month; + ret.year = 2000 + gDate.Year; + return ret; } -void Global_RTC::set_rtc_data(uint16_t counter, uint8_t second, uint8_t minute, uint8_t hour, uint8_t day, uint8_t month, uint16_t year){ - RTC_TimeTypeDef gTime; - RTC_DateTypeDef gDate; - gTime.SubSeconds = counter; - gTime.Seconds = second; - gTime.Minutes = minute; - gTime.Hours = hour; - gTime.DayLightSaving = RTC_DAYLIGHTSAVING_NONE; - gTime.StoreOperation = RTC_STOREOPERATION_RESET; - gDate.WeekDay = 0; - gDate.Date = day; - gDate.Month = month; - gDate.Year = year - 2000; - if (HAL_RTC_SetTime(&hrtc, &gTime, RTC_FORMAT_BIN) != HAL_OK) - { - ErrorHandler("Error on writing Time on the RTC"); - } - if (HAL_RTC_SetDate(&hrtc, &gDate, RTC_FORMAT_BIN) != HAL_OK) - { - ErrorHandler("Error on writing Date on the RTC"); - } - +void Global_RTC::set_rtc_data( + uint16_t counter, + uint8_t second, + uint8_t minute, + uint8_t hour, + uint8_t day, + uint8_t month, + uint16_t year +) { + RTC_TimeTypeDef gTime; + RTC_DateTypeDef gDate; + gTime.SubSeconds = counter; + gTime.Seconds = second; + gTime.Minutes = minute; + gTime.Hours = hour; + gTime.DayLightSaving = RTC_DAYLIGHTSAVING_NONE; + gTime.StoreOperation = RTC_STOREOPERATION_RESET; + gDate.WeekDay = 0; + gDate.Date = day; + gDate.Month = month; + gDate.Year = year - 2000; + if (HAL_RTC_SetTime(&hrtc, &gTime, RTC_FORMAT_BIN) != HAL_OK) { + ErrorHandler("Error on writing Time on the RTC"); + } + if (HAL_RTC_SetDate(&hrtc, &gDate, RTC_FORMAT_BIN) != HAL_OK) { + ErrorHandler("Error on writing Date on the RTC"); + } } -void Global_RTC::update_rtc_data(){ - global_RTC = get_rtc_timestamp(); -} \ No newline at end of file +void Global_RTC::update_rtc_data() { global_RTC = get_rtc_timestamp(); } \ No newline at end of file diff --git a/Src/HALAL/Services/Time/Scheduler.cpp b/Src/HALAL/Services/Time/Scheduler.cpp index 6b3810431..9754d10ee 100644 --- a/Src/HALAL/Services/Time/Scheduler.cpp +++ b/Src/HALAL/Services/Time/Scheduler.cpp @@ -11,15 +11,12 @@ #include /* NOTE(vic): Pido perdón a Boris pero es la mejor manera que se me ha ocurrido hacer esto */ -#define SCHEDULER_RCC_TIMER_ENABLE \ - glue(glue(RCC_APB1LENR_TIM, SCHEDULER_TIMER_IDX), EN) -#define SCHEDULER_GLOBAL_TIMER_IRQn \ - glue(TIM, glue(SCHEDULER_TIMER_IDX, _IRQn)) +#define SCHEDULER_RCC_TIMER_ENABLE glue(glue(RCC_APB1LENR_TIM, SCHEDULER_TIMER_IDX), EN) +#define SCHEDULER_GLOBAL_TIMER_IRQn glue(TIM, glue(SCHEDULER_TIMER_IDX, _IRQn)) #define Scheduler_global_timer ((TIM_TypeDef*)SCHEDULER_TIMER_BASE) namespace { -constexpr uint64_t kMaxIntervalUs = - static_cast(UINT32_MAX)/2 + 1ULL; +constexpr uint64_t kMaxIntervalUs = static_cast(UINT32_MAX) / 2 + 1ULL; } std::array Scheduler::tasks_{}; @@ -38,14 +35,13 @@ inline uint8_t Scheduler::get_at(uint8_t idx) { return (((uint32_t*)&sorted_task_ids_)[word_idx] & (0x0F << shift)) >> shift; } inline void Scheduler::set_at(uint8_t idx, uint8_t id) { - uint32_t shift = idx*4; + uint32_t shift = idx * 4; uint64_t clearmask = ~(0xFF << shift); Scheduler::sorted_task_ids_ = (sorted_task_ids_ & clearmask) | (id << shift); - // sorted_task_ids_ |= ((id & 0x0F) << shift); // This is also an option in case id is incorrect, I don't think it's necessary though -} -inline uint8_t Scheduler::front_id() { - return *((uint8_t*)&sorted_task_ids_) & 0xF; + // sorted_task_ids_ |= ((id & 0x0F) << shift); // This is also an option in case id is + // incorrect, I don't think it's necessary though } +inline uint8_t Scheduler::front_id() { return *((uint8_t*)&sorted_task_ids_) & 0xF; } inline void Scheduler::pop_front() { // O(1) remove of logical index 0 Scheduler::active_task_count_--; @@ -56,15 +52,15 @@ inline void Scheduler::pop_front() { inline void Scheduler::global_timer_disable() { LL_TIM_DisableCounter(Scheduler_global_timer); - //Scheduler_global_timer->CR1 &= ~TIM_CR1_CEN; + // Scheduler_global_timer->CR1 &= ~TIM_CR1_CEN; } inline void Scheduler::global_timer_enable() { LL_TIM_EnableCounter(Scheduler_global_timer); - //Scheduler_global_timer->CR1 |= TIM_CR1_CEN; + // Scheduler_global_timer->CR1 |= TIM_CR1_CEN; } // ---------------------------- -void scheduler_global_timer_callback(void *raw) { +void scheduler_global_timer_callback(void* raw) { (void)raw; Scheduler::on_timer_update(); } @@ -73,51 +69,74 @@ void scheduler_global_timer_callback(void *raw) { void Scheduler::start() { static_assert((Scheduler::FREQUENCY % 1'000'000) == 0u, "frequenct must be a multiple of 1MHz"); - uint32_t prescaler = (SystemCoreClock / Scheduler::FREQUENCY); // setup prescaler { // ref manual: section 8.7.7 RCC domain 1 clock configuration register uint32_t ahb_prescaler = RCC->D1CFGR & RCC_D1CFGR_HPRE_Msk; - if((ahb_prescaler & 0b1000) != 0) { - switch(ahb_prescaler) { - case 0b1000: prescaler /= 2; break; - case 0b1001: prescaler /= 4; break; - case 0b1010: prescaler /= 8; break; - case 0b1011: prescaler /= 16; break; - case 0b1100: prescaler /= 64; break; - case 0b1101: prescaler /= 128; break; - case 0b1110: prescaler /= 256; break; - case 0b1111: prescaler /= 512; break; + if ((ahb_prescaler & 0b1000) != 0) { + switch (ahb_prescaler) { + case 0b1000: + prescaler /= 2; + break; + case 0b1001: + prescaler /= 4; + break; + case 0b1010: + prescaler /= 8; + break; + case 0b1011: + prescaler /= 16; + break; + case 0b1100: + prescaler /= 64; + break; + case 0b1101: + prescaler /= 128; + break; + case 0b1110: + prescaler /= 256; + break; + case 0b1111: + prescaler /= 512; + break; } } // ref manual: section 8.7.8: RCC domain 2 clock configuration register uint32_t apb1_prescaler = (RCC->D2CFGR & RCC_D2CFGR_D2PPRE1_Msk) >> RCC_D2CFGR_D2PPRE1_Pos; - if((apb1_prescaler & 0b100) != 0) { - switch(apb1_prescaler) { - case 0b100: prescaler /= 2; break; - case 0b101: prescaler /= 4; break; - case 0b110: prescaler /= 8; break; - case 0b111: prescaler /= 16; break; + if ((apb1_prescaler & 0b100) != 0) { + switch (apb1_prescaler) { + case 0b100: + prescaler /= 2; + break; + case 0b101: + prescaler /= 4; + break; + case 0b110: + prescaler /= 8; + break; + case 0b111: + prescaler /= 16; + break; } } // tim2clk = 2 x pclk1 when apb1_prescaler != 1 - if(apb1_prescaler != 1) { + if (apb1_prescaler != 1) { prescaler *= 2; } - if(prescaler > 1) { + if (prescaler > 1) { prescaler--; } } - - if(prescaler == 0 || prescaler > 0xFFFF) { + + if (prescaler == 0 || prescaler > 0xFFFF) { ErrorHandler("Invalid prescaler value: %u", prescaler); } - //static_assert(prescaler < 0xFFFF, "Prescaler is 16 bit, so it must be in that range"); - //static_assert(prescaler != 0, "Prescaler must be in the range [1, 65535]"); + // static_assert(prescaler < 0xFFFF, "Prescaler is 16 bit, so it must be in that range"); + // static_assert(prescaler != 0, "Prescaler must be in the range [1, 65535]"); #ifndef SIM_ON RCC->APB1LENR |= SCHEDULER_RCC_TIMER_ENABLE; #endif @@ -125,10 +144,12 @@ void Scheduler::start() { Scheduler_global_timer->PSC = (uint16_t)prescaler; Scheduler_global_timer->ARR = 0; Scheduler_global_timer->DIER |= LL_TIM_DIER_UIE; - Scheduler_global_timer->CR1 = LL_TIM_CLOCKDIVISION_DIV1 | (Scheduler_global_timer->CR1 & ~TIM_CR1_CKD); + Scheduler_global_timer->CR1 = + LL_TIM_CLOCKDIVISION_DIV1 | (Scheduler_global_timer->CR1 & ~TIM_CR1_CKD); // Temporary solution for TimerDomain - ST_LIB::TimerDomain::callbacks[ST_LIB::timer_idxmap[SCHEDULER_TIMER_IDX]] = scheduler_global_timer_callback; + ST_LIB::TimerDomain::callbacks[ST_LIB::timer_idxmap[SCHEDULER_TIMER_IDX]] = + scheduler_global_timer_callback; Scheduler_global_timer->CNT = 0; /* Clear counter value */ @@ -139,7 +160,7 @@ void Scheduler::start() { } void Scheduler::update() { - while(ready_bitmap_ != 0u) { + while (ready_bitmap_ != 0u) { uint32_t bit_index = static_cast(__builtin_ctz(ready_bitmap_)); ready_bitmap_ &= ~(1u << bit_index); // Clear the bit Task& task = tasks_[bit_index]; @@ -152,7 +173,7 @@ void Scheduler::update() { inline uint8_t Scheduler::allocate_slot() { uint32_t idx = __builtin_ffs(Scheduler::free_bitmap_) - 1; - if(idx > static_cast(Scheduler::kMaxTasks)) [[unlikely]] + if (idx > static_cast(Scheduler::kMaxTasks)) [[unlikely]] return static_cast(Scheduler::INVALID_ID); Scheduler::free_bitmap_ &= ~(1UL << idx); return static_cast(idx); @@ -182,20 +203,20 @@ void Scheduler::insert_sorted(uint8_t id) { uint32_t lo = (uint32_t)sorted_task_ids_; uint32_t hi = (uint32_t)(sorted_task_ids_ >> 32); - + // take the shift for only high or low 32 bits uint32_t shift = (pos & 7) << 2; uint32_t id_shifted = id << shift; - + uint32_t mask = (1UL << shift) - 1; - uint32_t inv_mask = ~mask; //Hole mask + uint32_t inv_mask = ~mask; // Hole mask - //Calculate both posibilities + // Calculate both posibilities uint32_t lo_modified = ((lo & inv_mask) << 4) | (lo & mask) | id_shifted; uint32_t hi_modified = ((hi & inv_mask) << 4) | (hi & mask) | id_shifted; - - uint32_t hi_spilled = (hi << 4) | (lo >> 28); - + + uint32_t hi_spilled = (hi << 4) | (lo >> 28); + if (pos >= 8) { hi = hi_modified; // lo remains unchanged @@ -220,24 +241,26 @@ void Scheduler::remove_sorted(uint8_t id) { // diff becomes 0x..._0_... where 0 is the nibble where id is in sorted_task_ids uint64_t diff = Scheduler::sorted_task_ids_ ^ pattern; - - //https://stackoverflow.com/questions/79058066/finding-position-of-zero-nibble-in-64-bits - //https://stackoverflow.com/questions/59480527/fast-lookup-of-a-null-nibble-in-a-64-bit-unsigned + + // https://stackoverflow.com/questions/79058066/finding-position-of-zero-nibble-in-64-bits + // https://stackoverflow.com/questions/59480527/fast-lookup-of-a-null-nibble-in-a-64-bit-unsigned uint64_t nibble_msb = 0x8888'8888'8888'8888ULL; uint64_t matches = (diff - nibble_lsb) & (~diff & nibble_msb); - if(matches == 0) [[unlikely]] return; // not found + if (matches == 0) [[unlikely]] + return; // not found - /* split the bm in two 0x0000...FFFFFF where removal index is placed + /* split the bm in two 0x0000...FFFFFF where removal index is placed * then invert to keep both sides that surround the discarded index - */ + */ uint32_t pos_msb = __builtin_ctzll(matches); uint32_t pos_lsb = pos_msb - 3; uint64_t mask = (1ULL << pos_lsb) - 1; // Remove element (lower part | higher pushing nibble out of mask) - Scheduler::sorted_task_ids_ = (Scheduler::sorted_task_ids_ & mask) | ((Scheduler::sorted_task_ids_ >> 4) & ~mask); + Scheduler::sorted_task_ids_ = + (Scheduler::sorted_task_ids_ & mask) | ((Scheduler::sorted_task_ids_ >> 4) & ~mask); Scheduler::active_task_count_--; } @@ -249,20 +272,20 @@ void Scheduler::schedule_next_interval() { return; } - uint8_t next_id = Scheduler::front_id(); // sorted_task_ids_[0] + uint8_t next_id = Scheduler::front_id(); // sorted_task_ids_[0] Task& next_task = tasks_[next_id]; int32_t diff = (int32_t)(next_task.next_fire_us - static_cast(global_tick_us_)); if (diff >= -1 && diff <= 1) [[unlikely]] { current_interval_us_ = 1; SET_BIT(Scheduler_global_timer->EGR, TIM_EGR_UG); // This should cause an interrupt } else { - if (diff < -1) [[unlikely]]{ + if (diff < -1) [[unlikely]] { current_interval_us_ = static_cast(0 - diff); } else { current_interval_us_ = static_cast(diff); } Scheduler_global_timer->ARR = static_cast(current_interval_us_ - 1u); - while(Scheduler_global_timer->CNT > Scheduler_global_timer->ARR) [[unlikely]] { + while (Scheduler_global_timer->CNT > Scheduler_global_timer->ARR) [[unlikely]] { uint32_t offset = Scheduler_global_timer->CNT - Scheduler_global_timer->ARR; current_interval_us_ = offset; SET_BIT(Scheduler_global_timer->EGR, TIM_EGR_UG); // This should cause an interrupt @@ -275,17 +298,17 @@ void Scheduler::schedule_next_interval() { void Scheduler::on_timer_update() { global_tick_us_ += current_interval_us_; - while (active_task_count_ > 0) { //Pop all due tasks, several might be due in the same tick + while (active_task_count_ > 0) { // Pop all due tasks, several might be due in the same tick uint8_t candidate_id = Scheduler::front_id(); Task& task = tasks_[candidate_id]; int32_t diff = (int32_t)(task.next_fire_us - static_cast(global_tick_us_)); - if(diff > 0) [[likely]]{ + if (diff > 0) [[likely]] { break; // Task is in the future, stop processing } pop_front(); ready_bitmap_ |= (1u << candidate_id); // mark task as ready - if(task.repeating) [[likely]] { + if (task.repeating) [[likely]] { task.next_fire_us = static_cast(global_tick_us_ + task.period_us); insert_sorted(candidate_id); } @@ -295,18 +318,23 @@ void Scheduler::on_timer_update() { } uint16_t Scheduler::register_task(uint32_t period_us, callback_t func) { - if(func == nullptr) [[unlikely]] return static_cast(Scheduler::INVALID_ID); - if(period_us == 0) [[unlikely]] period_us = 1; - if(period_us >= kMaxIntervalUs) [[unlikely]] return static_cast(Scheduler::INVALID_ID); + if (func == nullptr) [[unlikely]] + return static_cast(Scheduler::INVALID_ID); + if (period_us == 0) [[unlikely]] + period_us = 1; + if (period_us >= kMaxIntervalUs) [[unlikely]] + return static_cast(Scheduler::INVALID_ID); uint8_t slot = allocate_slot(); - if(slot == Scheduler::INVALID_ID) return slot; + if (slot == Scheduler::INVALID_ID) + return slot; Task& task = tasks_[slot]; task.callback = func; task.period_us = period_us; task.repeating = true; - task.next_fire_us = static_cast(global_tick_us_ + Scheduler_global_timer->CNT + period_us); + task.next_fire_us = + static_cast(global_tick_us_ + Scheduler_global_timer->CNT + period_us); task.id = static_cast(slot); insert_sorted(slot); schedule_next_interval(); @@ -314,12 +342,16 @@ uint16_t Scheduler::register_task(uint32_t period_us, callback_t func) { } uint16_t Scheduler::set_timeout(uint32_t microseconds, callback_t func) { - if (func == nullptr) [[unlikely]] return static_cast(Scheduler::INVALID_ID); - if(microseconds == 0) [[unlikely]] microseconds = 1; - if(microseconds >= kMaxIntervalUs) [[unlikely]] return static_cast(Scheduler::INVALID_ID); + if (func == nullptr) [[unlikely]] + return static_cast(Scheduler::INVALID_ID); + if (microseconds == 0) [[unlikely]] + microseconds = 1; + if (microseconds >= kMaxIntervalUs) [[unlikely]] + return static_cast(Scheduler::INVALID_ID); uint8_t slot = allocate_slot(); - if(slot == Scheduler::INVALID_ID) return slot; + if (slot == Scheduler::INVALID_ID) + return slot; Task& task = tasks_[slot]; task.callback = func; @@ -328,7 +360,7 @@ uint16_t Scheduler::set_timeout(uint32_t microseconds, callback_t func) { task.next_fire_us = static_cast(global_tick_us_ + microseconds); task.id = slot + Scheduler::timeout_idx_ * Scheduler::kMaxTasks; - // Add 2 instead of 1 so overflow doesn't make timeout_idx == 0, + // Add 2 instead of 1 so overflow doesn't make timeout_idx == 0, // we need it to never be 0 Scheduler::timeout_idx_ += 2; @@ -338,8 +370,10 @@ uint16_t Scheduler::set_timeout(uint32_t microseconds, callback_t func) { } bool Scheduler::unregister_task(uint16_t id) { - if(id >= kMaxTasks) return false; - if(free_bitmap_ & (1UL << id)) return false; + if (id >= kMaxTasks) + return false; + if (free_bitmap_ & (1UL << id)) + return false; remove_sorted(id); release_slot(id); @@ -350,14 +384,15 @@ bool Scheduler::unregister_task(uint16_t id) { bool Scheduler::cancel_timeout(uint16_t id) { static_assert((kMaxTasks & (kMaxTasks - 1)) == 0, "kMaxTasks must be a power of two"); uint32_t idx = id & (Scheduler::kMaxTasks - 1UL); - if(tasks_[idx].repeating) return false; - if(tasks_[idx].id != id) return false; - if(free_bitmap_ & (1UL << idx)) return false; + if (tasks_[idx].repeating) + return false; + if (tasks_[idx].id != id) + return false; + if (free_bitmap_ & (1UL << idx)) + return false; remove_sorted(idx); release_slot(idx); schedule_next_interval(); return true; } - - diff --git a/Src/HALAL/Services/Time/Time.cpp b/Src/HALAL/Services/Time/Time.cpp index 7700a2db0..b79dd6e22 100644 --- a/Src/HALAL/Services/Time/Time.cpp +++ b/Src/HALAL/Services/Time/Time.cpp @@ -23,17 +23,12 @@ stack Time::available_high_precision_timers; unordered_map Time::high_precision_alarms_by_id; unordered_map Time::low_precision_alarms_by_id; unordered_map Time::mid_precision_alarms_by_id; -unordered_map - Time::high_precision_alarms_by_timer; - -unordered_map - Time::timer32_by_timer32handler_map = { - {&htim2, TIM2}, {&htim5, TIM5}, {&htim23, TIM23}, {&htim24, TIM24}}; -unordered_map - Time::timer32interrupt_by_timer32handler_map = {{&htim2, TIM2_IRQn}, - {&htim5, TIM5_IRQn}, - {&htim23, TIM23_IRQn}, - {&htim24, TIM24_IRQn}}; +unordered_map Time::high_precision_alarms_by_timer; + +unordered_map Time::timer32_by_timer32handler_map = + {{&htim2, TIM2}, {&htim5, TIM5}, {&htim23, TIM23}, {&htim24, TIM24}}; +unordered_map Time::timer32interrupt_by_timer32handler_map = + {{&htim2, TIM2_IRQn}, {&htim5, TIM5_IRQn}, {&htim23, TIM23_IRQn}, {&htim24, TIM24_IRQn}}; stack Time::low_precision_erasable_ids; stack Time::mid_precision_erasable_ids; @@ -42,9 +37,13 @@ uint64_t Time::global_tick = 0; uint64_t Time::low_precision_tick = 0; uint64_t Time::mid_precision_tick = 0; -void Time::init_timer(TIM_TypeDef* tim, TIM_HandleTypeDef* htim, - uint32_t prescaler, uint32_t period, - IRQn_Type interrupt_channel) { +void Time::init_timer( + TIM_TypeDef* tim, + TIM_HandleTypeDef* htim, + uint32_t prescaler, + uint32_t period, + IRQn_Type interrupt_channel +) { TIM_ClockConfigTypeDef sClockSourceConfig = {0}; TIM_MasterConfigTypeDef sMasterConfig = {0}; @@ -86,30 +85,36 @@ void Time::start() { } else { hal_enable_timer(global_timer); Time::init_timer( - timer32_by_timer32handler_map[global_timer], global_timer, 0, + timer32_by_timer32handler_map[global_timer], + global_timer, + 0, HIGH_PRECISION_MAX_ARR, - timer32interrupt_by_timer32handler_map[global_timer]); + timer32interrupt_by_timer32handler_map[global_timer] + ); HAL_TIM_Base_Start_IT(global_timer); } } for (auto timer : high_precision_timers) { - if (timer32_by_timer32handler_map.find(timer) == - timer32_by_timer32handler_map.end()) { + if (timer32_by_timer32handler_map.find(timer) == timer32_by_timer32handler_map.end()) { ErrorHandler("Global Timer pointer is not valid"); high_precision_timers.erase(timer); } else { hal_enable_timer(timer); - Time::init_timer(timer32_by_timer32handler_map[timer], timer, 0, - HIGH_PRECISION_MAX_ARR, - timer32interrupt_by_timer32handler_map[timer]); + Time::init_timer( + timer32_by_timer32handler_map[timer], + timer, + 0, + HIGH_PRECISION_MAX_ARR, + timer32interrupt_by_timer32handler_map[timer] + ); Time::available_high_precision_timers.push(timer); } } } bool Time::is_valid_timer(TIM_HandleTypeDef* tim) { - if (tim == global_timer || tim == low_precision_timer || - tim == mid_precision_timer || high_precision_timers.contains(tim)) + if (tim == global_timer || tim == low_precision_timer || tim == mid_precision_timer || + high_precision_timers.contains(tim)) return true; return false; } @@ -130,8 +135,7 @@ void Time::hal_enable_timer(TIM_HandleTypeDef* tim) { uint64_t Time::get_global_tick() { if (global_timer == nullptr) { - ErrorHandler( - "tried to use global tick without global timer configured"); + ErrorHandler("tried to use global tick without global timer configured"); return 0; } uint64_t current_tick = Time::global_tick + global_timer->Instance->CNT; @@ -140,19 +144,15 @@ uint64_t Time::get_global_tick() { return current_tick * to_nanoseconds; } -void Time::start_timer(TIM_HandleTypeDef* handle, uint32_t prescaler, - uint32_t arr) { +void Time::start_timer(TIM_HandleTypeDef* handle, uint32_t prescaler, uint32_t arr) { handle->Instance->ARR = arr; handle->Instance->PSC = prescaler; HAL_TIM_Base_Start_IT(handle); } -void Time::stop_timer(TIM_HandleTypeDef* handle) { - HAL_TIM_Base_Stop_IT(handle); -} +void Time::stop_timer(TIM_HandleTypeDef* handle) { HAL_TIM_Base_Stop_IT(handle); } -uint8_t Time::register_high_precision_alarm(uint32_t period_in_us, - function func) { +uint8_t Time::register_high_precision_alarm(uint32_t period_in_us, function func) { if (available_high_precision_timers.size() == 0) { ErrorHandler("There are no available high precision timers left"); return 0; @@ -161,8 +161,7 @@ uint8_t Time::register_high_precision_alarm(uint32_t period_in_us, TIM_HandleTypeDef* tim = Time::available_high_precision_timers.top(); Time::available_high_precision_timers.pop(); - Time::Alarm alarm = { - .period = period_in_us, .tim = tim, .alarm = [&]() {}, .is_on = true}; + Time::Alarm alarm = {.period = period_in_us, .tim = tim, .alarm = [&]() {}, .is_on = true}; for (auto timer : high_precision_timers) { NVIC_DisableIRQ(timer32interrupt_by_timer32handler_map[timer]); @@ -203,31 +202,28 @@ bool Time::unregister_high_precision_alarm(uint8_t id) { return true; } -uint8_t Time::register_mid_precision_alarm(uint32_t period_in_us, - function func) { +uint8_t Time::register_mid_precision_alarm(uint32_t period_in_us, function func) { if (mid_precision_timer == nullptr) { - ErrorHandler( - "Cannot register mid precision alarm if no timer is used for mid " - "precision"); + ErrorHandler("Cannot register mid precision alarm if no timer is used for mid " + "precision"); return 0; } - if (mid_precision_alarms_by_id.size() == - std::numeric_limits::max()) { - ErrorHandler( - "Cannot register mid precision alarm as all id's are already " - "occupied"); + if (mid_precision_alarms_by_id.size() == std::numeric_limits::max()) { + ErrorHandler("Cannot register mid precision alarm as all id's are already " + "occupied"); return 0; } if (std::find_if( - TimerPeripheral::timers.begin(), TimerPeripheral::timers.end(), + TimerPeripheral::timers.begin(), + TimerPeripheral::timers.end(), [&](TimerPeripheral& tim) -> bool { return tim.handle == mid_precision_timer && (tim.is_occupied()); - }) != TimerPeripheral::timers.end()) { - ErrorHandler( - "a timer cannot be used as mid precision timer and PWM or Input " - "Capture Simultaneously"); - return 0; // TODO: put this check in high precision timers and global - // timer too + } + ) != TimerPeripheral::timers.end()) { + ErrorHandler("a timer cannot be used as mid precision timer and PWM or Input " + "Capture Simultaneously"); + return 0; // TODO: put this check in high precision timers and global + // timer too } Time::Alarm alarm = { @@ -235,25 +231,26 @@ uint8_t Time::register_mid_precision_alarm(uint32_t period_in_us, .tim = Time::mid_precision_timer, .alarm = [&]() {}, .offset = Time::mid_precision_tick, - .is_on = true}; + .is_on = true + }; while (mid_precision_alarms_by_id.contains(mid_precision_ids)) mid_precision_ids++; - NVIC_DisableIRQ( - timer32interrupt_by_timer32handler_map[mid_precision_timer]); + NVIC_DisableIRQ(timer32interrupt_by_timer32handler_map[mid_precision_timer]); Time::mid_precision_alarms_by_id[mid_precision_ids] = alarm; if (not Time::mid_precision_registered) { hal_enable_timer(mid_precision_timer); Time::init_timer( timer32_by_timer32handler_map[mid_precision_timer], - Time::mid_precision_timer, 275, Time::mid_precision_step_in_us, - timer32interrupt_by_timer32handler_map[mid_precision_timer]); - Time::ConfigTimer(Time::mid_precision_timer, - Time::mid_precision_step_in_us); - NVIC_DisableIRQ( - timer32interrupt_by_timer32handler_map[mid_precision_timer]); + Time::mid_precision_timer, + 275, + Time::mid_precision_step_in_us, + timer32interrupt_by_timer32handler_map[mid_precision_timer] + ); + Time::ConfigTimer(Time::mid_precision_timer, Time::mid_precision_step_in_us); + NVIC_DisableIRQ(timer32interrupt_by_timer32handler_map[mid_precision_timer]); Time::mid_precision_registered = true; } @@ -269,8 +266,7 @@ bool Time::unregister_mid_precision_alarm(uint8_t id) { return false; } - NVIC_DisableIRQ( - timer32interrupt_by_timer32handler_map[mid_precision_timer]); + NVIC_DisableIRQ(timer32interrupt_by_timer32handler_map[mid_precision_timer]); Time::Alarm& alarm = Time::mid_precision_alarms_by_id[id]; alarm.is_on = false; Time::mid_precision_erasable_ids.push(id); @@ -279,20 +275,19 @@ bool Time::unregister_mid_precision_alarm(uint8_t id) { return true; } -uint8_t Time::register_low_precision_alarm(uint32_t period_in_ms, - function func) { - if (low_precision_alarms_by_id.size() == - std::numeric_limits::max()) { - ErrorHandler( - "Cannot register low precision alarm as all id's are already " - "occupied"); +uint8_t Time::register_low_precision_alarm(uint32_t period_in_ms, function func) { + if (low_precision_alarms_by_id.size() == std::numeric_limits::max()) { + ErrorHandler("Cannot register low precision alarm as all id's are already " + "occupied"); return 0; } - Time::Alarm alarm = {.period = period_in_ms, - .tim = low_precision_timer, - .alarm = func, - .offset = low_precision_tick, - .is_on = true}; + Time::Alarm alarm = { + .period = period_in_ms, + .tim = low_precision_timer, + .alarm = func, + .offset = low_precision_tick, + .is_on = true + }; while (low_precision_alarms_by_id.contains(low_precision_ids)) low_precision_ids++; @@ -303,20 +298,19 @@ uint8_t Time::register_low_precision_alarm(uint32_t period_in_ms, return low_precision_ids++; } -uint8_t Time::register_low_precision_alarm(uint32_t period_in_ms, - void (*func)()) { - if (low_precision_alarms_by_id.size() == - std::numeric_limits::max()) { - ErrorHandler( - "Cannot register low precision alarm as all id's are already " - "occupied"); +uint8_t Time::register_low_precision_alarm(uint32_t period_in_ms, void (*func)()) { + if (low_precision_alarms_by_id.size() == std::numeric_limits::max()) { + ErrorHandler("Cannot register low precision alarm as all id's are already " + "occupied"); return 0; } - Time::Alarm alarm = {.period = period_in_ms, - .tim = low_precision_timer, - .alarm = func, - .offset = low_precision_tick, - .is_on = true}; + Time::Alarm alarm = { + .period = period_in_ms, + .tim = low_precision_timer, + .alarm = func, + .offset = low_precision_tick, + .is_on = true + }; while (low_precision_alarms_by_id.contains(low_precision_ids)) low_precision_ids++; @@ -342,25 +336,22 @@ bool Time::unregister_low_precision_alarm(uint8_t id) { } uint8_t Time::set_timeout(int milliseconds, function callback) { - if (low_precision_alarms_by_id.size() == - std::numeric_limits::max()) { - ErrorHandler( - "Cannot register timeout as all low precision alarms id's are " - "already occupied"); + if (low_precision_alarms_by_id.size() == std::numeric_limits::max()) { + ErrorHandler("Cannot register timeout as all low precision alarms id's are " + "already occupied"); return 255; } while (low_precision_alarms_by_id.contains(low_precision_ids)) low_precision_ids++; uint8_t id = low_precision_ids; uint64_t tick_on_register = low_precision_tick; - Time::register_low_precision_alarm( - milliseconds, [&, id, callback, tick_on_register]() { - if (tick_on_register == low_precision_tick) { - return; - } - callback(); - Time::unregister_low_precision_alarm(id); - }); + Time::register_low_precision_alarm(milliseconds, [&, id, callback, tick_on_register]() { + if (tick_on_register == low_precision_tick) { + return; + } + callback(); + Time::unregister_low_precision_alarm(id); + }); return id; } @@ -371,17 +362,14 @@ void Time::cancel_timeout(uint8_t id) { Time::unregister_low_precision_alarm(id); } -void Time::global_timer_callback() { - Time::global_tick += Time::HIGH_PRECISION_MAX_ARR; -} +void Time::global_timer_callback() { Time::global_tick += Time::HIGH_PRECISION_MAX_ARR; } void Time::high_precision_timer_callback(TIM_HandleTypeDef* tim) { Time::high_precision_alarms_by_timer[tim].alarm(); } void Time::mid_precision_timer_callback() { - for (pair& pair : - Time::mid_precision_alarms_by_id) { + for (pair& pair : Time::mid_precision_alarms_by_id) { Time::Alarm& alarm = pair.second; if (alarm.is_on == false || alarm.offset == Time::mid_precision_tick) continue; @@ -391,8 +379,7 @@ void Time::mid_precision_timer_callback() { } Time::mid_precision_tick++; while (not Time::mid_precision_erasable_ids.empty()) { - Time::mid_precision_alarms_by_id.erase( - Time::mid_precision_erasable_ids.top()); + Time::mid_precision_alarms_by_id.erase(Time::mid_precision_erasable_ids.top()); Time::mid_precision_erasable_ids.pop(); } } @@ -408,8 +395,7 @@ void Time::low_precision_timer_callback() { } low_precision_tick++; while (not Time::low_precision_erasable_ids.empty()) { - Time::low_precision_alarms_by_id.erase( - Time::low_precision_erasable_ids.top()); + Time::low_precision_alarms_by_id.erase(Time::low_precision_erasable_ids.top()); Time::low_precision_erasable_ids.pop(); } } diff --git a/Src/HALAL/Services/Watchdog/Watchdog.cpp b/Src/HALAL/Services/Watchdog/Watchdog.cpp index 4afe2d291..2fcf78090 100644 --- a/Src/HALAL/Services/Watchdog/Watchdog.cpp +++ b/Src/HALAL/Services/Watchdog/Watchdog.cpp @@ -1,5 +1,6 @@ #include "HALAL/Services/Watchdog/Watchdog.hpp" IWDG_HandleTypeDef watchdog_handle; -std::chrono::microseconds Watchdog::watchdog_time = std::chrono::microseconds(1000000); //1 second by default +std::chrono::microseconds Watchdog::watchdog_time = + std::chrono::microseconds(1000000); // 1 second by default bool reset_by_iwdg{}; \ No newline at end of file diff --git a/Src/MockedDrivers/NVIC.cpp b/Src/MockedDrivers/NVIC.cpp index cb5646615..6bd90f2e1 100644 --- a/Src/MockedDrivers/NVIC.cpp +++ b/Src/MockedDrivers/NVIC.cpp @@ -1,38 +1,31 @@ #include "MockedDrivers/NVIC.hpp" - NVIC_Type __NVIC; NVIC_Type* NVIC = &__NVIC; - -void NVIC_EnableIRQ(IRQn_Type IRQn) -{ - if ((int32_t)(IRQn) >= 0) - { - __COMPILER_BARRIER(); - NVIC->ISER[(((uint32_t)IRQn) >> 5UL)] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL)); - __COMPILER_BARRIER(); - } +void NVIC_EnableIRQ(IRQn_Type IRQn) { + if ((int32_t)(IRQn) >= 0) { + __COMPILER_BARRIER(); + NVIC->ISER[(((uint32_t)IRQn) >> 5UL)] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL)); + __COMPILER_BARRIER(); + } } -uint32_t NVIC_GetEnableIRQ(IRQn_Type IRQn) -{ - if ((int32_t)(IRQn) >= 0) - { - return((uint32_t)(((NVIC->ISER[(((uint32_t)IRQn) >> 5UL)] & (1UL << (((uint32_t)IRQn) & 0x1FUL))) != 0UL) ? 1UL : 0UL)); - } - else - { - return(0U); - } +uint32_t NVIC_GetEnableIRQ(IRQn_Type IRQn) { + if ((int32_t)(IRQn) >= 0) { + return ((uint32_t)(((NVIC->ISER[(((uint32_t)IRQn) >> 5UL)] & + (1UL << (((uint32_t)IRQn) & 0x1FUL))) != 0UL) + ? 1UL + : 0UL)); + } else { + return (0U); + } } -void NVIC_DisableIRQ(IRQn_Type IRQn) -{ - if ((int32_t)(IRQn) >= 0) - { - NVIC->ICER[(((uint32_t)IRQn) >> 5UL)] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL)); - __DSB(); - __ISB(); - } +void NVIC_DisableIRQ(IRQn_Type IRQn) { + if ((int32_t)(IRQn) >= 0) { + NVIC->ICER[(((uint32_t)IRQn) >> 5UL)] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL)); + __DSB(); + __ISB(); + } } \ No newline at end of file diff --git a/Src/MockedDrivers/mocked_hal_adc.cpp b/Src/MockedDrivers/mocked_hal_adc.cpp index d44e035c0..22c4aa183 100644 --- a/Src/MockedDrivers/mocked_hal_adc.cpp +++ b/Src/MockedDrivers/mocked_hal_adc.cpp @@ -10,50 +10,50 @@ ADC_HandleTypeDef hadc3{}; namespace { struct ADCPeripheralState { - bool initialized = false; - bool configured = false; - bool running = false; - bool conversion_ready = false; - bool force_poll_timeout = false; - uint32_t active_channel = ADC_CHANNEL_0; - uint32_t last_raw = 0; - std::unordered_map channel_raw_values{}; + bool initialized = false; + bool configured = false; + bool running = false; + bool conversion_ready = false; + bool force_poll_timeout = false; + uint32_t active_channel = ADC_CHANNEL_0; + uint32_t last_raw = 0; + std::unordered_map channel_raw_values{}; }; #if STLIB_HAS_ADC3 constexpr std::size_t kAdcCount = 3; -static std::array adc_instances{ADC1, ADC2, ADC3}; +static std::array adc_instances{ADC1, ADC2, ADC3}; #else constexpr std::size_t kAdcCount = 2; -static std::array adc_instances{ADC1, ADC2}; +static std::array adc_instances{ADC1, ADC2}; #endif static std::array adc_states{}; -static ADCPeripheralState &state_for(ADC_TypeDef *instance) { - for (std::size_t i = 0; i < kAdcCount; ++i) { - if (adc_instances[i] == instance) { - return adc_states[i]; +static ADCPeripheralState& state_for(ADC_TypeDef* instance) { + for (std::size_t i = 0; i < kAdcCount; ++i) { + if (adc_instances[i] == instance) { + return adc_states[i]; + } } - } - return adc_states[0]; + return adc_states[0]; } static uint32_t resolution_mask(uint32_t resolution) { - switch (resolution) { - case ADC_RESOLUTION_16B: - return 0xFFFFU; - case ADC_RESOLUTION_14B: - return 0x3FFFU; - case ADC_RESOLUTION_12B: - return 0x0FFFU; - case ADC_RESOLUTION_10B: - return 0x03FFU; - case ADC_RESOLUTION_8B: - return 0x00FFU; - default: - return 0x0FFFU; - } + switch (resolution) { + case ADC_RESOLUTION_16B: + return 0xFFFFU; + case ADC_RESOLUTION_14B: + return 0x3FFFU; + case ADC_RESOLUTION_12B: + return 0x0FFFU; + case ADC_RESOLUTION_10B: + return 0x03FFU; + case ADC_RESOLUTION_8B: + return 0x00FFU; + default: + return 0x0FFFU; + } } } // namespace @@ -61,163 +61,158 @@ static uint32_t resolution_mask(uint32_t resolution) { namespace ST_LIB::MockedHAL { void adc_reset() { - for (auto &state : adc_states) { - state = {}; - } + for (auto& state : adc_states) { + state = {}; + } } -void adc_set_channel_raw(ADC_TypeDef *adc, uint32_t channel, uint32_t raw_value) { - auto &state = state_for(adc); - state.channel_raw_values[channel] = raw_value; +void adc_set_channel_raw(ADC_TypeDef* adc, uint32_t channel, uint32_t raw_value) { + auto& state = state_for(adc); + state.channel_raw_values[channel] = raw_value; } -void adc_set_poll_timeout(ADC_TypeDef *adc, bool enabled) { - state_for(adc).force_poll_timeout = enabled; +void adc_set_poll_timeout(ADC_TypeDef* adc, bool enabled) { + state_for(adc).force_poll_timeout = enabled; } -uint32_t adc_get_last_channel(ADC_TypeDef *adc) { - return state_for(adc).active_channel; -} +uint32_t adc_get_last_channel(ADC_TypeDef* adc) { return state_for(adc).active_channel; } -bool adc_is_running(ADC_TypeDef *adc) { - return state_for(adc).running; -} +bool adc_is_running(ADC_TypeDef* adc) { return state_for(adc).running; } } // namespace ST_LIB::MockedHAL -extern "C" HAL_StatusTypeDef HAL_ADC_Init(ADC_HandleTypeDef *hadc) { - if (hadc == nullptr || hadc->Instance == nullptr) { - return HAL_ERROR; - } - - auto &state = state_for(hadc->Instance); - state.initialized = true; - state.running = false; - state.configured = false; - state.conversion_ready = false; - state.last_raw = 0; - - hadc->State = HAL_ADC_STATE_READY; - hadc->ErrorCode = HAL_ADC_ERROR_NONE; - return HAL_OK; +extern "C" HAL_StatusTypeDef HAL_ADC_Init(ADC_HandleTypeDef* hadc) { + if (hadc == nullptr || hadc->Instance == nullptr) { + return HAL_ERROR; + } + + auto& state = state_for(hadc->Instance); + state.initialized = true; + state.running = false; + state.configured = false; + state.conversion_ready = false; + state.last_raw = 0; + + hadc->State = HAL_ADC_STATE_READY; + hadc->ErrorCode = HAL_ADC_ERROR_NONE; + return HAL_OK; } -extern "C" HAL_StatusTypeDef HAL_ADC_DeInit(ADC_HandleTypeDef *hadc) { - if (hadc == nullptr || hadc->Instance == nullptr) { - return HAL_ERROR; - } +extern "C" HAL_StatusTypeDef HAL_ADC_DeInit(ADC_HandleTypeDef* hadc) { + if (hadc == nullptr || hadc->Instance == nullptr) { + return HAL_ERROR; + } - auto &state = state_for(hadc->Instance); - state = {}; - hadc->State = HAL_ADC_STATE_RESET; - hadc->ErrorCode = HAL_ADC_ERROR_NONE; - return HAL_OK; + auto& state = state_for(hadc->Instance); + state = {}; + hadc->State = HAL_ADC_STATE_RESET; + hadc->ErrorCode = HAL_ADC_ERROR_NONE; + return HAL_OK; } -extern "C" HAL_StatusTypeDef HAL_ADC_ConfigChannel(ADC_HandleTypeDef *hadc, - ADC_ChannelConfTypeDef *sConfig) { - if (hadc == nullptr || hadc->Instance == nullptr || sConfig == nullptr) { - return HAL_ERROR; - } - - auto &state = state_for(hadc->Instance); - if (!state.initialized) { - return HAL_ERROR; - } - - state.active_channel = sConfig->Channel; - state.configured = true; - hadc->ErrorCode = HAL_ADC_ERROR_NONE; - return HAL_OK; +extern "C" HAL_StatusTypeDef +HAL_ADC_ConfigChannel(ADC_HandleTypeDef* hadc, ADC_ChannelConfTypeDef* sConfig) { + if (hadc == nullptr || hadc->Instance == nullptr || sConfig == nullptr) { + return HAL_ERROR; + } + + auto& state = state_for(hadc->Instance); + if (!state.initialized) { + return HAL_ERROR; + } + + state.active_channel = sConfig->Channel; + state.configured = true; + hadc->ErrorCode = HAL_ADC_ERROR_NONE; + return HAL_OK; } -extern "C" HAL_StatusTypeDef HAL_ADC_Start(ADC_HandleTypeDef *hadc) { - if (hadc == nullptr || hadc->Instance == nullptr) { - return HAL_ERROR; - } - - auto &state = state_for(hadc->Instance); - if (!state.initialized || !state.configured) { - return HAL_ERROR; - } - if (state.running) { - return HAL_BUSY; - } - - state.running = true; - state.conversion_ready = false; - hadc->State |= HAL_ADC_STATE_REG_BUSY; - hadc->State &= ~HAL_ADC_STATE_REG_EOC; - hadc->ErrorCode = HAL_ADC_ERROR_NONE; - return HAL_OK; +extern "C" HAL_StatusTypeDef HAL_ADC_Start(ADC_HandleTypeDef* hadc) { + if (hadc == nullptr || hadc->Instance == nullptr) { + return HAL_ERROR; + } + + auto& state = state_for(hadc->Instance); + if (!state.initialized || !state.configured) { + return HAL_ERROR; + } + if (state.running) { + return HAL_BUSY; + } + + state.running = true; + state.conversion_ready = false; + hadc->State |= HAL_ADC_STATE_REG_BUSY; + hadc->State &= ~HAL_ADC_STATE_REG_EOC; + hadc->ErrorCode = HAL_ADC_ERROR_NONE; + return HAL_OK; } -extern "C" HAL_StatusTypeDef HAL_ADC_PollForConversion(ADC_HandleTypeDef *hadc, - uint32_t Timeout) { - (void)Timeout; +extern "C" HAL_StatusTypeDef HAL_ADC_PollForConversion(ADC_HandleTypeDef* hadc, uint32_t Timeout) { + (void)Timeout; - if (hadc == nullptr || hadc->Instance == nullptr) { - return HAL_ERROR; - } + if (hadc == nullptr || hadc->Instance == nullptr) { + return HAL_ERROR; + } - auto &state = state_for(hadc->Instance); - if (!state.running) { - return HAL_ERROR; - } + auto& state = state_for(hadc->Instance); + if (!state.running) { + return HAL_ERROR; + } - if (state.force_poll_timeout) { - hadc->State |= HAL_ADC_STATE_TIMEOUT; - return HAL_TIMEOUT; - } + if (state.force_poll_timeout) { + hadc->State |= HAL_ADC_STATE_TIMEOUT; + return HAL_TIMEOUT; + } - const auto it = state.channel_raw_values.find(state.active_channel); - const uint32_t raw = (it == state.channel_raw_values.end()) ? 0U : it->second; - state.last_raw = raw & resolution_mask(hadc->Init.Resolution); - state.conversion_ready = true; + const auto it = state.channel_raw_values.find(state.active_channel); + const uint32_t raw = (it == state.channel_raw_values.end()) ? 0U : it->second; + state.last_raw = raw & resolution_mask(hadc->Init.Resolution); + state.conversion_ready = true; - hadc->State &= ~HAL_ADC_STATE_TIMEOUT; - hadc->State |= HAL_ADC_STATE_REG_EOC; - return HAL_OK; + hadc->State &= ~HAL_ADC_STATE_TIMEOUT; + hadc->State |= HAL_ADC_STATE_REG_EOC; + return HAL_OK; } -extern "C" uint32_t HAL_ADC_GetValue(const ADC_HandleTypeDef *hadc) { - if (hadc == nullptr || hadc->Instance == nullptr) { - return 0; - } - auto &state = state_for(hadc->Instance); - return state.last_raw; +extern "C" uint32_t HAL_ADC_GetValue(const ADC_HandleTypeDef* hadc) { + if (hadc == nullptr || hadc->Instance == nullptr) { + return 0; + } + auto& state = state_for(hadc->Instance); + return state.last_raw; } -extern "C" HAL_StatusTypeDef HAL_ADC_Stop(ADC_HandleTypeDef *hadc) { - if (hadc == nullptr || hadc->Instance == nullptr) { - return HAL_ERROR; - } +extern "C" HAL_StatusTypeDef HAL_ADC_Stop(ADC_HandleTypeDef* hadc) { + if (hadc == nullptr || hadc->Instance == nullptr) { + return HAL_ERROR; + } - auto &state = state_for(hadc->Instance); - state.running = false; - state.conversion_ready = false; + auto& state = state_for(hadc->Instance); + state.running = false; + state.conversion_ready = false; - hadc->State &= ~HAL_ADC_STATE_REG_BUSY; - hadc->State &= ~HAL_ADC_STATE_REG_EOC; - return HAL_OK; + hadc->State &= ~HAL_ADC_STATE_REG_BUSY; + hadc->State &= ~HAL_ADC_STATE_REG_EOC; + return HAL_OK; } -extern "C" uint32_t HAL_ADC_GetState(const ADC_HandleTypeDef *hadc) { - if (hadc == nullptr) { - return HAL_ADC_STATE_RESET; - } - return hadc->State; +extern "C" uint32_t HAL_ADC_GetState(const ADC_HandleTypeDef* hadc) { + if (hadc == nullptr) { + return HAL_ADC_STATE_RESET; + } + return hadc->State; } -extern "C" uint32_t HAL_ADC_GetError(const ADC_HandleTypeDef *hadc) { - if (hadc == nullptr) { - return HAL_ADC_ERROR_NONE; - } - return hadc->ErrorCode; +extern "C" uint32_t HAL_ADC_GetError(const ADC_HandleTypeDef* hadc) { + if (hadc == nullptr) { + return HAL_ADC_ERROR_NONE; + } + return hadc->ErrorCode; } -extern "C" void HAL_SYSCFG_AnalogSwitchConfig(uint32_t SYSCFG_AnalogSwitch, - uint32_t SYSCFG_SwitchState) { - (void)SYSCFG_AnalogSwitch; - (void)SYSCFG_SwitchState; +extern "C" void +HAL_SYSCFG_AnalogSwitchConfig(uint32_t SYSCFG_AnalogSwitch, uint32_t SYSCFG_SwitchState) { + (void)SYSCFG_AnalogSwitch; + (void)SYSCFG_SwitchState; } diff --git a/Src/MockedDrivers/mocked_hal_dma.cpp b/Src/MockedDrivers/mocked_hal_dma.cpp index 06d3782f7..53ff7dea4 100644 --- a/Src/MockedDrivers/mocked_hal_dma.cpp +++ b/Src/MockedDrivers/mocked_hal_dma.cpp @@ -5,15 +5,15 @@ namespace { struct DMAState { - HAL_StatusTypeDef init_status = HAL_OK; - HAL_StatusTypeDef start_status = HAL_OK; - std::array calls{}; - DMA_HandleTypeDef *last_init_handle = nullptr; - DMA_HandleTypeDef *last_start_handle = nullptr; - DMA_HandleTypeDef *last_irq_handle = nullptr; - uint32_t last_start_src = 0; - uint32_t last_start_dst = 0; - uint32_t last_start_length = 0; + HAL_StatusTypeDef init_status = HAL_OK; + HAL_StatusTypeDef start_status = HAL_OK; + std::array calls{}; + DMA_HandleTypeDef* last_init_handle = nullptr; + DMA_HandleTypeDef* last_start_handle = nullptr; + DMA_HandleTypeDef* last_irq_handle = nullptr; + uint32_t last_start_src = 0; + uint32_t last_start_dst = 0; + uint32_t last_start_length = 0; }; DMAState g_state{}; @@ -29,16 +29,14 @@ void dma_set_init_status(HAL_StatusTypeDef status) { g_state.init_status = statu void dma_set_start_status(HAL_StatusTypeDef status) { g_state.start_status = status; } std::size_t dma_get_call_count(DMAOperation op) { - return g_state.calls[static_cast(op)]; + return g_state.calls[static_cast(op)]; } -DMA_HandleTypeDef *dma_get_last_init_handle() { return g_state.last_init_handle; } +DMA_HandleTypeDef* dma_get_last_init_handle() { return g_state.last_init_handle; } -DMA_HandleTypeDef *dma_get_last_start_handle() { - return g_state.last_start_handle; -} +DMA_HandleTypeDef* dma_get_last_start_handle() { return g_state.last_start_handle; } -DMA_HandleTypeDef *dma_get_last_irq_handle() { return g_state.last_irq_handle; } +DMA_HandleTypeDef* dma_get_last_irq_handle() { return g_state.last_irq_handle; } uint32_t dma_get_last_start_src() { return g_state.last_start_src; } @@ -48,31 +46,33 @@ uint32_t dma_get_last_start_length() { return g_state.last_start_length; } } // namespace ST_LIB::MockedHAL -extern "C" HAL_StatusTypeDef MockedHAL_DMA_Init_Impl(DMA_HandleTypeDef *hdma) { - g_state.calls[static_cast(ST_LIB::MockedHAL::DMAOperation::Init)]++; - g_state.last_init_handle = hdma; - if (hdma == nullptr) { - return HAL_ERROR; - } - return g_state.init_status; +extern "C" HAL_StatusTypeDef MockedHAL_DMA_Init_Impl(DMA_HandleTypeDef* hdma) { + g_state.calls[static_cast(ST_LIB::MockedHAL::DMAOperation::Init)]++; + g_state.last_init_handle = hdma; + if (hdma == nullptr) { + return HAL_ERROR; + } + return g_state.init_status; } -extern "C" HAL_StatusTypeDef -MockedHAL_DMA_Start_IT_Impl(DMA_HandleTypeDef *hdma, uint32_t SrcAddress, - uint32_t DstAddress, uint32_t DataLength) { - g_state.calls[static_cast(ST_LIB::MockedHAL::DMAOperation::StartIT)]++; - g_state.last_start_handle = hdma; - g_state.last_start_src = SrcAddress; - g_state.last_start_dst = DstAddress; - g_state.last_start_length = DataLength; - if (hdma == nullptr) { - return HAL_ERROR; - } - return g_state.start_status; +extern "C" HAL_StatusTypeDef MockedHAL_DMA_Start_IT_Impl( + DMA_HandleTypeDef* hdma, + uint32_t SrcAddress, + uint32_t DstAddress, + uint32_t DataLength +) { + g_state.calls[static_cast(ST_LIB::MockedHAL::DMAOperation::StartIT)]++; + g_state.last_start_handle = hdma; + g_state.last_start_src = SrcAddress; + g_state.last_start_dst = DstAddress; + g_state.last_start_length = DataLength; + if (hdma == nullptr) { + return HAL_ERROR; + } + return g_state.start_status; } -extern "C" void MockedHAL_DMA_IRQHandler_Impl(DMA_HandleTypeDef *hdma) { - g_state.calls[static_cast(ST_LIB::MockedHAL::DMAOperation::IRQHandler)]++; - g_state.last_irq_handle = hdma; +extern "C" void MockedHAL_DMA_IRQHandler_Impl(DMA_HandleTypeDef* hdma) { + g_state.calls[static_cast(ST_LIB::MockedHAL::DMAOperation::IRQHandler)]++; + g_state.last_irq_handle = hdma; } - diff --git a/Src/MockedDrivers/mocked_hal_spi.cpp b/Src/MockedDrivers/mocked_hal_spi.cpp index 7ecb165ad..dff3c52a9 100644 --- a/Src/MockedDrivers/mocked_hal_spi.cpp +++ b/Src/MockedDrivers/mocked_hal_spi.cpp @@ -6,61 +6,61 @@ namespace { struct SPIState { - HAL_StatusTypeDef next_status = HAL_OK; - bool busy = false; - std::array calls{}; - std::vector rx_pattern{}; - std::vector last_tx{}; - std::size_t last_size_words = 0; - SPI_HandleTypeDef *last_handle = nullptr; + HAL_StatusTypeDef next_status = HAL_OK; + bool busy = false; + std::array calls{}; + std::vector rx_pattern{}; + std::vector last_tx{}; + std::size_t last_size_words = 0; + SPI_HandleTypeDef* last_handle = nullptr; }; SPIState g_state{}; -void fill_rx(uint8_t *dst, std::size_t size_bytes) { - if (dst == nullptr || size_bytes == 0) { - return; - } - if (g_state.rx_pattern.empty()) { +void fill_rx(uint8_t* dst, std::size_t size_bytes) { + if (dst == nullptr || size_bytes == 0) { + return; + } + if (g_state.rx_pattern.empty()) { + for (std::size_t i = 0; i < size_bytes; ++i) { + dst[i] = 0; + } + return; + } for (std::size_t i = 0; i < size_bytes; ++i) { - dst[i] = 0; + dst[i] = g_state.rx_pattern[i % g_state.rx_pattern.size()]; } - return; - } - for (std::size_t i = 0; i < size_bytes; ++i) { - dst[i] = g_state.rx_pattern[i % g_state.rx_pattern.size()]; - } } -void store_tx(uint8_t *src, std::size_t size_bytes) { - g_state.last_tx.assign(size_bytes, 0); - if (src == nullptr || size_bytes == 0) { - return; - } - for (std::size_t i = 0; i < size_bytes; ++i) { - g_state.last_tx[i] = src[i]; - } +void store_tx(uint8_t* src, std::size_t size_bytes) { + g_state.last_tx.assign(size_bytes, 0); + if (src == nullptr || size_bytes == 0) { + return; + } + for (std::size_t i = 0; i < size_bytes; ++i) { + g_state.last_tx[i] = src[i]; + } } -std::size_t frame_bytes(const SPI_HandleTypeDef *hspi) { - if (hspi == nullptr) { - return 1; - } - const uint32_t bits = hspi->Init.DataSize + 1U; - if (bits <= 8U) { - return 1; - } - if (bits <= 16U) { - return 2; - } - return 4; +std::size_t frame_bytes(const SPI_HandleTypeDef* hspi) { + if (hspi == nullptr) { + return 1; + } + const uint32_t bits = hspi->Init.DataSize + 1U; + if (bits <= 8U) { + return 1; + } + if (bits <= 16U) { + return 2; + } + return 4; } HAL_StatusTypeDef take_status() { - if (g_state.busy) { - return HAL_BUSY; - } - return g_state.next_status; + if (g_state.busy) { + return HAL_BUSY; + } + return g_state.next_status; } } // namespace @@ -74,110 +74,115 @@ void spi_set_status(HAL_StatusTypeDef status) { g_state.next_status = status; } void spi_set_busy(bool busy) { g_state.busy = busy; } void spi_set_rx_pattern(std::span pattern) { - g_state.rx_pattern.assign(pattern.begin(), pattern.end()); + g_state.rx_pattern.assign(pattern.begin(), pattern.end()); } std::size_t spi_get_call_count(SPIOperation op) { - return g_state.calls[static_cast(op)]; + return g_state.calls[static_cast(op)]; } std::size_t spi_get_last_size_words() { return g_state.last_size_words; } -const uint8_t *spi_get_last_tx_data() { return g_state.last_tx.data(); } +const uint8_t* spi_get_last_tx_data() { return g_state.last_tx.data(); } std::size_t spi_get_last_tx_size_bytes() { return g_state.last_tx.size(); } -SPI_HandleTypeDef *spi_get_last_handle() { return g_state.last_handle; } +SPI_HandleTypeDef* spi_get_last_handle() { return g_state.last_handle; } } // namespace ST_LIB::MockedHAL -extern "C" HAL_StatusTypeDef HAL_SPI_Init(SPI_HandleTypeDef *hspi) { - g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::Init)]++; - g_state.last_handle = hspi; - if (hspi == nullptr) { - return HAL_ERROR; - } - hspi->State = HAL_SPI_STATE_READY; - hspi->ErrorCode = HAL_SPI_ERROR_NONE; - return take_status(); +extern "C" HAL_StatusTypeDef HAL_SPI_Init(SPI_HandleTypeDef* hspi) { + g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::Init)]++; + g_state.last_handle = hspi; + if (hspi == nullptr) { + return HAL_ERROR; + } + hspi->State = HAL_SPI_STATE_READY; + hspi->ErrorCode = HAL_SPI_ERROR_NONE; + return take_status(); } -extern "C" HAL_StatusTypeDef HAL_SPI_Abort(SPI_HandleTypeDef *hspi) { - g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::Abort)]++; - g_state.last_handle = hspi; - if (hspi == nullptr) { - return HAL_ERROR; - } - hspi->State = HAL_SPI_STATE_READY; - return take_status(); +extern "C" HAL_StatusTypeDef HAL_SPI_Abort(SPI_HandleTypeDef* hspi) { + g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::Abort)]++; + g_state.last_handle = hspi; + if (hspi == nullptr) { + return HAL_ERROR; + } + hspi->State = HAL_SPI_STATE_READY; + return take_status(); } -extern "C" HAL_StatusTypeDef HAL_SPI_Transmit(SPI_HandleTypeDef *hspi, uint8_t *pData, - uint16_t Size, uint32_t Timeout) { - (void)Timeout; - g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::Transmit)]++; - g_state.last_handle = hspi; - g_state.last_size_words = Size; - store_tx(pData, static_cast(Size) * frame_bytes(hspi)); - return take_status(); +extern "C" HAL_StatusTypeDef +HAL_SPI_Transmit(SPI_HandleTypeDef* hspi, uint8_t* pData, uint16_t Size, uint32_t Timeout) { + (void)Timeout; + g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::Transmit)]++; + g_state.last_handle = hspi; + g_state.last_size_words = Size; + store_tx(pData, static_cast(Size) * frame_bytes(hspi)); + return take_status(); } -extern "C" HAL_StatusTypeDef HAL_SPI_Receive(SPI_HandleTypeDef *hspi, uint8_t *pData, - uint16_t Size, uint32_t Timeout) { - (void)Timeout; - g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::Receive)]++; - g_state.last_handle = hspi; - g_state.last_size_words = Size; - fill_rx(pData, static_cast(Size) * frame_bytes(hspi)); - return take_status(); +extern "C" HAL_StatusTypeDef +HAL_SPI_Receive(SPI_HandleTypeDef* hspi, uint8_t* pData, uint16_t Size, uint32_t Timeout) { + (void)Timeout; + g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::Receive)]++; + g_state.last_handle = hspi; + g_state.last_size_words = Size; + fill_rx(pData, static_cast(Size) * frame_bytes(hspi)); + return take_status(); } -extern "C" HAL_StatusTypeDef HAL_SPI_TransmitReceive(SPI_HandleTypeDef *hspi, uint8_t *pTxData, - uint8_t *pRxData, uint16_t Size, - uint32_t Timeout) { - (void)Timeout; - g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::TransmitReceive)]++; - g_state.last_handle = hspi; - g_state.last_size_words = Size; - const auto size_bytes = static_cast(Size) * frame_bytes(hspi); - store_tx(pTxData, size_bytes); - fill_rx(pRxData, size_bytes); - return take_status(); +extern "C" HAL_StatusTypeDef HAL_SPI_TransmitReceive( + SPI_HandleTypeDef* hspi, + uint8_t* pTxData, + uint8_t* pRxData, + uint16_t Size, + uint32_t Timeout +) { + (void)Timeout; + g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::TransmitReceive)]++; + g_state.last_handle = hspi; + g_state.last_size_words = Size; + const auto size_bytes = static_cast(Size) * frame_bytes(hspi); + store_tx(pTxData, size_bytes); + fill_rx(pRxData, size_bytes); + return take_status(); } -extern "C" HAL_StatusTypeDef HAL_SPI_Transmit_DMA(SPI_HandleTypeDef *hspi, uint8_t *pData, - uint16_t Size) { - g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::TransmitDMA)]++; - g_state.last_handle = hspi; - g_state.last_size_words = Size; - store_tx(pData, static_cast(Size) * frame_bytes(hspi)); - return take_status(); +extern "C" HAL_StatusTypeDef +HAL_SPI_Transmit_DMA(SPI_HandleTypeDef* hspi, uint8_t* pData, uint16_t Size) { + g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::TransmitDMA)]++; + g_state.last_handle = hspi; + g_state.last_size_words = Size; + store_tx(pData, static_cast(Size) * frame_bytes(hspi)); + return take_status(); } -extern "C" HAL_StatusTypeDef HAL_SPI_Receive_DMA(SPI_HandleTypeDef *hspi, uint8_t *pData, - uint16_t Size) { - g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::ReceiveDMA)]++; - g_state.last_handle = hspi; - g_state.last_size_words = Size; - fill_rx(pData, static_cast(Size) * frame_bytes(hspi)); - return take_status(); +extern "C" HAL_StatusTypeDef +HAL_SPI_Receive_DMA(SPI_HandleTypeDef* hspi, uint8_t* pData, uint16_t Size) { + g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::ReceiveDMA)]++; + g_state.last_handle = hspi; + g_state.last_size_words = Size; + fill_rx(pData, static_cast(Size) * frame_bytes(hspi)); + return take_status(); } -extern "C" HAL_StatusTypeDef HAL_SPI_TransmitReceive_DMA(SPI_HandleTypeDef *hspi, - uint8_t *pTxData, - uint8_t *pRxData, - uint16_t Size) { - g_state.calls[static_cast( - ST_LIB::MockedHAL::SPIOperation::TransmitReceiveDMA)]++; - g_state.last_handle = hspi; - g_state.last_size_words = Size; - const auto size_bytes = static_cast(Size) * frame_bytes(hspi); - store_tx(pTxData, size_bytes); - fill_rx(pRxData, size_bytes); - return take_status(); +extern "C" HAL_StatusTypeDef HAL_SPI_TransmitReceive_DMA( + SPI_HandleTypeDef* hspi, + uint8_t* pTxData, + uint8_t* pRxData, + uint16_t Size +) { + g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::TransmitReceiveDMA)]++; + g_state.last_handle = hspi; + g_state.last_size_words = Size; + const auto size_bytes = static_cast(Size) * frame_bytes(hspi); + store_tx(pTxData, size_bytes); + fill_rx(pRxData, size_bytes); + return take_status(); } -extern "C" void HAL_SPI_IRQHandler(SPI_HandleTypeDef *hspi) { - g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::IRQHandler)]++; - g_state.last_handle = hspi; +extern "C" void HAL_SPI_IRQHandler(SPI_HandleTypeDef* hspi) { + g_state.calls[static_cast(ST_LIB::MockedHAL::SPIOperation::IRQHandler)]++; + g_state.last_handle = hspi; } diff --git a/Src/MockedDrivers/mocked_ll_tim.cpp b/Src/MockedDrivers/mocked_ll_tim.cpp index 143237473..e73d3401b 100644 --- a/Src/MockedDrivers/mocked_ll_tim.cpp +++ b/Src/MockedDrivers/mocked_ll_tim.cpp @@ -3,53 +3,53 @@ #include TIM_TypeDef __htim2{TIM2_IRQHandler, TIM2_IRQn}; -TIM_TypeDef *TIM2_BASE = &__htim2; +TIM_TypeDef* TIM2_BASE = &__htim2; TIM_TypeDef __htim3{TIM3_IRQHandler, TIM3_IRQn}; -TIM_TypeDef *TIM3_BASE = &__htim3; +TIM_TypeDef* TIM3_BASE = &__htim3; TIM_TypeDef __htim4{TIM4_IRQHandler, TIM4_IRQn}; -TIM_TypeDef *TIM4_BASE = &__htim4; +TIM_TypeDef* TIM4_BASE = &__htim4; TIM_TypeDef __htim5{TIM5_IRQHandler, TIM5_IRQn}; -TIM_TypeDef *TIM5_BASE = &__htim5; +TIM_TypeDef* TIM5_BASE = &__htim5; TIM_TypeDef __htim6{TIM6_DAC_IRQHandler, TIM6_DAC_IRQn}; -TIM_TypeDef *TIM6_BASE = &__htim6; +TIM_TypeDef* TIM6_BASE = &__htim6; TIM_TypeDef __htim7{TIM7_IRQHandler, TIM7_IRQn}; -TIM_TypeDef *TIM7_BASE = &__htim7; +TIM_TypeDef* TIM7_BASE = &__htim7; TIM_TypeDef __htim12{TIM8_BRK_TIM12_IRQHandler, TIM8_BRK_TIM12_IRQn}; -TIM_TypeDef *TIM12_BASE = &__htim12; +TIM_TypeDef* TIM12_BASE = &__htim12; TIM_TypeDef __htim13{TIM8_UP_TIM13_IRQHandler, TIM8_UP_TIM13_IRQn}; -TIM_TypeDef *TIM13_BASE = &__htim13; +TIM_TypeDef* TIM13_BASE = &__htim13; TIM_TypeDef __htim14{TIM8_TRG_COM_TIM14_IRQHandler, TIM8_TRG_COM_TIM14_IRQn}; -TIM_TypeDef *TIM14_BASE = &__htim14; +TIM_TypeDef* TIM14_BASE = &__htim14; TIM_TypeDef __htim23{TIM23_IRQHandler, TIM23_IRQn}; -TIM_TypeDef *TIM23_BASE = &__htim23; +TIM_TypeDef* TIM23_BASE = &__htim23; TIM_TypeDef __htim24{TIM24_IRQHandler, TIM24_IRQn}; -TIM_TypeDef *TIM24_BASE = &__htim24; +TIM_TypeDef* TIM24_BASE = &__htim24; TIM_TypeDef __htim1{TIM1_UP_IRQHandler, TIM1_UP_IRQn}; -TIM_TypeDef *TIM1_BASE = &__htim1; +TIM_TypeDef* TIM1_BASE = &__htim1; TIM_TypeDef __htim8{TIM8_UP_TIM13_IRQHandler, TIM8_UP_TIM13_IRQn}; -TIM_TypeDef *TIM8_BASE = &__htim8; +TIM_TypeDef* TIM8_BASE = &__htim8; TIM_TypeDef __htim15{TIM15_IRQHandler, TIM15_IRQn}; -TIM_TypeDef *TIM15_BASE = &__htim15; +TIM_TypeDef* TIM15_BASE = &__htim15; TIM_TypeDef __htim16{TIM16_IRQHandler, TIM16_IRQn}; -TIM_TypeDef *TIM16_BASE = &__htim16; +TIM_TypeDef* TIM16_BASE = &__htim16; TIM_TypeDef __htim17{TIM17_IRQHandler, TIM17_IRQn}; -TIM_TypeDef *TIM17_BASE = &__htim17; +TIM_TypeDef* TIM17_BASE = &__htim17; void TIM_TypeDef::generate_update() { - active_PSC = PSC; - active_ARR = ARR; - active_RCR = RCR; - internal_rcr_cnt = active_RCR; - internal_psc_cnt = 0; - *((uint32_t*)&CNT) = 0; // Usually UEV also resets CNT unless configured otherwise - SR &= ~(1U << 0); // Clear UIF if needed, or set it depending on CR1 + active_PSC = PSC; + active_ARR = ARR; + active_RCR = RCR; + internal_rcr_cnt = active_RCR; + internal_psc_cnt = 0; + *((uint32_t*)&CNT) = 0; // Usually UEV also resets CNT unless configured otherwise + SR &= ~(1U << 0); // Clear UIF if needed, or set it depending on CR1 } -void simulate_ticks(TIM_TypeDef* tim){ +void simulate_ticks(TIM_TypeDef* tim) { // Bit definitions for clarity const uint32_t CR1_UDIS = (1U << 1); // Update Disable const uint32_t CR1_ARPE = (1U << 7); // Auto-Reload Preload Enable - const uint32_t SR_UIF = (1U << 0); // Update Interrupt Flag + const uint32_t SR_UIF = (1U << 0); // Update Interrupt Flag // Determine the current Auto-Reload limit. // If ARPE is set, use the buffered (shadow) value. @@ -64,7 +64,7 @@ void simulate_ticks(TIM_TypeDef* tim){ // 4. Repetition Counter & Update Event Logic // The Update Event (UEV) is generated when the Repetition Counter underflows. if (tim->internal_rcr_cnt == 0) { - + // --- GENERATE UPDATE EVENT (UEV) --- // A. Update Shadow Registers from Preload Registers @@ -76,7 +76,7 @@ void simulate_ticks(TIM_TypeDef* tim){ // Only if UDIS (Update Disable) is NOT set if (!(tim->CR1 & CR1_UDIS)) { tim->SR |= SR_UIF; - if(NVIC_GetEnableIRQ(tim->irq_n)){ + if (NVIC_GetEnableIRQ(tim->irq_n)) { tim->callback(); } } @@ -92,7 +92,7 @@ void simulate_ticks(TIM_TypeDef* tim){ } void TIM_TypeDef::inc_cnt_and_check(uint32_t val) { - if(val != 0 && this->check_CNT_increase_preconditions()){ + if (val != 0 && this->check_CNT_increase_preconditions()) { this->CNT += val; simulate_ticks(this); } diff --git a/Src/MockedDrivers/stm32h723xx_wrapper.c b/Src/MockedDrivers/stm32h723xx_wrapper.c index cdae259ce..6b621b0d4 100644 --- a/Src/MockedDrivers/stm32h723xx_wrapper.c +++ b/Src/MockedDrivers/stm32h723xx_wrapper.c @@ -1,7 +1,7 @@ #include "stm32h723xx_wrapper.h" static RCC_TypeDef RCC_struct; -RCC_TypeDef *RCC = &RCC_struct; +RCC_TypeDef* RCC = &RCC_struct; static GPIO_TypeDef GPIOA_struct; static GPIO_TypeDef GPIOB_struct; @@ -12,22 +12,22 @@ static GPIO_TypeDef GPIOF_struct; static GPIO_TypeDef GPIOG_struct; static GPIO_TypeDef GPIOH_struct; -GPIO_TypeDef *GPIOA = &GPIOA_struct; -GPIO_TypeDef *GPIOB = &GPIOB_struct; -GPIO_TypeDef *GPIOC = &GPIOC_struct; -GPIO_TypeDef *GPIOD = &GPIOD_struct; -GPIO_TypeDef *GPIOE = &GPIOE_struct; -GPIO_TypeDef *GPIOF = &GPIOF_struct; -GPIO_TypeDef *GPIOG = &GPIOG_struct; -GPIO_TypeDef *GPIOH = &GPIOH_struct; +GPIO_TypeDef* GPIOA = &GPIOA_struct; +GPIO_TypeDef* GPIOB = &GPIOB_struct; +GPIO_TypeDef* GPIOC = &GPIOC_struct; +GPIO_TypeDef* GPIOD = &GPIOD_struct; +GPIO_TypeDef* GPIOE = &GPIOE_struct; +GPIO_TypeDef* GPIOF = &GPIOF_struct; +GPIO_TypeDef* GPIOG = &GPIOG_struct; +GPIO_TypeDef* GPIOH = &GPIOH_struct; static ADC_TypeDef ADC1_struct; static ADC_TypeDef ADC2_struct; static ADC_TypeDef ADC3_struct; -ADC_TypeDef *ADC1 = &ADC1_struct; -ADC_TypeDef *ADC2 = &ADC2_struct; -ADC_TypeDef *ADC3 = &ADC3_struct; +ADC_TypeDef* ADC1 = &ADC1_struct; +ADC_TypeDef* ADC2 = &ADC2_struct; +ADC_TypeDef* ADC3 = &ADC3_struct; static DMA_Stream_TypeDef DMA1_Stream0_struct; static DMA_Stream_TypeDef DMA1_Stream1_struct; @@ -46,22 +46,22 @@ static DMA_Stream_TypeDef DMA2_Stream5_struct; static DMA_Stream_TypeDef DMA2_Stream6_struct; static DMA_Stream_TypeDef DMA2_Stream7_struct; -DMA_Stream_TypeDef *DMA1_Stream0 = &DMA1_Stream0_struct; -DMA_Stream_TypeDef *DMA1_Stream1 = &DMA1_Stream1_struct; -DMA_Stream_TypeDef *DMA1_Stream2 = &DMA1_Stream2_struct; -DMA_Stream_TypeDef *DMA1_Stream3 = &DMA1_Stream3_struct; -DMA_Stream_TypeDef *DMA1_Stream4 = &DMA1_Stream4_struct; -DMA_Stream_TypeDef *DMA1_Stream5 = &DMA1_Stream5_struct; -DMA_Stream_TypeDef *DMA1_Stream6 = &DMA1_Stream6_struct; -DMA_Stream_TypeDef *DMA1_Stream7 = &DMA1_Stream7_struct; -DMA_Stream_TypeDef *DMA2_Stream0 = &DMA2_Stream0_struct; -DMA_Stream_TypeDef *DMA2_Stream1 = &DMA2_Stream1_struct; -DMA_Stream_TypeDef *DMA2_Stream2 = &DMA2_Stream2_struct; -DMA_Stream_TypeDef *DMA2_Stream3 = &DMA2_Stream3_struct; -DMA_Stream_TypeDef *DMA2_Stream4 = &DMA2_Stream4_struct; -DMA_Stream_TypeDef *DMA2_Stream5 = &DMA2_Stream5_struct; -DMA_Stream_TypeDef *DMA2_Stream6 = &DMA2_Stream6_struct; -DMA_Stream_TypeDef *DMA2_Stream7 = &DMA2_Stream7_struct; +DMA_Stream_TypeDef* DMA1_Stream0 = &DMA1_Stream0_struct; +DMA_Stream_TypeDef* DMA1_Stream1 = &DMA1_Stream1_struct; +DMA_Stream_TypeDef* DMA1_Stream2 = &DMA1_Stream2_struct; +DMA_Stream_TypeDef* DMA1_Stream3 = &DMA1_Stream3_struct; +DMA_Stream_TypeDef* DMA1_Stream4 = &DMA1_Stream4_struct; +DMA_Stream_TypeDef* DMA1_Stream5 = &DMA1_Stream5_struct; +DMA_Stream_TypeDef* DMA1_Stream6 = &DMA1_Stream6_struct; +DMA_Stream_TypeDef* DMA1_Stream7 = &DMA1_Stream7_struct; +DMA_Stream_TypeDef* DMA2_Stream0 = &DMA2_Stream0_struct; +DMA_Stream_TypeDef* DMA2_Stream1 = &DMA2_Stream1_struct; +DMA_Stream_TypeDef* DMA2_Stream2 = &DMA2_Stream2_struct; +DMA_Stream_TypeDef* DMA2_Stream3 = &DMA2_Stream3_struct; +DMA_Stream_TypeDef* DMA2_Stream4 = &DMA2_Stream4_struct; +DMA_Stream_TypeDef* DMA2_Stream5 = &DMA2_Stream5_struct; +DMA_Stream_TypeDef* DMA2_Stream6 = &DMA2_Stream6_struct; +DMA_Stream_TypeDef* DMA2_Stream7 = &DMA2_Stream7_struct; static SPI_TypeDef SPI1_struct; static SPI_TypeDef SPI2_struct; @@ -70,9 +70,9 @@ static SPI_TypeDef SPI4_struct; static SPI_TypeDef SPI5_struct; static SPI_TypeDef SPI6_struct; -SPI_TypeDef *SPI1 = &SPI1_struct; -SPI_TypeDef *SPI2 = &SPI2_struct; -SPI_TypeDef *SPI3 = &SPI3_struct; -SPI_TypeDef *SPI4 = &SPI4_struct; -SPI_TypeDef *SPI5 = &SPI5_struct; -SPI_TypeDef *SPI6 = &SPI6_struct; +SPI_TypeDef* SPI1 = &SPI1_struct; +SPI_TypeDef* SPI2 = &SPI2_struct; +SPI_TypeDef* SPI3 = &SPI3_struct; +SPI_TypeDef* SPI4 = &SPI4_struct; +SPI_TypeDef* SPI5 = &SPI5_struct; +SPI_TypeDef* SPI6 = &SPI6_struct; diff --git a/Src/ST-LIB.cpp b/Src/ST-LIB.cpp index 293a925af..fb8a4378d 100644 --- a/Src/ST-LIB.cpp +++ b/Src/ST-LIB.cpp @@ -3,26 +3,34 @@ #ifdef STLIB_ETH // Con Ethernet: interfaz con MAC/IP + overload con strings -void STLIB::start(MAC mac, IPV4 ip, IPV4 subnet_mask, IPV4 gateway, - UART::Peripheral &printf_peripheral) { - HALAL::start(mac, ip, subnet_mask, gateway, printf_peripheral); - STLIB_LOW::start(); - STLIB_HIGH::start(); +void STLIB::start( + MAC mac, + IPV4 ip, + IPV4 subnet_mask, + IPV4 gateway, + UART::Peripheral& printf_peripheral +) { + HALAL::start(mac, ip, subnet_mask, gateway, printf_peripheral); + STLIB_LOW::start(); + STLIB_HIGH::start(); } -void STLIB::start(const std::string &mac, const std::string &ip, - const std::string &subnet_mask, const std::string &gateway, - UART::Peripheral &printf_peripheral) { - STLIB::start(MAC(mac), IPV4(ip), IPV4(subnet_mask), IPV4(gateway), - printf_peripheral); +void STLIB::start( + const std::string& mac, + const std::string& ip, + const std::string& subnet_mask, + const std::string& gateway, + UART::Peripheral& printf_peripheral +) { + STLIB::start(MAC(mac), IPV4(ip), IPV4(subnet_mask), IPV4(gateway), printf_peripheral); } #else // !STLIB_ETH -void STLIB::start(UART::Peripheral &printf_peripheral) { - HALAL::start(printf_peripheral); - STLIB_LOW::start(); - STLIB_HIGH::start(); +void STLIB::start(UART::Peripheral& printf_peripheral) { + HALAL::start(printf_peripheral); + STLIB_LOW::start(); + STLIB_HIGH::start(); } #endif // STLIB_ETH @@ -30,16 +38,15 @@ void STLIB::start(UART::Peripheral &printf_peripheral) { void STLIB::update() { #ifdef NDEBUG #ifdef HAL_IWDG_MODULE_ENABLED - Watchdog::refresh(); + Watchdog::refresh(); #endif #endif #if !defined STLIB_ETH #else - Ethernet::update(); - Server::update_servers(); + Ethernet::update(); + Server::update_servers(); #endif ErrorHandlerModel::ErrorHandlerUpdate(); MDMA::update(); - } diff --git a/Src/ST-LIB_HIGH/FlashStorer/FlashStorer.cpp b/Src/ST-LIB_HIGH/FlashStorer/FlashStorer.cpp index e9007eeb4..b7f9dbba7 100644 --- a/Src/ST-LIB_HIGH/FlashStorer/FlashStorer.cpp +++ b/Src/ST-LIB_HIGH/FlashStorer/FlashStorer.cpp @@ -5,40 +5,43 @@ vector FlashStorer::variable_list = {}; uint32_t FlashStorer::total_size = 0; -void FlashStorer::read(){ - uint32_t* data = (uint32_t*)malloc(FlashStorer::total_size); - uint32_t number_of_words = ((FlashStorer::total_size + FLASH_32BITS_WORLD - 1) / FLASH_32BITS_WORLD); //Fast ceiling the dvision result +void FlashStorer::read() { + uint32_t* data = (uint32_t*)malloc(FlashStorer::total_size); + uint32_t number_of_words = + ((FlashStorer::total_size + FLASH_32BITS_WORLD - 1) / FLASH_32BITS_WORLD + ); // Fast ceiling the dvision result - Flash::read(FLASH_STORER_START_ADDRESS, (uint32_t*)data, number_of_words); + Flash::read(FLASH_STORER_START_ADDRESS, (uint32_t*)data, number_of_words); - uint32_t offset = 0; - for(FlashVariable& v: FlashStorer::variable_list){ - memcpy(v.get_pointer(), (((uint8_t*)data) + offset), v.get_size()); - offset += v.get_size(); - } + uint32_t offset = 0; + for (FlashVariable& v : FlashStorer::variable_list) { + memcpy(v.get_pointer(), (((uint8_t*)data) + offset), v.get_size()); + offset += v.get_size(); + } - free(data); + free(data); } -bool FlashStorer::store_all(){ +bool FlashStorer::store_all() { - uint32_t number_of_words = ((FlashStorer::total_size + FLASH_32BITS_WORLD - 1) / FLASH_32BITS_WORLD); //Fast ceiling the dvision result + uint32_t number_of_words = + ((FlashStorer::total_size + FLASH_32BITS_WORLD - 1) / FLASH_32BITS_WORLD + ); // Fast ceiling the dvision result - uint32_t* data = (uint32_t*)malloc(number_of_words * FLASH_32BITS_WORLD); - uint32_t offset = 0; - for(FlashVariable& v: FlashStorer::variable_list){ - memcpy((((uint8_t*)data) + offset), v.get_pointer(), v.get_size()); - offset += v.get_size(); - } + uint32_t* data = (uint32_t*)malloc(number_of_words * FLASH_32BITS_WORLD); + uint32_t offset = 0; + for (FlashVariable& v : FlashStorer::variable_list) { + memcpy((((uint8_t*)data) + offset), v.get_pointer(), v.get_size()); + offset += v.get_size(); + } - if(not Flash::write(data, FLASH_STORER_START_ADDRESS, number_of_words)){ - return false; - } + if (not Flash::write(data, FLASH_STORER_START_ADDRESS, number_of_words)) { + return false; + } - free(data); + free(data); - return true; + return true; } -#endif //SIM_ON - +#endif // SIM_ON diff --git a/Src/ST-LIB_HIGH/FlashStorer/FlashVariable.cpp b/Src/ST-LIB_HIGH/FlashStorer/FlashVariable.cpp index d7b0c7873..c81631c32 100644 --- a/Src/ST-LIB_HIGH/FlashStorer/FlashVariable.cpp +++ b/Src/ST-LIB_HIGH/FlashStorer/FlashVariable.cpp @@ -1,11 +1,6 @@ #include "FlashStorer/FlashVariable.hpp" #include "ErrorHandler/ErrorHandler.hpp" +void* FlashVariable::get_pointer() { return FlashVariable::v_pointer; } -void* FlashVariable::get_pointer(){ - return FlashVariable::v_pointer; -} - -size_t FlashVariable::get_size(){ - return FlashVariable::size; -} +size_t FlashVariable::get_size() { return FlashVariable::size; } diff --git a/Src/ST-LIB_HIGH/Protections/Boundary.cpp b/Src/ST-LIB_HIGH/Protections/Boundary.cpp index 9b01b8c12..8dd8d39e5 100644 --- a/Src/ST-LIB_HIGH/Protections/Boundary.cpp +++ b/Src/ST-LIB_HIGH/Protections/Boundary.cpp @@ -1,16 +1,16 @@ #include "ST-LIB_HIGH/Protections/Boundary.hpp" - const map BoundaryInterface::format_look_up{ - {type_id,0}, - {type_id,1}, - {type_id,2}, - {type_id,3}, - {type_id,4}, - {type_id,5}, - {type_id,6}, - {type_id,7}, - {type_id,8}, - {type_id,9}, - {type_id,10}, - {type_id,11}, +const map BoundaryInterface::format_look_up{ + {type_id, 0}, + {type_id, 1}, + {type_id, 2}, + {type_id, 3}, + {type_id, 4}, + {type_id, 5}, + {type_id, 6}, + {type_id, 7}, + {type_id, 8}, + {type_id, 9}, + {type_id, 10}, + {type_id, 11}, }; \ No newline at end of file diff --git a/Src/ST-LIB_HIGH/Protections/ProtectionManager.cpp b/Src/ST-LIB_HIGH/Protections/ProtectionManager.cpp index 0f93b1378..058f7c1de 100644 --- a/Src/ST-LIB_HIGH/Protections/ProtectionManager.cpp +++ b/Src/ST-LIB_HIGH/Protections/ProtectionManager.cpp @@ -5,12 +5,9 @@ #include "Protections/Notification.hpp" IStateMachine* ProtectionManager::general_state_machine = nullptr; -Notification ProtectionManager::fault_notification = { - ProtectionManager::fault_id, nullptr}; -Notification ProtectionManager::warning_notification = { - ProtectionManager::warning_id, nullptr}; -StackOrder<0> ProtectionManager::fault_order(Protections::FAULT, - external_to_fault); +Notification ProtectionManager::fault_notification = {ProtectionManager::fault_id, nullptr}; +Notification ProtectionManager::warning_notification = {ProtectionManager::warning_id, nullptr}; +StackOrder<0> ProtectionManager::fault_order(Protections::FAULT, external_to_fault); uint64_t ProtectionManager::last_notify = 0; bool ProtectionManager::external_trigger = false; bool ProtectionManager::test_fault = false; @@ -35,12 +32,12 @@ void ProtectionManager::add_standard_protections() { add_protection(info_warning, Boundary(info_warning)); } -void ProtectionManager::set_id(Boards::ID board_id) { - ProtectionManager::board_id = board_id; -} +void ProtectionManager::set_id(Boards::ID board_id) { ProtectionManager::board_id = board_id; } -void ProtectionManager::link_state_machine(IStateMachine& general_state_machine, - state_id fault_id) { +void ProtectionManager::link_state_machine( + IStateMachine& general_state_machine, + state_id fault_id +) { ProtectionManager::general_state_machine = &general_state_machine; ProtectionManager::fault_state_id = fault_id; } @@ -64,8 +61,7 @@ void ProtectionManager::external_to_fault() { } void ProtectionManager::fault_and_propagate() { - ProtectionManager::general_state_machine->force_change_state( - fault_state_id); + ProtectionManager::general_state_machine->force_change_state(fault_state_id); propagate_fault(); } @@ -74,9 +70,8 @@ void ProtectionManager::check_protections() { auto protection_status = protection.check_state(); if (general_state_machine == nullptr) { - ErrorHandler( - "Protection Manager does not have General State Machine " - "Linked"); + ErrorHandler("Protection Manager does not have General State Machine " + "Linked"); return; } // ensure we only go to FAULT if a FAULT was triggered, and not only a @@ -86,7 +81,8 @@ void ProtectionManager::check_protections() { ProtectionManager::to_fault(); } Global_RTC::update_rtc_data(); - if(Time::get_global_tick() > protection.get_last_notify_tick() + notify_delay_in_nanoseconds) { + if (Time::get_global_tick() > + protection.get_last_notify_tick() + notify_delay_in_nanoseconds) { ProtectionManager::notify(protection); protection.update_last_notify_tick(Time::get_global_tick()); } @@ -96,36 +92,35 @@ void ProtectionManager::check_protections() { void ProtectionManager::check_high_frequency_protections() { for (Protection& protection : high_frequency_protections) { if (general_state_machine == nullptr) { - ErrorHandler( - "Protection Manager does not have General State Machine " - "Linked"); + ErrorHandler("Protection Manager does not have General State Machine " + "Linked"); return; } if (protection.fault_type == Protections::FAULT) { ProtectionManager::to_fault(); - } + } Global_RTC::update_rtc_data(); - if(Time::get_global_tick() > protection.get_last_notify_tick() + notify_delay_in_nanoseconds) { + if (Time::get_global_tick() > + protection.get_last_notify_tick() + notify_delay_in_nanoseconds) { ProtectionManager::notify(protection); protection.update_last_notify_tick(Time::get_global_tick()); } } } -void ProtectionManager::warn(string message) { - warning_notification.notify(message); -} +void ProtectionManager::warn(string message) { warning_notification.notify(message); } void ProtectionManager::notify(Protection& protection) { for (OrderProtocol* socket : OrderProtocol::sockets) { if (protection.fault_protection) { if (protection.fault_protection->boundary_type_id == ERROR_HANDLER) { - protection.fault_protection->update_error_handler_message( - protection.fault_protection->get_error_handler_string()); - } - socket->send_order(*protection.fault_protection->fault_message); - ErrorHandlerModel::error_to_communicate = false; + protection.fault_protection->update_error_handler_message( + protection.fault_protection->get_error_handler_string() + ); + } + socket->send_order(*protection.fault_protection->fault_message); + ErrorHandlerModel::error_to_communicate = false; } for (auto& warning : protection.warnings_triggered) { if (warning->boundary_type_id == INFO_WARNING - 2) { @@ -146,9 +141,9 @@ void ProtectionManager::propagate_fault() { for (OrderProtocol* socket : OrderProtocol::sockets) { socket->send_order(ProtectionManager::fault_order); } - for(const auto& [key,value] : FDCAN::registered_fdcan){ - FDCAN::transmit(key,FDCAN::ID::FAULT_ID,NULL); - } + for (const auto& [key, value] : FDCAN::registered_fdcan) { + FDCAN::transmit(key, FDCAN::ID::FAULT_ID, NULL); + } } Boards::ID ProtectionManager::board_id = Boards::ID::NOBOARD; diff --git a/Src/ST-LIB_HIGH/ST-LIB_HIGH.cpp b/Src/ST-LIB_HIGH/ST-LIB_HIGH.cpp index 85d0c31be..d67acbb21 100644 --- a/Src/ST-LIB_HIGH/ST-LIB_HIGH.cpp +++ b/Src/ST-LIB_HIGH/ST-LIB_HIGH.cpp @@ -8,5 +8,5 @@ #include "ST-LIB_HIGH.hpp" void STLIB_HIGH::start() { - // ProtectionManager::add_standard_protections(); + // ProtectionManager::add_standard_protections(); } diff --git a/Src/ST-LIB_LOW/Clocks/Counter.cpp b/Src/ST-LIB_LOW/Clocks/Counter.cpp index 2644c7c88..2f4d9896f 100644 --- a/Src/ST-LIB_LOW/Clocks/Counter.cpp +++ b/Src/ST-LIB_LOW/Clocks/Counter.cpp @@ -9,19 +9,16 @@ #include "HALAL/Services/Time/Time.hpp" -void Counter::update(){ - this->freq = (this->counter + 0.0)/(this->update_period_ms / 1000.0); - this->counter = 0; +void Counter::update() { + this->freq = (this->counter + 0.0) / (this->update_period_ms / 1000.0); + this->counter = 0; } -Counter::Counter(uint64_t update_period_ms) : update_period_ms(update_period_ms){ - this->time_id = Time::register_low_precision_alarm(this->update_period_ms,[&](){ this->update(); }); +Counter::Counter(uint64_t update_period_ms) : update_period_ms(update_period_ms) { + this->time_id = + Time::register_low_precision_alarm(this->update_period_ms, [&]() { this->update(); }); } -Counter::~Counter(){ - Time::unregister_low_precision_alarm(this->time_id); -} +Counter::~Counter() { Time::unregister_low_precision_alarm(this->time_id); } -void Counter::count(){ - this->counter++; -} +void Counter::count() { this->counter++; } diff --git a/Src/ST-LIB_LOW/Clocks/Stopwatch.cpp b/Src/ST-LIB_LOW/Clocks/Stopwatch.cpp index c6fab13b2..44935b563 100644 --- a/Src/ST-LIB_LOW/Clocks/Stopwatch.cpp +++ b/Src/ST-LIB_LOW/Clocks/Stopwatch.cpp @@ -9,18 +9,15 @@ #include "HALAL/Services/Time/Time.hpp" +void Stopwatch::start(const string id) { start_times[id] = Time::get_global_tick(); } -void Stopwatch::start(const string id){ - start_times[id] = Time::get_global_tick(); -} - -uint64_t Stopwatch::stop(const string id){ - if(not start_times.contains(id)){ - ErrorHandler("No encoder registered with id %u", id); - return 0; - } +uint64_t Stopwatch::stop(const string id) { + if (not start_times.contains(id)) { + ErrorHandler("No encoder registered with id %u", id); + return 0; + } - uint64_t result = Time::get_global_tick() - start_times[id]; - start(id); - return result; + uint64_t result = Time::get_global_tick() - start_times[id]; + start(id); + return result; } diff --git a/Src/ST-LIB_LOW/Communication/Server/Server.cpp b/Src/ST-LIB_LOW/Communication/Server/Server.cpp index 66b5cd99a..2638bf29a 100644 --- a/Src/ST-LIB_LOW/Communication/Server/Server.cpp +++ b/Src/ST-LIB_LOW/Communication/Server/Server.cpp @@ -9,68 +9,75 @@ vector Server::running_servers = {}; -Server::Server(IPV4 local_ip, uint32_t local_port): local_ip(local_ip), local_port(local_port), status(RUNNING){ - open_connection = new ServerSocket(local_ip, local_port); - running_servers.push_back(this); - running_connections = {}; - running_connections_count = 0; +Server::Server(IPV4 local_ip, uint32_t local_port) + : local_ip(local_ip), local_port(local_port), status(RUNNING) { + open_connection = new ServerSocket(local_ip, local_port); + running_servers.push_back(this); + running_connections = {}; + running_connections_count = 0; } -Server::~Server(){ - open_connection->~ServerSocket(); +Server::~Server() { + open_connection->~ServerSocket(); - for(ServerSocket *s : running_connections){ - s->~ServerSocket(); - } + for (ServerSocket* s : running_connections) { + s->~ServerSocket(); + } - running_servers.erase(find(running_servers.begin(), running_servers.end(), this)); + running_servers.erase(find(running_servers.begin(), running_servers.end(), this)); } -void Server::update(){ - if(open_connection->is_connected()){ - running_connections[running_connections_count] = open_connection; - running_connections_count++; - open_connection = new ServerSocket(local_ip, local_port); - } +void Server::update() { + if (open_connection->is_connected()) { + running_connections[running_connections_count] = open_connection; + running_connections_count++; + open_connection = new ServerSocket(local_ip, local_port); + } - for(uint16_t s = 0; s < running_connections_count; s++){ - if(status == RUNNING && !running_connections[s]->is_connected()){ - ErrorHandler("ip %s disconnected, going to FAULT",running_connections[s]->remote_ip.string_address.c_str()); - status = CLOSING; - break; - } - } + for (uint16_t s = 0; s < running_connections_count; s++) { + if (status == RUNNING && !running_connections[s]->is_connected()) { + ErrorHandler( + "ip %s disconnected, going to FAULT", + running_connections[s]->remote_ip.string_address.c_str() + ); + status = CLOSING; + break; + } + } - if(status == CLOSING){ - close_all(); - } + if (status == CLOSING) { + close_all(); + } } -void Server::broadcast_order(Order& order){ - for(uint16_t s = 0; s < running_connections_count; s++){ - if(running_connections[s]->send_order(order)){ - ErrorHandler("Couldn t put Order %d into buffer of ip's %s ServerSocket, buffer may be full or the ServerSocket may be ill formed",order.get_id(),running_connections[s]->remote_ip.string_address.c_str()); - } - } +void Server::broadcast_order(Order& order) { + for (uint16_t s = 0; s < running_connections_count; s++) { + if (running_connections[s]->send_order(order)) { + ErrorHandler( + "Couldn t put Order %d into buffer of ip's %s ServerSocket, buffer may be full or " + "the ServerSocket may be ill formed", + order.get_id(), + running_connections[s]->remote_ip.string_address.c_str() + ); + } + } } -void Server::close_all(){ - for(uint16_t s = 0; s < running_connections_count; s++){ - running_connections[s]->close(); - running_connections[s] = nullptr; - } - running_connections_count = 0; - status = CLOSED; +void Server::close_all() { + for (uint16_t s = 0; s < running_connections_count; s++) { + running_connections[s]->close(); + running_connections[s] = nullptr; + } + running_connections_count = 0; + status = CLOSED; } -uint32_t Server::connections_count(){ - return running_connections_count; -} +uint32_t Server::connections_count() { return running_connections_count; } -void Server::update_servers(){ - for(Server *s : running_servers){ - s->update(); - } +void Server::update_servers() { + for (Server* s : running_servers) { + s->update(); + } } -#endif //STLIB_ETH +#endif // STLIB_ETH diff --git a/Src/ST-LIB_LOW/DigitalOutput/DigitalOutput.cpp b/Src/ST-LIB_LOW/DigitalOutput/DigitalOutput.cpp index 9348a3607..dff6cd83e 100644 --- a/Src/ST-LIB_LOW/DigitalOutput/DigitalOutput.cpp +++ b/Src/ST-LIB_LOW/DigitalOutput/DigitalOutput.cpp @@ -9,8 +9,7 @@ #include "HALAL/Services/DigitalOutputService/DigitalOutputService.hpp" -DigitalOutput::DigitalOutput(Pin& pin) - : pin(pin), id(DigitalOutputService::inscribe(pin)) {} +DigitalOutput::DigitalOutput(Pin& pin) : pin(pin), id(DigitalOutputService::inscribe(pin)) {} void DigitalOutput::turn_on() { DigitalOutputService::turn_on(id); } diff --git a/Src/ST-LIB_LOW/ErrorHandler/ErrorHandler.cpp b/Src/ST-LIB_LOW/ErrorHandler/ErrorHandler.cpp index 93ccc0300..df73b875b 100644 --- a/Src/ST-LIB_LOW/ErrorHandler/ErrorHandler.cpp +++ b/Src/ST-LIB_LOW/ErrorHandler/ErrorHandler.cpp @@ -14,56 +14,57 @@ string ErrorHandlerModel::file = "Error-No-File-Found"; double ErrorHandlerModel::error_triggered = 0; bool ErrorHandlerModel::error_to_communicate = false; -void ErrorHandlerModel::SetMetaData(int line, const char * func, const char * file){ - ErrorHandlerModel::line = to_string(line); - ErrorHandlerModel::func = string(func); - ErrorHandlerModel::file = string(file); +void ErrorHandlerModel::SetMetaData(int line, const char* func, const char* file) { + ErrorHandlerModel::line = to_string(line); + ErrorHandlerModel::func = string(func); + ErrorHandlerModel::file = string(file); } -void ErrorHandlerModel::ErrorHandlerTrigger(string format, ... ){ - if (ErrorHandlerModel::error_triggered) { - return; - } +void ErrorHandlerModel::ErrorHandlerTrigger(string format, ...) { + if (ErrorHandlerModel::error_triggered) { + return; + } - ErrorHandlerModel::error_triggered = 1.0; - ErrorHandlerModel::error_to_communicate = true; //This flag is marked so the ProtectionManager can know if it already consumed the error in question. + ErrorHandlerModel::error_triggered = 1.0; + ErrorHandlerModel::error_to_communicate = + true; // This flag is marked so the ProtectionManager can know if it already consumed the + // error in question. - if (format.length() != 0) { - description = ""; - } + if (format.length() != 0) { + description = ""; + } - va_list arguments; - va_start(arguments, format); - va_list arg_copy; - va_copy(arg_copy, arguments); + va_list arguments; + va_start(arguments, format); + va_list arg_copy; + va_copy(arg_copy, arguments); - const int32_t size = vsnprintf(nullptr, 0, format.c_str(), arguments) + 1; - const unique_ptr buffer = make_unique(size); - va_end(arguments); + const int32_t size = vsnprintf(nullptr, 0, format.c_str(), arguments) + 1; + const unique_ptr buffer = make_unique(size); + va_end(arguments); - vsnprintf(buffer.get(), size, format.c_str(), arg_copy); - va_end(arg_copy); + vsnprintf(buffer.get(), size, format.c_str(), arg_copy); + va_end(arg_copy); - description += string(buffer.get(), buffer.get() + size - 1) + " | Line: " + ErrorHandlerModel::line - + " Function: '" + ErrorHandlerModel::func + "' File: " + ErrorHandlerModel::file ; + description += string(buffer.get(), buffer.get() + size - 1) + + " | Line: " + ErrorHandlerModel::line + " Function: '" + + ErrorHandlerModel::func + "' File: " + ErrorHandlerModel::file; #ifdef HAL_TIM_MODULE_ENABLED - description += " | TimeStamp: " + to_string(Time::get_global_tick()); + description += " | TimeStamp: " + to_string(Time::get_global_tick()); #endif - } -void ErrorHandlerModel::ErrorHandlerUpdate(){ - if (!ErrorHandlerModel::error_triggered) { - return; - } +void ErrorHandlerModel::ErrorHandlerUpdate() { + if (!ErrorHandlerModel::error_triggered) { + return; + } #ifdef HAL_UART_MODULE_ENABLED - if (!UART::printf_ready) { - return; - } + if (!UART::printf_ready) { + return; + } - printf("Error: %s%s", ErrorHandlerModel::description.c_str(), endl); + printf("Error: %s%s", ErrorHandlerModel::description.c_str(), endl); #endif - } diff --git a/Src/ST-LIB_LOW/HalfBridge/HalfBridge.cpp b/Src/ST-LIB_LOW/HalfBridge/HalfBridge.cpp index 6e4cc03e2..f1934027a 100644 --- a/Src/ST-LIB_LOW/HalfBridge/HalfBridge.cpp +++ b/Src/ST-LIB_LOW/HalfBridge/HalfBridge.cpp @@ -9,52 +9,57 @@ #include "HALAL/Services/DigitalOutputService/DigitalOutputService.hpp" -HalfBridge::HalfBridge(Pin& positive_pwm_pin, Pin& positive_pwm_negated_pin, - Pin& negative_pwm_pin, Pin& negative_pwm_negated_pin, Pin& enable_pin) : is_dual(true), - positive_pwm{positive_pwm_pin, positive_pwm_negated_pin}, - negative_pwm{negative_pwm_pin, negative_pwm_negated_pin} -{ - HalfBridge::enable = DigitalOutputService::inscribe(enable_pin); +HalfBridge::HalfBridge( + Pin& positive_pwm_pin, + Pin& positive_pwm_negated_pin, + Pin& negative_pwm_pin, + Pin& negative_pwm_negated_pin, + Pin& enable_pin +) + : is_dual(true), positive_pwm{positive_pwm_pin, positive_pwm_negated_pin}, + negative_pwm{negative_pwm_pin, negative_pwm_negated_pin} { + HalfBridge::enable = DigitalOutputService::inscribe(enable_pin); } void HalfBridge::turn_on() { - positive_pwm.turn_on(); - negative_pwm.turn_on(); - DigitalOutputService::turn_on(enable); // enable at the end to avoid noise - DigitalOutputService::set_pin_state(enable, PinState::OFF); + positive_pwm.turn_on(); + negative_pwm.turn_on(); + DigitalOutputService::turn_on(enable); // enable at the end to avoid noise + DigitalOutputService::set_pin_state(enable, PinState::OFF); } void HalfBridge::turn_off() { - DigitalOutputService::turn_off(enable); // enable at the start to avoid noise - positive_pwm.turn_off(); - negative_pwm.turn_off(); + DigitalOutputService::turn_off(enable); // enable at the start to avoid noise + positive_pwm.turn_off(); + negative_pwm.turn_off(); } void HalfBridge::set_duty_cycle(float duty_cycle) { - if(duty_cycle != 50){ - ErrorHandler("Cannot modify duty cycle in HalfBridge"); - return; - } - positive_pwm.set_duty_cycle(duty_cycle); - negative_pwm.set_duty_cycle(duty_cycle); + if (duty_cycle != 50) { + ErrorHandler("Cannot modify duty cycle in HalfBridge"); + return; + } + positive_pwm.set_duty_cycle(duty_cycle); + negative_pwm.set_duty_cycle(duty_cycle); } void HalfBridge::set_frequency(int32_t frequency) { - positive_pwm.set_frequency(frequency); - negative_pwm.set_frequency(frequency); + positive_pwm.set_frequency(frequency); + negative_pwm.set_frequency(frequency); } void HalfBridge::set_phase(float phase) { - positive_pwm.set_phase(0); - negative_pwm.set_phase(phase); + positive_pwm.set_phase(0); + negative_pwm.set_phase(phase); } -void HalfBridge::set_negative_pwm_phase(float phase){ - negative_pwm.set_phase(positive_pwm.get_phase() + phase); +void HalfBridge::set_negative_pwm_phase(float phase) { + negative_pwm.set_phase(positive_pwm.get_phase() + phase); } -void HalfBridge::set_positive_pwm_phase(float phase){ - positive_pwm.set_phase(negative_pwm.get_phase() + phase); +void HalfBridge::set_positive_pwm_phase(float phase) { + positive_pwm.set_phase(negative_pwm.get_phase() + phase); } float HalfBridge::get_phase() { - return negative_pwm.get_phase();; + return negative_pwm.get_phase(); + ; } diff --git a/Src/ST-LIB_LOW/Math/Math.cpp b/Src/ST-LIB_LOW/Math/Math.cpp index fb80fbc53..d663dd79a 100644 --- a/Src/ST-LIB_LOW/Math/Math.cpp +++ b/Src/ST-LIB_LOW/Math/Math.cpp @@ -5,150 +5,133 @@ #ifndef SIM_ON std::array Math::pointers = {0}; -int32_t Math::sin(int32_t angle){ - pointers[0] = angle; - RotationComputer::sin(&pointers[0],&pointers[1],1); - return pointers[1]; +int32_t Math::sin(int32_t angle) { + pointers[0] = angle; + RotationComputer::sin(&pointers[0], &pointers[1], 1); + return pointers[1]; } -int32_t Math::cos(int32_t angle){ - pointers[0] = angle; - RotationComputer::cos(&pointers[0],&pointers[1],1); - return pointers[1]; +int32_t Math::cos(int32_t angle) { + pointers[0] = angle; + RotationComputer::cos(&pointers[0], &pointers[1], 1); + return pointers[1]; } -int32_t Math::tg(int32_t angle){ - pointers[0] = angle; - RotationComputer::cos_and_sin(&pointers[0],&pointers[1],&pointers[2],1); - if(pointers[1] > MIN_COS_MARGIN_TG || pointers[1] < -MIN_COS_MARGIN_TG){ - return pointers[2] / (pointers[1] >> TG_DECIMAL_BITS); +int32_t Math::tg(int32_t angle) { + pointers[0] = angle; + RotationComputer::cos_and_sin(&pointers[0], &pointers[1], &pointers[2], 1); + if (pointers[1] > MIN_COS_MARGIN_TG || pointers[1] < -MIN_COS_MARGIN_TG) { + return pointers[2] / (pointers[1] >> TG_DECIMAL_BITS); } - if((pointers[1] >= 0) == (pointers[2]>=0)){ - return MAX_INT; + if ((pointers[1] >= 0) == (pointers[2] >= 0)) { + return MAX_INT; } - return -MAX_INT-1; + return -MAX_INT - 1; } -int32_t Math::phase(int32_t x, int32_t y){ - pointers[0] = x; - pointers[1] = y; - RotationComputer::phase(&pointers[0], &pointers[1], &pointers[2], 1); - return pointers[2]; +int32_t Math::phase(int32_t x, int32_t y) { + pointers[0] = x; + pointers[1] = y; + RotationComputer::phase(&pointers[0], &pointers[1], &pointers[2], 1); + return pointers[2]; } -uint32_t Math::modulus(int32_t x, int32_t y){ - if(((uint32_t) x -4 + y) > MAX_MOD_MARGIN){ - pointers[0] = (x >> 1); - pointers[1] = (y >> 1); - RotationComputer::modulus(&pointers[0], &pointers[1], &pointers[2], 1); - uint32_t mod= pointers[2]; - mod = (mod << 1); - return mod; - } - pointers[0] = x; - pointers[1] = y; - RotationComputer::modulus(&pointers[0], &pointers[1], &pointers[2], 1); - return pointers[2]; -} - -int32_t Math::atg(int32_t in){ - pointers[0] = 1 << TG_DECIMAL_BITS; - pointers[1] = in; - RotationComputer::phase(&pointers[0], &pointers[1], &pointers[2], 1); - return pointers[2]; -} - - -int32_t Math::sqrt(int32_t sq_in){ - int32_t sol = 0; - int32_t temp = 0; - int32_t over = sq_in >> SQ_DECIMAL_BITS; - for(int32_t i = 0x8000 >> (SQ_DECIMAL_BITS >> 1); i >= 1; i = i >> 1){ - temp = (sol + i); - if(((temp*temp)) <= over){ - sol = sol + i; - } - } +uint32_t Math::modulus(int32_t x, int32_t y) { + if (((uint32_t)x - 4 + y) > MAX_MOD_MARGIN) { + pointers[0] = (x >> 1); + pointers[1] = (y >> 1); + RotationComputer::modulus(&pointers[0], &pointers[1], &pointers[2], 1); + uint32_t mod = pointers[2]; + mod = (mod << 1); + return mod; + } + pointers[0] = x; + pointers[1] = y; + RotationComputer::modulus(&pointers[0], &pointers[1], &pointers[2], 1); + return pointers[2]; +} + +int32_t Math::atg(int32_t in) { + pointers[0] = 1 << TG_DECIMAL_BITS; + pointers[1] = in; + RotationComputer::phase(&pointers[0], &pointers[1], &pointers[2], 1); + return pointers[2]; +} + +int32_t Math::sqrt(int32_t sq_in) { + int32_t sol = 0; + int32_t temp = 0; + int32_t over = sq_in >> SQ_DECIMAL_BITS; + for (int32_t i = 0x8000 >> (SQ_DECIMAL_BITS >> 1); i >= 1; i = i >> 1) { + temp = (sol + i); + if (((temp * temp)) <= over) { + sol = sol + i; + } + } - return sol<> (32 - SQ_DECIMAL_BITS); -} +int32_t Math::unitary_to_sq(int32_t in) { return in >> (32 - SQ_DECIMAL_BITS); } -int32_t Math::tg_to_unitary(int32_t tg_in){ - return tg_in << (32 - TG_DECIMAL_BITS); -} +int32_t Math::tg_to_unitary(int32_t tg_in) { return tg_in << (32 - TG_DECIMAL_BITS); } -int32_t Math::unitary_to_tg(int32_t in){ - return in >> (32 - TG_DECIMAL_BITS); -} +int32_t Math::unitary_to_tg(int32_t in) { return in >> (32 - TG_DECIMAL_BITS); } #else #include -int32_t Math::sin(int32_t angle){ - double rad_angle= angle*M_PI/180.0; - return static_cast(std::sin(rad_angle)*(1<(std::sin(rad_angle) * (1 << TG_DECIMAL_BITS)); } -int32_t Math::cos(int32_t angle){ - double rad_angle= angle*M_PI/180.0; - return static_cast(std::cos(rad_angle)*(1<(std::cos(rad_angle) * (1 << TG_DECIMAL_BITS)); } -int32_t Math::tg(int32_t angle){ - double rad_angle= angle*M_PI/180.0; - return static_cast(std::tan(rad_angle)*(1<(std::tan(rad_angle) * (1 << TG_DECIMAL_BITS)); } -int32_t Math::phase(int32_t x, int32_t y){ - double rad_angle= std::atan2(y,x); - return static_cast(rad_angle*(1<(rad_angle * (1 << TG_DECIMAL_BITS)); } -uint32_t Math::modulus(int32_t x, int32_t y){ - if(x-4+y > MAX_MOD_MARGIN){ - x=x>>1; - y=y>>1; - } - double x_double = static_cast(x); +uint32_t Math::modulus(int32_t x, int32_t y) { + if (x - 4 + y > MAX_MOD_MARGIN) { + x = x >> 1; + y = y >> 1; + } + double x_double = static_cast(x); double y_double = static_cast(y); - double x_squared=x_double*x_double; - double y_squared=y_double*y_double; - double mod= std::sqrt(x_squared+y_squared); - return static_cast(mod*(1<(mod * (1 << SQ_DECIMAL_BITS)); } -int32_t Math::atg(int32_t tg_in){ - double rad_angle= std::atan(tg_in); - return static_cast(rad_angle*(1<(rad_angle * (1 << TG_DECIMAL_BITS)); } -int32_t Math::sqrt(int32_t sq_in){ - double decimal_sq_in= static_cast(sq_in); - return static_cast(std::sqrt(decimal_sq_in)*(1<(sq_in); + return static_cast(std::sqrt(decimal_sq_in) * (1 << SQ_DECIMAL_BITS)); } -int32_t Math::sq_to_unitary(int32_t sq_in){ - return sq_in << (32 - SQ_DECIMAL_BITS); -} +int32_t Math::sq_to_unitary(int32_t sq_in) { return sq_in << (32 - SQ_DECIMAL_BITS); } -int32_t Math::unitary_to_sq(int32_t in){ - return in >> (32 - SQ_DECIMAL_BITS); -} +int32_t Math::unitary_to_sq(int32_t in) { return in >> (32 - SQ_DECIMAL_BITS); } -int32_t Math::tg_to_unitary(int32_t tg_in){ - return tg_in << (32 - TG_DECIMAL_BITS); -} +int32_t Math::tg_to_unitary(int32_t tg_in) { return tg_in << (32 - TG_DECIMAL_BITS); } -int32_t Math::unitary_to_tg(int32_t in){ - return in >> (32 - TG_DECIMAL_BITS); -} +int32_t Math::unitary_to_tg(int32_t in) { return in >> (32 - TG_DECIMAL_BITS); } -#endif //SIM_ON +#endif // SIM_ON diff --git a/Src/ST-LIB_LOW/ST-LIB_LOW.cpp b/Src/ST-LIB_LOW/ST-LIB_LOW.cpp index 92077a98e..7d2b43cb3 100644 --- a/Src/ST-LIB_LOW/ST-LIB_LOW.cpp +++ b/Src/ST-LIB_LOW/ST-LIB_LOW.cpp @@ -7,6 +7,4 @@ #include "ST-LIB_LOW.hpp" -void STLIB_LOW::start() { - Sensor::start(); -} +void STLIB_LOW::start() { Sensor::start(); } diff --git a/Src/ST-LIB_LOW/Sd/Sd.cpp b/Src/ST-LIB_LOW/Sd/Sd.cpp index f105bb704..0eeacb54f 100644 --- a/Src/ST-LIB_LOW/Sd/Sd.cpp +++ b/Src/ST-LIB_LOW/Sd/Sd.cpp @@ -1,9 +1,9 @@ #include "ST-LIB_LOW/Sd/Sd.hpp" -SD_HandleTypeDef *g_sdmmc1_handle = nullptr; -SD_HandleTypeDef *g_sdmmc2_handle = nullptr; -void *g_sdmmc1_instance_ptr = nullptr; -void *g_sdmmc2_instance_ptr = nullptr; +SD_HandleTypeDef* g_sdmmc1_handle = nullptr; +SD_HandleTypeDef* g_sdmmc2_handle = nullptr; +void* g_sdmmc1_instance_ptr = nullptr; +void* g_sdmmc2_instance_ptr = nullptr; using namespace ST_LIB; @@ -27,16 +27,24 @@ void SdDomain::Instance::on_abort() { ErrorHandler("SD Card operation aborted"); void SdDomain::Instance::on_error() { ErrorHandler("SD Card error occurred"); } -bool SdDomain::Instance::is_card_present() { return cd_instance->first->read() == cd_instance->second; } -bool SdDomain::Instance::is_write_protected() { return wp_instance->first->read() == wp_instance->second; } +bool SdDomain::Instance::is_card_present() { + return cd_instance->first->read() == cd_instance->second; +} +bool SdDomain::Instance::is_write_protected() { + return wp_instance->first->read() == wp_instance->second; +} -bool SdDomain::Instance::is_busy() { - if (!card_initialized) return false; - return (hsd.State == HAL_SD_STATE_BUSY) || (hsd.State == HAL_SD_STATE_PROGRAMMING) || (hsd.State == HAL_SD_STATE_RECEIVING); +bool SdDomain::Instance::is_busy() { + if (!card_initialized) + return false; + return (hsd.State == HAL_SD_STATE_BUSY) || (hsd.State == HAL_SD_STATE_PROGRAMMING) || + (hsd.State == HAL_SD_STATE_RECEIVING); } bool SdDomain::Instance::initialize_card() { - if (card_initialized) { return true; } // Already initialized + if (card_initialized) { + return true; + } // Already initialized HAL_StatusTypeDef status = HAL_SD_Init(&hsd); if (status != HAL_OK) { @@ -60,7 +68,9 @@ bool SdDomain::Instance::initialize_card() { } bool SdDomain::Instance::deinitialize_card() { - if (!card_initialized) { return true; } // Already deinitialized + if (!card_initialized) { + return true; + } // Already deinitialized HAL_StatusTypeDef status = HAL_SD_DeInit(&hsd); if (status != HAL_OK) { @@ -72,20 +82,28 @@ bool SdDomain::Instance::deinitialize_card() { } void SdDomain::Instance::switch_buffer() { - current_buffer = (current_buffer == BufferSelect::Buffer0) ? BufferSelect::Buffer1 : BufferSelect::Buffer0; + current_buffer = + (current_buffer == BufferSelect::Buffer0) ? BufferSelect::Buffer1 : BufferSelect::Buffer0; } bool SdDomain::Instance::configure_idma() { - HAL_StatusTypeDef status = HAL_SDEx_ConfigDMAMultiBuffer(&hsd, + HAL_StatusTypeDef status = HAL_SDEx_ConfigDMAMultiBuffer( + &hsd, reinterpret_cast(mpu_buffer0_instance->ptr), reinterpret_cast(mpu_buffer1_instance->ptr), - mpu_buffer0_instance->size / 512); // Number of 512B-blocks + mpu_buffer0_instance->size / 512 + ); // Number of 512B-blocks - if (status != HAL_OK) { return false; } + if (status != HAL_OK) { + return false; + } return true; } -HAL_StatusTypeDef SdDomain::Instance::Not_HAL_SDEx_ReadBlocksDMAMultiBuffer(uint32_t BlockAdd, uint32_t NumberOfBlocks) { +HAL_StatusTypeDef SdDomain::Instance::Not_HAL_SDEx_ReadBlocksDMAMultiBuffer( + uint32_t BlockAdd, + uint32_t NumberOfBlocks +) { SDMMC_DataInitTypeDef config; uint32_t errorstate; uint32_t DmaBase0_reg; @@ -119,12 +137,12 @@ HAL_StatusTypeDef SdDomain::Instance::Not_HAL_SDEx_ReadBlocksDMAMultiBuffer(uint } /* Configure the SD DPSM (Data Path State Machine) */ - config.DataTimeOut = SDMMC_DATATIMEOUT; - config.DataLength = BLOCKSIZE * NumberOfBlocks; + config.DataTimeOut = SDMMC_DATATIMEOUT; + config.DataLength = BLOCKSIZE * NumberOfBlocks; config.DataBlockSize = SDMMC_DATABLOCK_SIZE_512B; - config.TransferDir = SDMMC_TRANSFER_DIR_TO_SDMMC; - config.TransferMode = SDMMC_TRANSFER_MODE_BLOCK; - config.DPSM = SDMMC_DPSM_DISABLE; + config.TransferDir = SDMMC_TRANSFER_DIR_TO_SDMMC; + config.TransferMode = SDMMC_TRANSFER_MODE_BLOCK; + config.DPSM = SDMMC_DPSM_DISABLE; (void)SDMMC_ConfigData(hsd.Instance, &config); // hsd.Instance->DCTRL |= SDMMC_DCTRL_FIFORST; @@ -148,17 +166,22 @@ HAL_StatusTypeDef SdDomain::Instance::Not_HAL_SDEx_ReadBlocksDMAMultiBuffer(uint return HAL_ERROR; } - __HAL_SD_ENABLE_IT(&hsd, (SDMMC_IT_DCRCFAIL | SDMMC_IT_DTIMEOUT | SDMMC_IT_RXOVERR | SDMMC_IT_DATAEND | - SDMMC_IT_IDMABTC)); + __HAL_SD_ENABLE_IT( + &hsd, + (SDMMC_IT_DCRCFAIL | SDMMC_IT_DTIMEOUT | SDMMC_IT_RXOVERR | SDMMC_IT_DATAEND | + SDMMC_IT_IDMABTC) + ); return HAL_OK; - } - else { + } else { return HAL_BUSY; } } -HAL_StatusTypeDef SdDomain::Instance::Not_HAL_SDEx_WriteBlocksDMAMultiBuffer(uint32_t BlockAdd, uint32_t NumberOfBlocks) { +HAL_StatusTypeDef SdDomain::Instance::Not_HAL_SDEx_WriteBlocksDMAMultiBuffer( + uint32_t BlockAdd, + uint32_t NumberOfBlocks +) { SDMMC_DataInitTypeDef config; uint32_t errorstate; uint32_t DmaBase0_reg; @@ -189,12 +212,12 @@ HAL_StatusTypeDef SdDomain::Instance::Not_HAL_SDEx_WriteBlocksDMAMultiBuffer(uin } /* Configure the SD DPSM (Data Path State Machine) */ - config.DataTimeOut = SDMMC_DATATIMEOUT; - config.DataLength = BLOCKSIZE * NumberOfBlocks; + config.DataTimeOut = SDMMC_DATATIMEOUT; + config.DataLength = BLOCKSIZE * NumberOfBlocks; config.DataBlockSize = SDMMC_DATABLOCK_SIZE_512B; - config.TransferDir = SDMMC_TRANSFER_DIR_TO_CARD; - config.TransferMode = SDMMC_TRANSFER_MODE_BLOCK; - config.DPSM = SDMMC_DPSM_DISABLE; + config.TransferDir = SDMMC_TRANSFER_DIR_TO_CARD; + config.TransferMode = SDMMC_TRANSFER_MODE_BLOCK; + config.DPSM = SDMMC_DPSM_DISABLE; (void)SDMMC_ConfigData(hsd.Instance, &config); // hsd.Instance->DCTRL |= SDMMC_DCTRL_FIFORST; @@ -219,20 +242,21 @@ HAL_StatusTypeDef SdDomain::Instance::Not_HAL_SDEx_WriteBlocksDMAMultiBuffer(uin return HAL_ERROR; } - __HAL_SD_ENABLE_IT(&hsd, (SDMMC_IT_DCRCFAIL | SDMMC_IT_DTIMEOUT | SDMMC_IT_TXUNDERR | SDMMC_IT_DATAEND | - SDMMC_IT_IDMABTC)); + __HAL_SD_ENABLE_IT( + &hsd, + (SDMMC_IT_DCRCFAIL | SDMMC_IT_DTIMEOUT | SDMMC_IT_TXUNDERR | SDMMC_IT_DATAEND | + SDMMC_IT_IDMABTC) + ); return HAL_OK; - } - else { + } else { return HAL_BUSY; } } - extern "C" { -void HAL_SD_MspInit(SD_HandleTypeDef *hsd) { +void HAL_SD_MspInit(SD_HandleTypeDef* hsd) { if (hsd->Instance == SDMMC1) { __HAL_RCC_SDMMC1_CLK_ENABLE(); __HAL_RCC_SDMMC1_FORCE_RESET(); @@ -260,19 +284,19 @@ void SDMMC2_IRQHandler(void) { } } -void HAL_SD_TxCpltCallback(SD_HandleTypeDef *hsd) { +void HAL_SD_TxCpltCallback(SD_HandleTypeDef* hsd) { if (auto sd_instance = ST_LIB::get_sd_instance(hsd)) { sd_instance->on_dma_write_complete(); } } -void HAL_SD_RxCpltCallback(SD_HandleTypeDef *hsd) { +void HAL_SD_RxCpltCallback(SD_HandleTypeDef* hsd) { if (auto sd_instance = ST_LIB::get_sd_instance(hsd)) { sd_instance->on_dma_read_complete(); } } -void HAL_SD_AbortCallback(SD_HandleTypeDef *hsd) { +void HAL_SD_AbortCallback(SD_HandleTypeDef* hsd) { if (auto sd_instance = ST_LIB::get_sd_instance(hsd)) { sd_instance->on_abort(); } else { @@ -280,7 +304,7 @@ void HAL_SD_AbortCallback(SD_HandleTypeDef *hsd) { } } -void HAL_SD_ErrorCallback(SD_HandleTypeDef *hsd) { +void HAL_SD_ErrorCallback(SD_HandleTypeDef* hsd) { if (auto sd_instance = ST_LIB::get_sd_instance(hsd)) { sd_instance->on_error(); } else { diff --git a/Src/ST-LIB_LOW/Sensors/DigitalSensor/DigitalSensor.cpp b/Src/ST-LIB_LOW/Sensors/DigitalSensor/DigitalSensor.cpp index 8f5c2ad6b..dceb33c96 100644 --- a/Src/ST-LIB_LOW/Sensors/DigitalSensor/DigitalSensor.cpp +++ b/Src/ST-LIB_LOW/Sensors/DigitalSensor/DigitalSensor.cpp @@ -2,17 +2,16 @@ #include "HALAL/Services/DigitalInputService/DigitalInputService.hpp" +DigitalSensor::DigitalSensor(Pin& pin, PinState* value) + : id(DigitalInput::inscribe(pin)), value(value) {} -DigitalSensor::DigitalSensor(Pin &pin, PinState *value) : id(DigitalInput::inscribe(pin)), value(value){} +DigitalSensor::DigitalSensor(Pin& pin, PinState& value) + : DigitalSensor::DigitalSensor(pin, &value) {} -DigitalSensor::DigitalSensor(Pin &pin, PinState &value) : DigitalSensor::DigitalSensor(pin,&value){} +void DigitalSensor::read() { + PinState val = DigitalInput::read_pin_state(id); -void DigitalSensor::read(){ - PinState val = DigitalInput::read_pin_state(id); - - *DigitalSensor::value = val; + *DigitalSensor::value = val; } -uint8_t DigitalSensor::get_id(){ - return id; -} +uint8_t DigitalSensor::get_id() { return id; } diff --git a/Src/ST-LIB_LOW/Sensors/LookupSensor/LookupSensor.cpp b/Src/ST-LIB_LOW/Sensors/LookupSensor/LookupSensor.cpp index d9dfcab47..5d557ecd1 100644 --- a/Src/ST-LIB_LOW/Sensors/LookupSensor/LookupSensor.cpp +++ b/Src/ST-LIB_LOW/Sensors/LookupSensor/LookupSensor.cpp @@ -1,22 +1,31 @@ #include "Sensors/LookupSensor/LookupSensor.hpp" +LookupSensor::LookupSensor( + ST_LIB::ADCDomain::Instance& adc, + double* table, + int table_size, + double* value +) + : adc(&adc), table(table), table_size(table_size), value(value) {} -LookupSensor::LookupSensor(ST_LIB::ADCDomain::Instance& adc, double *table, int table_size, double *value) - : adc(&adc), table(table), table_size(table_size), value(value){} +LookupSensor::LookupSensor( + ST_LIB::ADCDomain::Instance& adc, + double* table, + int table_size, + double& value +) + : LookupSensor::LookupSensor(adc, table, table_size, &value) {} -LookupSensor::LookupSensor(ST_LIB::ADCDomain::Instance& adc, double *table, int table_size, double &value) - : LookupSensor::LookupSensor(adc, table, table_size, &value){} +void LookupSensor::read() { + if (adc == nullptr || value == nullptr) { + return; + } + const float raw = adc->get_raw(); + const float adc_voltage = adc->get_value_from_raw(raw, REFERENCE_VOLTAGE); -void LookupSensor::read(){ - if (adc == nullptr || value == nullptr) { - return; - } - const float raw = adc->get_raw(); - const float adc_voltage = adc->get_value_from_raw(raw, REFERENCE_VOLTAGE); - - int table_index = (int)(adc_voltage * table_size / REFERENCE_VOLTAGE); - if(table_index >= table_size){ - table_index = table_size - 1; - } - *value = table[table_index]; + int table_index = (int)(adc_voltage * table_size / REFERENCE_VOLTAGE); + if (table_index >= table_size) { + table_index = table_size - 1; + } + *value = table[table_index]; } diff --git a/Src/ST-LIB_LOW/Sensors/NTC/NTC.cpp b/Src/ST-LIB_LOW/Sensors/NTC/NTC.cpp index 466e47b7e..922a1573a 100644 --- a/Src/ST-LIB_LOW/Sensors/NTC/NTC.cpp +++ b/Src/ST-LIB_LOW/Sensors/NTC/NTC.cpp @@ -1,9 +1,7 @@ #include "Sensors/NTC/NTC.hpp" -NTC::NTC(ST_LIB::ADCDomain::Instance& adc, float *src) - : value(src), adc(&adc) {} -NTC::NTC(ST_LIB::ADCDomain::Instance& adc, float& src) - : value(&src), adc(&adc) {} +NTC::NTC(ST_LIB::ADCDomain::Instance& adc, float* src) : value(src), adc(&adc) {} +NTC::NTC(ST_LIB::ADCDomain::Instance& adc, float& src) : value(&src), adc(&adc) {} void NTC::read() { if (adc == nullptr || value == nullptr) { diff --git a/Src/ST-LIB_LOW/Sensors/Sensor/Sensor.cpp b/Src/ST-LIB_LOW/Sensors/Sensor/Sensor.cpp index 174e111cc..1bfd74568 100644 --- a/Src/ST-LIB_LOW/Sensors/Sensor/Sensor.cpp +++ b/Src/ST-LIB_LOW/Sensors/Sensor/Sensor.cpp @@ -6,13 +6,12 @@ std::vector Sensor::EXTI_id_list{}; std::vector Sensor::inputcapture_id_list{}; -void Sensor::start(){ - for(uint8_t exti_id : EXTI_id_list){ - ExternalInterrupt::turn_on(exti_id); - } - - for(uint8_t inputcapture_id : inputcapture_id_list){ - InputCapture::turn_on(inputcapture_id); - } +void Sensor::start() { + for (uint8_t exti_id : EXTI_id_list) { + ExternalInterrupt::turn_on(exti_id); + } + for (uint8_t inputcapture_id : inputcapture_id_list) { + InputCapture::turn_on(inputcapture_id); + } } diff --git a/Src/ST-LIB_LOW/Sensors/SensorInterrupt/SensorInterrupt.cpp b/Src/ST-LIB_LOW/Sensors/SensorInterrupt/SensorInterrupt.cpp index 0d49f1a2c..a70a6161e 100644 --- a/Src/ST-LIB_LOW/Sensors/SensorInterrupt/SensorInterrupt.cpp +++ b/Src/ST-LIB_LOW/Sensors/SensorInterrupt/SensorInterrupt.cpp @@ -1,18 +1,31 @@ #include "Sensors/SensorInterrupt/SensorInterrupt.hpp" #include "Sensors/Sensor/Sensor.hpp" -SensorInterrupt::SensorInterrupt(Pin &pin, std::function &&action, PinState *value, TRIGGER trigger) : value(value) { - id = ExternalInterrupt::inscribe(pin, std::forward>(action), trigger); +SensorInterrupt::SensorInterrupt( + Pin& pin, + std::function&& action, + PinState* value, + TRIGGER trigger +) + : value(value) { + id = ExternalInterrupt::inscribe(pin, std::forward>(action), trigger); - Sensor::EXTI_id_list.insert(Sensor::EXTI_id_list.begin(),id); + Sensor::EXTI_id_list.insert(Sensor::EXTI_id_list.begin(), id); } -SensorInterrupt::SensorInterrupt(Pin &pin, std::function &&action, PinState &value, TRIGGER trigger) : SensorInterrupt::SensorInterrupt(pin,std::forward>(action),&value, trigger){} +SensorInterrupt::SensorInterrupt( + Pin& pin, + std::function&& action, + PinState& value, + TRIGGER trigger +) + : SensorInterrupt::SensorInterrupt( + pin, + std::forward>(action), + &value, + trigger + ) {} -void SensorInterrupt::read(){ - *value = (PinState)ExternalInterrupt::get_pin_value(id); -} +void SensorInterrupt::read() { *value = (PinState)ExternalInterrupt::get_pin_value(id); } -uint8_t SensorInterrupt::get_id(){ - return id; -} +uint8_t SensorInterrupt::get_id() { return id; } diff --git a/Src/ST-LIB_LOW/StateMachine/StateOrder.cpp b/Src/ST-LIB_LOW/StateMachine/StateOrder.cpp index 1f8449d27..b51659fde 100644 --- a/Src/ST-LIB_LOW/StateMachine/StateOrder.cpp +++ b/Src/ST-LIB_LOW/StateMachine/StateOrder.cpp @@ -4,5 +4,13 @@ OrderProtocol* StateOrder::informer_socket = nullptr; uint16_t StateOrder::state_orders_ids_size = 0; std::span StateOrder::state_orders_ids; -StackOrder<0,uint16_t, std::span> StateOrder::add_state_orders_order(StateOrdersID::ADD_STATE_ORDERS, &StateOrder::state_orders_ids_size, &StateOrder::state_orders_ids); -StackOrder<0,uint16_t, std::span> StateOrder::remove_state_orders_order(StateOrdersID::REMOVE_STATE_ORDERS, &StateOrder::state_orders_ids_size, &StateOrder::state_orders_ids); +StackOrder<0, uint16_t, std::span> StateOrder::add_state_orders_order( + StateOrdersID::ADD_STATE_ORDERS, + &StateOrder::state_orders_ids_size, + &StateOrder::state_orders_ids +); +StackOrder<0, uint16_t, std::span> StateOrder::remove_state_orders_order( + StateOrdersID::REMOVE_STATE_ORDERS, + &StateOrder::state_orders_ids_size, + &StateOrder::state_orders_ids +); diff --git a/Tests/Time/common_tests.cpp b/Tests/Time/common_tests.cpp index 1d80a0c15..847b996ef 100644 --- a/Tests/Time/common_tests.cpp +++ b/Tests/Time/common_tests.cpp @@ -14,23 +14,21 @@ void reset() { call_count = 0; } -void set_fail_on_error(bool enabled) { - fail_on_error = enabled; -} +void set_fail_on_error(bool enabled) { fail_on_error = enabled; } } // namespace ST_LIB::TestErrorHandler -void ErrorHandlerModel::SetMetaData(int line, const char * func, const char * file){ - ErrorHandlerModel::line = to_string(line); - ErrorHandlerModel::func = string(func); - ErrorHandlerModel::file = string(file); +void ErrorHandlerModel::SetMetaData(int line, const char* func, const char* file) { + ErrorHandlerModel::line = to_string(line); + ErrorHandlerModel::func = string(func); + ErrorHandlerModel::file = string(file); } -void ErrorHandlerModel::ErrorHandlerTrigger(string format, ... ){ +void ErrorHandlerModel::ErrorHandlerTrigger(string format, ...) { (void)format; ST_LIB::TestErrorHandler::call_count++; - if(ST_LIB::TestErrorHandler::fail_on_error){ + if (ST_LIB::TestErrorHandler::fail_on_error) { EXPECT_EQ(1, 0); } } -void ErrorHandlerModel::ErrorHandlerUpdate(){} +void ErrorHandlerModel::ErrorHandlerUpdate() {} diff --git a/Tests/Time/scheduler_test.cpp b/Tests/Time/scheduler_test.cpp index 4ca80f940..600fdba0a 100644 --- a/Tests/Time/scheduler_test.cpp +++ b/Tests/Time/scheduler_test.cpp @@ -1,13 +1,11 @@ #include #include -#include +#include #include "HALAL/Services/Time/Scheduler.hpp" int count = 0; -void fake_workload(){ - count++; -} +void fake_workload() { count++; } class SchedulerTests : public ::testing::Test { protected: @@ -18,10 +16,10 @@ class SchedulerTests : public ::testing::Test { Scheduler::sorted_task_ids_ = 0; Scheduler::global_tick_us_ = 0; Scheduler::current_interval_us_ = 0; - + // Reset global callback task count count = 0; - + // Reset Timer TIM2_BASE->CNT = 0; TIM2_BASE->ARR = 0; @@ -32,23 +30,24 @@ class SchedulerTests : public ::testing::Test { }; TEST_F(SchedulerTests, FreeBitmap) { - Scheduler::register_task(10,&fake_workload); + Scheduler::register_task(10, &fake_workload); EXPECT_EQ(Scheduler::free_bitmap_, 0xFFFF'FFFE); } TEST_F(SchedulerTests, TaskRegistration) { - Scheduler::register_task(10,&fake_workload); - EXPECT_EQ(Scheduler::tasks_[0].callback,fake_workload); + Scheduler::register_task(10, &fake_workload); + EXPECT_EQ(Scheduler::tasks_[0].callback, fake_workload); } TEST_F(SchedulerTests, TaskExecutionShort) { - Scheduler::register_task(10,&fake_workload); + Scheduler::register_task(10, &fake_workload); Scheduler::start(); TIM2_BASE->PSC = 2; // quicker test - + constexpr int NUM_TICKS = 1'000; - for(int i = 0; i < NUM_TICKS; i++){ - for(int j = 0; j <= TIM2_BASE->PSC; j++) TIM2_BASE->inc_cnt_and_check(1); + for (int i = 0; i < NUM_TICKS; i++) { + for (int j = 0; j <= TIM2_BASE->PSC; j++) + TIM2_BASE->inc_cnt_and_check(1); Scheduler::update(); } // 1000 ticks / 10 ticks/task = 100 executions. @@ -56,15 +55,16 @@ TEST_F(SchedulerTests, TaskExecutionShort) { } TEST_F(SchedulerTests, TaskExecutionLong) { - Scheduler::register_task(10,&fake_workload); + Scheduler::register_task(10, &fake_workload); Scheduler::start(); // TIM2_BASE->ARR = 500; TIM2_BASE->generate_update(); TIM2_BASE->PSC = 2; // quicker test - + constexpr int NUM_TICKS = 1'000'000; - for(int i = 0; i < NUM_TICKS; i++){ - for(int j = 0; j <= TIM2_BASE->PSC; j++) TIM2_BASE->inc_cnt_and_check(1); + for (int i = 0; i < NUM_TICKS; i++) { + for (int j = 0; j <= TIM2_BASE->PSC; j++) + TIM2_BASE->inc_cnt_and_check(1); Scheduler::update(); } EXPECT_EQ(count, 100'000); @@ -76,8 +76,9 @@ TEST_F(SchedulerTests, SetTimeout) { TIM2_BASE->PSC = 2; // quicker test constexpr int NUM_TICKS = 100; - for(int i = 0; i < NUM_TICKS; i++){ - for(int j = 0; j <= TIM2_BASE->PSC; j++) TIM2_BASE->inc_cnt_and_check(1); + for (int i = 0; i < NUM_TICKS; i++) { + for (int j = 0; j <= TIM2_BASE->PSC; j++) + TIM2_BASE->inc_cnt_and_check(1); Scheduler::update(); } EXPECT_EQ(count, 1); @@ -90,8 +91,9 @@ TEST_F(SchedulerTests, GlobalTickOverflow) { TIM2_BASE->PSC = 2; // quicker test constexpr int NUM_TICKS = 100; - for(int i = 0; i < NUM_TICKS; i++){ - for(int j = 0; j <= TIM2_BASE->PSC; j++) TIM2_BASE->inc_cnt_and_check(1); + for (int i = 0; i < NUM_TICKS; i++) { + for (int j = 0; j <= TIM2_BASE->PSC; j++) + TIM2_BASE->inc_cnt_and_check(1); Scheduler::update(); } @@ -99,29 +101,27 @@ TEST_F(SchedulerTests, GlobalTickOverflow) { EXPECT_EQ(count, 5); } -#define multiple_tasks \ - X(1) \ - X(2) \ - X(3) \ - X(4) \ - X(5) \ - X(6) \ - X(7) \ - X(8) \ - X(9) \ - X(10) \ - X(11) \ - X(12) \ - X(13) \ - X(14) \ - X(15) \ +#define multiple_tasks \ + X(1) \ + X(2) \ + X(3) \ + X(4) \ + X(5) \ + X(6) \ + X(7) \ + X(8) \ + X(9) \ + X(10) \ + X(11) \ + X(12) \ + X(13) \ + X(14) \ + X(15) \ X(16) -#define X(n) \ - int multiple_task##n##count = 0; \ - void multiple_task_##n(void) { \ - multiple_task##n##count++; \ - } +#define X(n) \ + int multiple_task##n##count = 0; \ + void multiple_task_##n(void) { multiple_task##n##count++; } multiple_tasks #undef X @@ -134,8 +134,9 @@ TEST_F(SchedulerTests, GlobalTickOverflowManyTasks) { TIM2_BASE->PSC = 2; // quicker test constexpr int NUM_TICKS = 100; - for(int i = 0; i < NUM_TICKS; i++){ - for(int j = 0; j <= TIM2_BASE->PSC; j++) TIM2_BASE->inc_cnt_and_check(1); + for (int i = 0; i < NUM_TICKS; i++) { + for (int j = 0; j <= TIM2_BASE->PSC; j++) + TIM2_BASE->inc_cnt_and_check(1); Scheduler::update(); } @@ -151,14 +152,15 @@ TEST_F(SchedulerTests, TimeoutClearAddTask) { TIM2_BASE->PSC = 2; // quicker test constexpr int NUM_TICKS = 100; - for(int i = 0; i < NUM_TICKS; i++) { - for(int j = 0; j <= TIM2_BASE->PSC; j++) TIM2_BASE->inc_cnt_and_check(1); + for (int i = 0; i < NUM_TICKS; i++) { + for (int j = 0; j <= TIM2_BASE->PSC; j++) + TIM2_BASE->inc_cnt_and_check(1); Scheduler::update(); } // timeout is already done here uint8_t timeout_id_2 = Scheduler::set_timeout(20, &fake_workload); - + // after timeout, cancel task Scheduler::cancel_timeout(timeout_id); @@ -168,16 +170,16 @@ TEST_F(SchedulerTests, TimeoutClearAddTask) { int tickidx = 0; static volatile int connecting_execs{0}; static volatile int operational_execs{0}; -static volatile int fault_execs{0}; -void connecting_cyclic(){ +static volatile int fault_execs{0}; +void connecting_cyclic() { auto next_connecting_execs = connecting_execs + 1; connecting_execs = next_connecting_execs; } -void operational_cyclic(){ +void operational_cyclic() { auto next_operational_execs = operational_execs + 1; operational_execs = next_operational_execs; } -void fault_cyclic(){ +void fault_cyclic() { auto next_fault_execs = fault_execs + 1; fault_execs = next_fault_execs; } @@ -189,19 +191,20 @@ TEST_F(SchedulerTests, TaskDe_ReRegistration) { TIM2_BASE->PSC = 2; // quicker test constexpr int NUM_TICKS = 100; - for(; tickidx < NUM_TICKS; tickidx++) { - for(int j = 0; j <= TIM2_BASE->PSC; j++) TIM2_BASE->inc_cnt_and_check(1); - if(tickidx == 21){ - operational_task = Scheduler::register_task(10,operational_cyclic); + for (; tickidx < NUM_TICKS; tickidx++) { + for (int j = 0; j <= TIM2_BASE->PSC; j++) + TIM2_BASE->inc_cnt_and_check(1); + if (tickidx == 21) { + operational_task = Scheduler::register_task(10, operational_cyclic); Scheduler::unregister_task(connecting_task); } - if(tickidx == 45){ - fault_task = Scheduler::register_task(10,fault_cyclic); + if (tickidx == 45) { + fault_task = Scheduler::register_task(10, fault_cyclic); Scheduler::unregister_task(operational_task); } - if(tickidx == 70){ + if (tickidx == 70) { Scheduler::unregister_task(fault_task); - tickidx = 100; // finish test + tickidx = 100; // finish test } Scheduler::update(); } @@ -228,9 +231,10 @@ TEST_F(SchedulerTests, MultipleTasks) { Scheduler::start(); TIM2_BASE->PSC = 2; // quicker test constexpr int NUM_TICKS = 300; - for(int i = 0; i < NUM_TICKS; i++) { + for (int i = 0; i < NUM_TICKS; i++) { Scheduler::update(); - for(int j = 0; j <= TIM2_BASE->PSC; j++) TIM2_BASE->inc_cnt_and_check(1); + for (int j = 0; j <= TIM2_BASE->PSC; j++) + TIM2_BASE->inc_cnt_and_check(1); } EXPECT_EQ(multiple_task1count, NUM_TICKS / 1 - 1); @@ -254,15 +258,15 @@ TEST_F(SchedulerTests, SameTaskMultipleTimes) { Scheduler::start(); TIM2_BASE->PSC = 2; // quicker test constexpr int NUM_TICKS = 300; - for(int i = 0; i < NUM_TICKS; i++) { + for (int i = 0; i < NUM_TICKS; i++) { Scheduler::update(); - for(int j = 0; j <= TIM2_BASE->PSC; j++) TIM2_BASE->inc_cnt_and_check(1); + for (int j = 0; j <= TIM2_BASE->PSC; j++) + TIM2_BASE->inc_cnt_and_check(1); } - EXPECT_EQ(multiple_task1count, NUM_TICKS / 1 - 1 + - NUM_TICKS / 2 - 1 + - NUM_TICKS / 3 - 1 + - NUM_TICKS / 4 - 1 + - NUM_TICKS / 5 - 1 + - NUM_TICKS / 6 - 1); + EXPECT_EQ( + multiple_task1count, + NUM_TICKS / 1 - 1 + NUM_TICKS / 2 - 1 + NUM_TICKS / 3 - 1 + NUM_TICKS / 4 - 1 + + NUM_TICKS / 5 - 1 + NUM_TICKS / 6 - 1 + ); } diff --git a/Tests/Time/timer_wrapper_test.cpp b/Tests/Time/timer_wrapper_test.cpp index 0ef7059a6..fc3936800 100644 --- a/Tests/Time/timer_wrapper_test.cpp +++ b/Tests/Time/timer_wrapper_test.cpp @@ -1,11 +1,11 @@ #include -//#include -//#include +// #include +// #include #include "HALAL/Models/TimerDomain/TimerDomain.hpp" #include "HALAL/Services/Time/TimerWrapper.hpp" -TIM_TypeDef *ST_LIB::TimerDomain::cmsis_timers[16] = { +TIM_TypeDef* ST_LIB::TimerDomain::cmsis_timers[16] = { [0] = TIM2_BASE, [1] = TIM3_BASE, [2] = TIM4_BASE, @@ -33,14 +33,13 @@ class TimerWrapperTests : public ::testing::Test { TIM1_BASE->SR = 0; TIM1_BASE->CR1 = 0; TIM1_BASE->DIER = 0; - } }; constexpr ST_LIB::TimerDomain::Timer tim1_decl{{ .request = ST_LIB::TimerRequest::Advanced_1, }}; -ST_LIB::TimerDomain::Instance tim1_inst { +ST_LIB::TimerDomain::Instance tim1_inst{ .tim = TIM1_BASE, .hal_tim = 0, .timer_idx = 14, @@ -100,7 +99,7 @@ TEST_F(TimerWrapperTests, OnePulseMode_EnabledDisabled) { EXPECT_EQ(TIM1_BASE->CR1 & TIM_CR1_OPM, 0); } -void callback(void *raw) {} +void callback(void* raw) {} TEST_F(TimerWrapperTests, ConfigureTimer) { ST_LIB::TimerWrapper tim1(&tim1_inst); @@ -109,8 +108,13 @@ TEST_F(TimerWrapperTests, ConfigureTimer) { #define PERIOD 1000 tim1.set_prescaler(PRESCALER_VAL); tim1.configure16bit(callback, 0, PERIOD); - EXPECT_EQ(static_cast(TIM1_BASE->PSC), static_cast(PRESCALER_VAL)); /* set prescaler */ - EXPECT_EQ(static_cast(TIM1_BASE->ARR), static_cast(PERIOD)); /* set period */ + EXPECT_EQ( + static_cast(TIM1_BASE->PSC), + static_cast(PRESCALER_VAL) + ); /* set prescaler */ + EXPECT_EQ( + static_cast(TIM1_BASE->ARR), + static_cast(PERIOD) + ); /* set period */ EXPECT_EQ(TIM1_BASE->CR1 & TIM_CR1_CEN, TIM_CR1_CEN); /* set counter enable */ } - diff --git a/Tests/adc_test.cpp b/Tests/adc_test.cpp index 0f8e3c8ae..870a2ba55 100644 --- a/Tests/adc_test.cpp +++ b/Tests/adc_test.cpp @@ -21,8 +21,8 @@ constexpr std::array auto_entry{{ .output = &compile_time_output}, }}; -constexpr auto auto_cfg = ST_LIB::ADCDomain::build<1>( - std::span{auto_entry}); +constexpr auto auto_cfg = + ST_LIB::ADCDomain::build<1>(std::span{auto_entry}); static_assert(auto_cfg[0].peripheral == ST_LIB::ADCDomain::Peripheral::ADC_1); static_assert(auto_cfg[0].channel == ST_LIB::ADCDomain::Channel::CH16); @@ -40,8 +40,8 @@ constexpr std::array auto_pf13_entry{{ .output = &compile_time_output}, }}; -constexpr auto auto_pf13_cfg = ST_LIB::ADCDomain::build<1>( - std::span{auto_pf13_entry}); +constexpr auto auto_pf13_cfg = + ST_LIB::ADCDomain::build<1>(std::span{auto_pf13_entry}); static_assert(auto_pf13_cfg[0].peripheral == ST_LIB::ADCDomain::Peripheral::ADC_2); static_assert(auto_pf13_cfg[0].channel == ST_LIB::ADCDomain::Channel::CH2); @@ -57,10 +57,9 @@ constexpr std::array auto_pc0_12bit_entry{{ .output = &compile_time_output}, }}; -constexpr auto auto_pc0_12bit_cfg = ST_LIB::ADCDomain::build<1>( - std::span{auto_pc0_12bit_entry}); -static_assert(auto_pc0_12bit_cfg[0].peripheral == - ST_LIB::ADCDomain::Peripheral::ADC_3); +constexpr auto auto_pc0_12bit_cfg = + ST_LIB::ADCDomain::build<1>(std::span{auto_pc0_12bit_entry}); +static_assert(auto_pc0_12bit_cfg[0].peripheral == ST_LIB::ADCDomain::Peripheral::ADC_3); static_assert(auto_pc0_12bit_cfg[0].channel == ST_LIB::ADCDomain::Channel::CH10); constexpr std::array auto_pc0_16bit_entry{{ @@ -75,10 +74,9 @@ constexpr std::array auto_pc0_16bit_entry{{ .output = &compile_time_output}, }}; -constexpr auto auto_pc0_16bit_cfg = ST_LIB::ADCDomain::build<1>( - std::span{auto_pc0_16bit_entry}); -static_assert(auto_pc0_16bit_cfg[0].peripheral == - ST_LIB::ADCDomain::Peripheral::ADC_1); +constexpr auto auto_pc0_16bit_cfg = + ST_LIB::ADCDomain::build<1>(std::span{auto_pc0_16bit_entry}); +static_assert(auto_pc0_16bit_cfg[0].peripheral == ST_LIB::ADCDomain::Peripheral::ADC_1); static_assert(auto_pc0_16bit_cfg[0].channel == ST_LIB::ADCDomain::Channel::CH10); constexpr std::array internal_entry{{ @@ -93,8 +91,8 @@ constexpr std::array internal_entry{{ .output = &compile_time_output}, }}; -constexpr auto internal_cfg = ST_LIB::ADCDomain::build<1>( - std::span{internal_entry}); +constexpr auto internal_cfg = + ST_LIB::ADCDomain::build<1>(std::span{internal_entry}); #if STLIB_HAS_ADC3 static_assert(internal_cfg[0].peripheral == ST_LIB::ADCDomain::Peripheral::ADC_3); #else @@ -105,236 +103,236 @@ static_assert(internal_cfg[0].peripheral == ST_LIB::ADCDomain::Peripheral::ADC_2 class ADCTest : public ::testing::Test { protected: - void SetUp() override { ST_LIB::MockedHAL::adc_reset(); } + void SetUp() override { ST_LIB::MockedHAL::adc_reset(); } }; TEST_F(ADCTest, PollingReadUpdatesOutputValue) { - float output = -1.0f; - const std::array cfgs{{ - {.gpio_idx = 0, - .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, - .channel = ST_LIB::ADCDomain::Channel::CH16, - .resolution = ST_LIB::ADCDomain::Resolution::BITS_12, - .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, - .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, - .sample_rate_hz = 0, - .output = &output}, - }}; - - ST_LIB::ADCDomain::Init<1>::init(cfgs); - ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_16, 2048U); - - auto &adc = ST_LIB::ADCDomain::Init<1>::instances[0]; - adc.read(3.3, 1); - - const float expected = (2048.0f / 4095.0f) * 3.3f; - EXPECT_NEAR(output, expected, 0.001f); - EXPECT_EQ(ST_LIB::MockedHAL::adc_get_last_channel(ADC1), ADC_CHANNEL_16); - EXPECT_TRUE(ST_LIB::MockedHAL::adc_is_running(ADC1)); + float output = -1.0f; + const std::array cfgs{{ + {.gpio_idx = 0, + .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, + .channel = ST_LIB::ADCDomain::Channel::CH16, + .resolution = ST_LIB::ADCDomain::Resolution::BITS_12, + .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, + .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, + .sample_rate_hz = 0, + .output = &output}, + }}; + + ST_LIB::ADCDomain::Init<1>::init(cfgs); + ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_16, 2048U); + + auto& adc = ST_LIB::ADCDomain::Init<1>::instances[0]; + adc.read(3.3, 1); + + const float expected = (2048.0f / 4095.0f) * 3.3f; + EXPECT_NEAR(output, expected, 0.001f); + EXPECT_EQ(ST_LIB::MockedHAL::adc_get_last_channel(ADC1), ADC_CHANNEL_16); + EXPECT_TRUE(ST_LIB::MockedHAL::adc_is_running(ADC1)); } TEST_F(ADCTest, PollTimeoutMapsToZeroReading) { - float output = 1.0f; - const std::array cfgs{{ - {.gpio_idx = 0, - .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, - .channel = ST_LIB::ADCDomain::Channel::CH16, - .resolution = ST_LIB::ADCDomain::Resolution::BITS_12, - .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, - .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, - .sample_rate_hz = 0, - .output = &output}, - }}; - - ST_LIB::ADCDomain::Init<1>::init(cfgs); - ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_16, 4095U); - ST_LIB::MockedHAL::adc_set_poll_timeout(ADC1, true); - - auto &adc = ST_LIB::ADCDomain::Init<1>::instances[0]; - adc.read(3.3, 1); - - EXPECT_FLOAT_EQ(output, 0.0f); + float output = 1.0f; + const std::array cfgs{{ + {.gpio_idx = 0, + .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, + .channel = ST_LIB::ADCDomain::Channel::CH16, + .resolution = ST_LIB::ADCDomain::Resolution::BITS_12, + .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, + .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, + .sample_rate_hz = 0, + .output = &output}, + }}; + + ST_LIB::ADCDomain::Init<1>::init(cfgs); + ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_16, 4095U); + ST_LIB::MockedHAL::adc_set_poll_timeout(ADC1, true); + + auto& adc = ST_LIB::ADCDomain::Init<1>::instances[0]; + adc.read(3.3, 1); + + EXPECT_FLOAT_EQ(output, 0.0f); } TEST_F(ADCTest, MultiChannelInstancesReadTheirOwnChannel) { - float out0 = -1.0f; - float out1 = -1.0f; - const std::array cfgs{{ - {.gpio_idx = 0, - .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, - .channel = ST_LIB::ADCDomain::Channel::CH16, - .resolution = ST_LIB::ADCDomain::Resolution::BITS_12, - .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, - .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, - .sample_rate_hz = 0, - .output = &out0}, - {.gpio_idx = 1, - .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, - .channel = ST_LIB::ADCDomain::Channel::CH15, - .resolution = ST_LIB::ADCDomain::Resolution::BITS_12, - .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, - .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, - .sample_rate_hz = 0, - .output = &out1}, - }}; - - ST_LIB::ADCDomain::Init<2>::init(cfgs); - ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_16, 1024U); - ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_15, 3072U); - - auto &adc0 = ST_LIB::ADCDomain::Init<2>::instances[0]; - auto &adc1 = ST_LIB::ADCDomain::Init<2>::instances[1]; - adc0.read(3.3, 1); - adc1.read(3.3, 1); - - const float expected0 = (1024.0f / 4095.0f) * 3.3f; - const float expected1 = (3072.0f / 4095.0f) * 3.3f; - EXPECT_NEAR(out0, expected0, 0.001f); - EXPECT_NEAR(out1, expected1, 0.001f); - EXPECT_EQ(ST_LIB::MockedHAL::adc_get_last_channel(ADC1), ADC_CHANNEL_15); + float out0 = -1.0f; + float out1 = -1.0f; + const std::array cfgs{{ + {.gpio_idx = 0, + .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, + .channel = ST_LIB::ADCDomain::Channel::CH16, + .resolution = ST_LIB::ADCDomain::Resolution::BITS_12, + .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, + .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, + .sample_rate_hz = 0, + .output = &out0}, + {.gpio_idx = 1, + .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, + .channel = ST_LIB::ADCDomain::Channel::CH15, + .resolution = ST_LIB::ADCDomain::Resolution::BITS_12, + .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, + .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, + .sample_rate_hz = 0, + .output = &out1}, + }}; + + ST_LIB::ADCDomain::Init<2>::init(cfgs); + ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_16, 1024U); + ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_15, 3072U); + + auto& adc0 = ST_LIB::ADCDomain::Init<2>::instances[0]; + auto& adc1 = ST_LIB::ADCDomain::Init<2>::instances[1]; + adc0.read(3.3, 1); + adc1.read(3.3, 1); + + const float expected0 = (1024.0f / 4095.0f) * 3.3f; + const float expected1 = (3072.0f / 4095.0f) * 3.3f; + EXPECT_NEAR(out0, expected0, 0.001f); + EXPECT_NEAR(out1, expected1, 0.001f); + EXPECT_EQ(ST_LIB::MockedHAL::adc_get_last_channel(ADC1), ADC_CHANNEL_15); } TEST_F(ADCTest, Resolution8BitUses8BitScaling) { - float output = -1.0f; - const std::array cfgs{{ - {.gpio_idx = 0, - .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, - .channel = ST_LIB::ADCDomain::Channel::CH16, - .resolution = ST_LIB::ADCDomain::Resolution::BITS_8, - .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, - .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, - .sample_rate_hz = 0, - .output = &output}, - }}; - - ST_LIB::ADCDomain::Init<1>::init(cfgs); - ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_16, 255U); - - auto &adc = ST_LIB::ADCDomain::Init<1>::instances[0]; - adc.read(3.3, 1); - - EXPECT_NEAR(output, 3.3f, 0.001f); + float output = -1.0f; + const std::array cfgs{{ + {.gpio_idx = 0, + .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, + .channel = ST_LIB::ADCDomain::Channel::CH16, + .resolution = ST_LIB::ADCDomain::Resolution::BITS_8, + .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, + .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, + .sample_rate_hz = 0, + .output = &output}, + }}; + + ST_LIB::ADCDomain::Init<1>::init(cfgs); + ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_16, 255U); + + auto& adc = ST_LIB::ADCDomain::Init<1>::instances[0]; + adc.read(3.3, 1); + + EXPECT_NEAR(output, 3.3f, 0.001f); } TEST_F(ADCTest, Resolution10BitClampsRawTo10BitRange) { - float output = -1.0f; - const std::array cfgs{{ - {.gpio_idx = 0, - .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, - .channel = ST_LIB::ADCDomain::Channel::CH16, - .resolution = ST_LIB::ADCDomain::Resolution::BITS_10, - .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, - .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, - .sample_rate_hz = 0, - .output = &output}, - }}; - - ST_LIB::ADCDomain::Init<1>::init(cfgs); - ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_16, 4095U); - - auto &adc = ST_LIB::ADCDomain::Init<1>::instances[0]; - adc.read(3.3, 1); - - EXPECT_NEAR(output, 3.3f, 0.001f); + float output = -1.0f; + const std::array cfgs{{ + {.gpio_idx = 0, + .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, + .channel = ST_LIB::ADCDomain::Channel::CH16, + .resolution = ST_LIB::ADCDomain::Resolution::BITS_10, + .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, + .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, + .sample_rate_hz = 0, + .output = &output}, + }}; + + ST_LIB::ADCDomain::Init<1>::init(cfgs); + ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_16, 4095U); + + auto& adc = ST_LIB::ADCDomain::Init<1>::instances[0]; + adc.read(3.3, 1); + + EXPECT_NEAR(output, 3.3f, 0.001f); } TEST_F(ADCTest, TimeoutRecoversOnNextSuccessfulRead) { - float output = -1.0f; - const std::array cfgs{{ - {.gpio_idx = 0, - .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, - .channel = ST_LIB::ADCDomain::Channel::CH16, - .resolution = ST_LIB::ADCDomain::Resolution::BITS_12, - .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, - .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, - .sample_rate_hz = 0, - .output = &output}, - }}; - - ST_LIB::ADCDomain::Init<1>::init(cfgs); - auto &adc = ST_LIB::ADCDomain::Init<1>::instances[0]; - - ST_LIB::MockedHAL::adc_set_poll_timeout(ADC1, true); - adc.read(3.3, 1); - EXPECT_NE(output, -1.0f); - EXPECT_NE((HAL_ADC_GetState(&hadc1) & HAL_ADC_STATE_TIMEOUT), 0U); - - ST_LIB::MockedHAL::adc_set_poll_timeout(ADC1, false); - ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_16, 1000U); - adc.read(3.3, 1); - EXPECT_NEAR(output, (1000.0f / 4095.0f) * 3.3f, 0.001f); - EXPECT_EQ((HAL_ADC_GetState(&hadc1) & HAL_ADC_STATE_TIMEOUT), 0U); + float output = -1.0f; + const std::array cfgs{{ + {.gpio_idx = 0, + .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, + .channel = ST_LIB::ADCDomain::Channel::CH16, + .resolution = ST_LIB::ADCDomain::Resolution::BITS_12, + .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, + .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, + .sample_rate_hz = 0, + .output = &output}, + }}; + + ST_LIB::ADCDomain::Init<1>::init(cfgs); + auto& adc = ST_LIB::ADCDomain::Init<1>::instances[0]; + + ST_LIB::MockedHAL::adc_set_poll_timeout(ADC1, true); + adc.read(3.3, 1); + EXPECT_NE(output, -1.0f); + EXPECT_NE((HAL_ADC_GetState(&hadc1) & HAL_ADC_STATE_TIMEOUT), 0U); + + ST_LIB::MockedHAL::adc_set_poll_timeout(ADC1, false); + ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_16, 1000U); + adc.read(3.3, 1); + EXPECT_NEAR(output, (1000.0f / 4095.0f) * 3.3f, 0.001f); + EXPECT_EQ((HAL_ADC_GetState(&hadc1) & HAL_ADC_STATE_TIMEOUT), 0U); } TEST_F(ADCTest, SeparatePeripheralsAreIndependent) { - float out1 = -1.0f; - float out2 = -1.0f; - const std::array cfgs{{ - {.gpio_idx = 0, - .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, - .channel = ST_LIB::ADCDomain::Channel::CH16, - .resolution = ST_LIB::ADCDomain::Resolution::BITS_12, - .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, - .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, - .sample_rate_hz = 0, - .output = &out1}, - {.gpio_idx = 1, - .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_2, - .channel = ST_LIB::ADCDomain::Channel::CH2, - .resolution = ST_LIB::ADCDomain::Resolution::BITS_12, - .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, - .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, - .sample_rate_hz = 0, - .output = &out2}, - }}; - - ST_LIB::ADCDomain::Init<2>::init(cfgs); - ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_16, 500U); - ST_LIB::MockedHAL::adc_set_channel_raw(ADC2, ADC_CHANNEL_2, 3000U); - - auto &adc1 = ST_LIB::ADCDomain::Init<2>::instances[0]; - auto &adc2 = ST_LIB::ADCDomain::Init<2>::instances[1]; - adc1.read(3.3, 1); - adc2.read(3.3, 1); - - EXPECT_NEAR(out1, (500.0f / 4095.0f) * 3.3f, 0.001f); - EXPECT_NEAR(out2, (3000.0f / 4095.0f) * 3.3f, 0.001f); - EXPECT_EQ(ST_LIB::MockedHAL::adc_get_last_channel(ADC1), ADC_CHANNEL_16); - EXPECT_EQ(ST_LIB::MockedHAL::adc_get_last_channel(ADC2), ADC_CHANNEL_2); + float out1 = -1.0f; + float out2 = -1.0f; + const std::array cfgs{{ + {.gpio_idx = 0, + .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, + .channel = ST_LIB::ADCDomain::Channel::CH16, + .resolution = ST_LIB::ADCDomain::Resolution::BITS_12, + .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, + .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, + .sample_rate_hz = 0, + .output = &out1}, + {.gpio_idx = 1, + .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_2, + .channel = ST_LIB::ADCDomain::Channel::CH2, + .resolution = ST_LIB::ADCDomain::Resolution::BITS_12, + .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, + .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, + .sample_rate_hz = 0, + .output = &out2}, + }}; + + ST_LIB::ADCDomain::Init<2>::init(cfgs); + ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_16, 500U); + ST_LIB::MockedHAL::adc_set_channel_raw(ADC2, ADC_CHANNEL_2, 3000U); + + auto& adc1 = ST_LIB::ADCDomain::Init<2>::instances[0]; + auto& adc2 = ST_LIB::ADCDomain::Init<2>::instances[1]; + adc1.read(3.3, 1); + adc2.read(3.3, 1); + + EXPECT_NEAR(out1, (500.0f / 4095.0f) * 3.3f, 0.001f); + EXPECT_NEAR(out2, (3000.0f / 4095.0f) * 3.3f, 0.001f); + EXPECT_EQ(ST_LIB::MockedHAL::adc_get_last_channel(ADC1), ADC_CHANNEL_16); + EXPECT_EQ(ST_LIB::MockedHAL::adc_get_last_channel(ADC2), ADC_CHANNEL_2); } TEST_F(ADCTest, PreStartedPeripheralStillReadsValue) { - float output = -1.0f; - const std::array cfgs{{ - {.gpio_idx = 0, - .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, - .channel = ST_LIB::ADCDomain::Channel::CH16, - .resolution = ST_LIB::ADCDomain::Resolution::BITS_12, - .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, - .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, - .sample_rate_hz = 0, - .output = &output}, - }}; - - ST_LIB::ADCDomain::Init<1>::init(cfgs); - ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_16, 3000U); - - // Simulate external start so ADCDomain internal running cache is stale. - ADC_ChannelConfTypeDef sConfig{}; - sConfig.Channel = ADC_CHANNEL_16; - sConfig.Rank = ADC_REGULAR_RANK_1; - sConfig.SamplingTime = ADC_SAMPLETIME_8CYCLES_5; - sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.OffsetNumber = ADC_OFFSET_NONE; - sConfig.Offset = 0; + float output = -1.0f; + const std::array cfgs{{ + {.gpio_idx = 0, + .peripheral = ST_LIB::ADCDomain::Peripheral::ADC_1, + .channel = ST_LIB::ADCDomain::Channel::CH16, + .resolution = ST_LIB::ADCDomain::Resolution::BITS_12, + .sample_time = ST_LIB::ADCDomain::SampleTime::CYCLES_8_5, + .prescaler = ST_LIB::ADCDomain::ClockPrescaler::DIV1, + .sample_rate_hz = 0, + .output = &output}, + }}; + + ST_LIB::ADCDomain::Init<1>::init(cfgs); + ST_LIB::MockedHAL::adc_set_channel_raw(ADC1, ADC_CHANNEL_16, 3000U); + + // Simulate external start so ADCDomain internal running cache is stale. + ADC_ChannelConfTypeDef sConfig{}; + sConfig.Channel = ADC_CHANNEL_16; + sConfig.Rank = ADC_REGULAR_RANK_1; + sConfig.SamplingTime = ADC_SAMPLETIME_8CYCLES_5; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; #if defined(ADC_VER_V5_V90) - sConfig.OffsetSignedSaturation = DISABLE; + sConfig.OffsetSignedSaturation = DISABLE; #endif - ASSERT_EQ(HAL_ADC_ConfigChannel(&hadc1, &sConfig), HAL_OK); - ASSERT_EQ(HAL_ADC_Start(&hadc1), HAL_OK); + ASSERT_EQ(HAL_ADC_ConfigChannel(&hadc1, &sConfig), HAL_OK); + ASSERT_EQ(HAL_ADC_Start(&hadc1), HAL_OK); - auto &adc = ST_LIB::ADCDomain::Init<1>::instances[0]; - adc.read(3.3f, 1); + auto& adc = ST_LIB::ADCDomain::Init<1>::instances[0]; + adc.read(3.3f, 1); - EXPECT_NEAR(output, (3000.0f / 4095.0f) * 3.3f, 0.001f); + EXPECT_NEAR(output, (3000.0f / 4095.0f) * 3.3f, 0.001f); } diff --git a/Tests/dma2_test.cpp b/Tests/dma2_test.cpp index 7f1d0a346..4c2436c55 100644 --- a/Tests/dma2_test.cpp +++ b/Tests/dma2_test.cpp @@ -31,17 +31,15 @@ constexpr std::array spi_dma_entries{{ .id = 1}, }}; -constexpr auto spi_dma_cfg = ST_LIB::DMA_Domain::build<2>( - std::span{spi_dma_entries}); +constexpr auto spi_dma_cfg = + ST_LIB::DMA_Domain::build<2>(std::span{spi_dma_entries}); static_assert(std::get<1>(spi_dma_cfg[0].init_data).Request == DMA_REQUEST_SPI2_RX); static_assert(std::get<1>(spi_dma_cfg[1].init_data).Request == DMA_REQUEST_SPI2_TX); static_assert(std::get<1>(spi_dma_cfg[0].init_data).Direction == DMA_PERIPH_TO_MEMORY); static_assert(std::get<1>(spi_dma_cfg[1].init_data).Direction == DMA_MEMORY_TO_PERIPH); -static_assert(std::get<1>(spi_dma_cfg[0].init_data).FIFOThreshold == - DMA_FIFO_THRESHOLD_FULL); -static_assert(std::get<1>(spi_dma_cfg[1].init_data).FIFOThreshold == - DMA_FIFO_THRESHOLD_FULL); +static_assert(std::get<1>(spi_dma_cfg[0].init_data).FIFOThreshold == DMA_FIFO_THRESHOLD_FULL); +static_assert(std::get<1>(spi_dma_cfg[1].init_data).FIFOThreshold == DMA_FIFO_THRESHOLD_FULL); static_assert(std::get<3>(spi_dma_cfg[0].init_data) == DMA1_Stream0_IRQn); static_assert(std::get<3>(spi_dma_cfg[1].init_data) == DMA1_Stream1_IRQn); @@ -56,12 +54,15 @@ constexpr std::array auto_stream_entries{{ .id = 0}, }}; -constexpr auto auto_stream_cfg = ST_LIB::DMA_Domain::build<2>( - std::span{auto_stream_entries}); -static_assert(std::get<2>(auto_stream_cfg[0].init_data) == - ST_LIB::DMA_Domain::Stream::dma1_stream0); -static_assert(std::get<2>(auto_stream_cfg[1].init_data) == - ST_LIB::DMA_Domain::Stream::dma1_stream1); +constexpr auto auto_stream_cfg = + ST_LIB::DMA_Domain::build<2>(std::span{auto_stream_entries} + ); +static_assert( + std::get<2>(auto_stream_cfg[0].init_data) == ST_LIB::DMA_Domain::Stream::dma1_stream0 +); +static_assert( + std::get<2>(auto_stream_cfg[1].init_data) == ST_LIB::DMA_Domain::Stream::dma1_stream1 +); static_assert(std::get<3>(auto_stream_cfg[0].init_data) == DMA1_Stream0_IRQn); static_assert(std::get<3>(auto_stream_cfg[1].init_data) == DMA1_Stream1_IRQn); @@ -80,8 +81,8 @@ constexpr std::array fmac_entries{{ .id = 2}, }}; -constexpr auto fmac_cfg = ST_LIB::DMA_Domain::build<3>( - std::span{fmac_entries}); +constexpr auto fmac_cfg = + ST_LIB::DMA_Domain::build<3>(std::span{fmac_entries}); static_assert(std::get<1>(fmac_cfg[0].init_data).Direction == DMA_MEMORY_TO_MEMORY); static_assert(std::get<1>(fmac_cfg[1].init_data).Direction == DMA_MEMORY_TO_PERIPH); static_assert(std::get<1>(fmac_cfg[2].init_data).Direction == DMA_PERIPH_TO_MEMORY); @@ -100,8 +101,8 @@ constexpr std::array irq_entries{{ .id = 1}, }}; -constexpr auto irq_cfg = ST_LIB::DMA_Domain::build<2>( - std::span{irq_entries}); +constexpr auto irq_cfg = + ST_LIB::DMA_Domain::build<2>(std::span{irq_entries}); constexpr std::array i2c_entries{{ {.instance = ST_LIB::DMA_Domain::Peripheral::i2c2, @@ -114,8 +115,8 @@ constexpr std::array i2c_entries{{ .id = 1}, }}; -constexpr auto i2c_cfg = ST_LIB::DMA_Domain::build<2>( - std::span{i2c_entries}); +constexpr auto i2c_cfg = + ST_LIB::DMA_Domain::build<2>(std::span{i2c_entries}); constexpr std::array none_entries{{ {.instance = ST_LIB::DMA_Domain::Peripheral::none, @@ -124,141 +125,144 @@ constexpr std::array none_entries{{ .id = 0}, }}; -constexpr auto none_cfg = ST_LIB::DMA_Domain::build<1>( - std::span{none_entries}); +constexpr auto none_cfg = + ST_LIB::DMA_Domain::build<1>(std::span{none_entries}); void clear_nvic_enables() { - for (auto ® : NVIC->ISER) { - reg = 0U; - } + for (auto& reg : NVIC->ISER) { + reg = 0U; + } } void clear_dma_irq_table() { - for (auto &slot : dma_irq_table) { - slot = nullptr; - } + for (auto& slot : dma_irq_table) { + slot = nullptr; + } } } // namespace class DMA2Test : public ::testing::Test { protected: - void SetUp() override { - ST_LIB::MockedHAL::dma_reset(); - ST_LIB::TestErrorHandler::reset(); - clear_nvic_enables(); - clear_dma_irq_table(); - } + void SetUp() override { + ST_LIB::MockedHAL::dma_reset(); + ST_LIB::TestErrorHandler::reset(); + clear_nvic_enables(); + clear_dma_irq_table(); + } }; TEST_F(DMA2Test, InitConfiguresStreamsNVICAndLookupTable) { - ST_LIB::DMA_Domain::Init<2>::init(spi_dma_cfg); + ST_LIB::DMA_Domain::Init<2>::init(spi_dma_cfg); - EXPECT_EQ(ST_LIB::MockedHAL::dma_get_call_count(ST_LIB::MockedHAL::DMAOperation::Init), 2U); + EXPECT_EQ(ST_LIB::MockedHAL::dma_get_call_count(ST_LIB::MockedHAL::DMAOperation::Init), 2U); - auto &dma0 = ST_LIB::DMA_Domain::Init<2>::instances[0].dma; - auto &dma1 = ST_LIB::DMA_Domain::Init<2>::instances[1].dma; - EXPECT_EQ(dma0.Instance, DMA1_Stream0); - EXPECT_EQ(dma1.Instance, DMA1_Stream1); + auto& dma0 = ST_LIB::DMA_Domain::Init<2>::instances[0].dma; + auto& dma1 = ST_LIB::DMA_Domain::Init<2>::instances[1].dma; + EXPECT_EQ(dma0.Instance, DMA1_Stream0); + EXPECT_EQ(dma1.Instance, DMA1_Stream1); - EXPECT_EQ(dma0.Init.Request, DMA_REQUEST_SPI2_RX); - EXPECT_EQ(dma1.Init.Request, DMA_REQUEST_SPI2_TX); - EXPECT_EQ(dma0.Init.Direction, DMA_PERIPH_TO_MEMORY); - EXPECT_EQ(dma1.Init.Direction, DMA_MEMORY_TO_PERIPH); + EXPECT_EQ(dma0.Init.Request, DMA_REQUEST_SPI2_RX); + EXPECT_EQ(dma1.Init.Request, DMA_REQUEST_SPI2_TX); + EXPECT_EQ(dma0.Init.Direction, DMA_PERIPH_TO_MEMORY); + EXPECT_EQ(dma1.Init.Direction, DMA_MEMORY_TO_PERIPH); - EXPECT_EQ(dma_irq_table[0], &dma0); - EXPECT_EQ(dma_irq_table[1], &dma1); + EXPECT_EQ(dma_irq_table[0], &dma0); + EXPECT_EQ(dma_irq_table[1], &dma1); } TEST_F(DMA2Test, StartForwardsTransferParametersToHALDMA) { - ST_LIB::DMA_Domain::Init<2>::init(spi_dma_cfg); - auto &instance = ST_LIB::DMA_Domain::Init<2>::instances[0]; + ST_LIB::DMA_Domain::Init<2>::init(spi_dma_cfg); + auto& instance = ST_LIB::DMA_Domain::Init<2>::instances[0]; - instance.start(0x1111U, 0x2222U, 128U); + instance.start(0x1111U, 0x2222U, 128U); - EXPECT_EQ(ST_LIB::MockedHAL::dma_get_call_count(ST_LIB::MockedHAL::DMAOperation::StartIT), - 1U); - EXPECT_EQ(ST_LIB::MockedHAL::dma_get_last_start_handle(), &instance.dma); - EXPECT_EQ(ST_LIB::MockedHAL::dma_get_last_start_src(), 0x1111U); - EXPECT_EQ(ST_LIB::MockedHAL::dma_get_last_start_dst(), 0x2222U); - EXPECT_EQ(ST_LIB::MockedHAL::dma_get_last_start_length(), 128U); + EXPECT_EQ(ST_LIB::MockedHAL::dma_get_call_count(ST_LIB::MockedHAL::DMAOperation::StartIT), 1U); + EXPECT_EQ(ST_LIB::MockedHAL::dma_get_last_start_handle(), &instance.dma); + EXPECT_EQ(ST_LIB::MockedHAL::dma_get_last_start_src(), 0x1111U); + EXPECT_EQ(ST_LIB::MockedHAL::dma_get_last_start_dst(), 0x2222U); + EXPECT_EQ(ST_LIB::MockedHAL::dma_get_last_start_length(), 128U); } TEST_F(DMA2Test, InitFailureTriggersErrorAndSkipsRegistration) { - ST_LIB::TestErrorHandler::set_fail_on_error(false); - ST_LIB::MockedHAL::dma_set_init_status(HAL_ERROR); + ST_LIB::TestErrorHandler::set_fail_on_error(false); + ST_LIB::MockedHAL::dma_set_init_status(HAL_ERROR); - ST_LIB::DMA_Domain::Init<2>::init(spi_dma_cfg); + ST_LIB::DMA_Domain::Init<2>::init(spi_dma_cfg); - EXPECT_EQ(ST_LIB::MockedHAL::dma_get_call_count(ST_LIB::MockedHAL::DMAOperation::Init), 2U); - EXPECT_EQ(ST_LIB::TestErrorHandler::call_count, 2); - EXPECT_EQ(dma_irq_table[0], nullptr); - EXPECT_EQ(dma_irq_table[1], nullptr); + EXPECT_EQ(ST_LIB::MockedHAL::dma_get_call_count(ST_LIB::MockedHAL::DMAOperation::Init), 2U); + EXPECT_EQ(ST_LIB::TestErrorHandler::call_count, 2); + EXPECT_EQ(dma_irq_table[0], nullptr); + EXPECT_EQ(dma_irq_table[1], nullptr); } TEST_F(DMA2Test, InitAppliesI2CDirectionAndAlignmentAtRuntime) { - ST_LIB::DMA_Domain::Init<2>::init(i2c_cfg); - - auto &rx = ST_LIB::DMA_Domain::Init<2>::instances[0].dma; - auto &tx = ST_LIB::DMA_Domain::Init<2>::instances[1].dma; - - EXPECT_EQ(rx.Init.Request, DMA_REQUEST_I2C2_RX); - EXPECT_EQ(tx.Init.Request, DMA_REQUEST_I2C2_TX); - EXPECT_EQ(rx.Init.Direction, DMA_PERIPH_TO_MEMORY); - EXPECT_EQ(tx.Init.Direction, DMA_MEMORY_TO_PERIPH); - EXPECT_EQ(rx.Init.MemDataAlignment, DMA_MDATAALIGN_WORD); - EXPECT_EQ(tx.Init.MemDataAlignment, DMA_MDATAALIGN_WORD); - EXPECT_EQ(rx.Init.Mode, DMA_CIRCULAR); - EXPECT_EQ(tx.Init.Mode, DMA_CIRCULAR); + ST_LIB::DMA_Domain::Init<2>::init(i2c_cfg); + + auto& rx = ST_LIB::DMA_Domain::Init<2>::instances[0].dma; + auto& tx = ST_LIB::DMA_Domain::Init<2>::instances[1].dma; + + EXPECT_EQ(rx.Init.Request, DMA_REQUEST_I2C2_RX); + EXPECT_EQ(tx.Init.Request, DMA_REQUEST_I2C2_TX); + EXPECT_EQ(rx.Init.Direction, DMA_PERIPH_TO_MEMORY); + EXPECT_EQ(tx.Init.Direction, DMA_MEMORY_TO_PERIPH); + EXPECT_EQ(rx.Init.MemDataAlignment, DMA_MDATAALIGN_WORD); + EXPECT_EQ(tx.Init.MemDataAlignment, DMA_MDATAALIGN_WORD); + EXPECT_EQ(rx.Init.Mode, DMA_CIRCULAR); + EXPECT_EQ(tx.Init.Mode, DMA_CIRCULAR); } TEST_F(DMA2Test, InitAppliesNonePeripheralSettingsAtRuntime) { - ST_LIB::DMA_Domain::Init<1>::init(none_cfg); - - auto &inst = ST_LIB::DMA_Domain::Init<1>::instances[0].dma; - EXPECT_EQ(inst.Init.Request, DMA_REQUEST_MEM2MEM); - EXPECT_EQ(inst.Init.Direction, DMA_MEMORY_TO_MEMORY); - EXPECT_EQ(inst.Init.PeriphInc, DMA_PINC_ENABLE); - EXPECT_EQ(inst.Init.PeriphDataAlignment, DMA_PDATAALIGN_WORD); - EXPECT_EQ(inst.Init.Mode, DMA_NORMAL); + ST_LIB::DMA_Domain::Init<1>::init(none_cfg); + + auto& inst = ST_LIB::DMA_Domain::Init<1>::instances[0].dma; + EXPECT_EQ(inst.Init.Request, DMA_REQUEST_MEM2MEM); + EXPECT_EQ(inst.Init.Direction, DMA_MEMORY_TO_MEMORY); + EXPECT_EQ(inst.Init.PeriphInc, DMA_PINC_ENABLE); + EXPECT_EQ(inst.Init.PeriphDataAlignment, DMA_PDATAALIGN_WORD); + EXPECT_EQ(inst.Init.Mode, DMA_NORMAL); } TEST_F(DMA2Test, InitAppliesFMACSpecialSettingsAtRuntime) { - ST_LIB::DMA_Domain::Init<3>::init(fmac_cfg); + ST_LIB::DMA_Domain::Init<3>::init(fmac_cfg); - auto &m2m = ST_LIB::DMA_Domain::Init<3>::instances[0].dma; - auto &write = ST_LIB::DMA_Domain::Init<3>::instances[1].dma; - auto &read = ST_LIB::DMA_Domain::Init<3>::instances[2].dma; + auto& m2m = ST_LIB::DMA_Domain::Init<3>::instances[0].dma; + auto& write = ST_LIB::DMA_Domain::Init<3>::instances[1].dma; + auto& read = ST_LIB::DMA_Domain::Init<3>::instances[2].dma; - EXPECT_EQ(m2m.Init.Request, DMA_REQUEST_MEM2MEM); - EXPECT_EQ(write.Init.Request, DMA_REQUEST_FMAC_WRITE); - EXPECT_EQ(read.Init.Request, DMA_REQUEST_FMAC_READ); + EXPECT_EQ(m2m.Init.Request, DMA_REQUEST_MEM2MEM); + EXPECT_EQ(write.Init.Request, DMA_REQUEST_FMAC_WRITE); + EXPECT_EQ(read.Init.Request, DMA_REQUEST_FMAC_READ); - EXPECT_EQ(m2m.Init.Direction, DMA_MEMORY_TO_MEMORY); - EXPECT_EQ(write.Init.Direction, DMA_MEMORY_TO_PERIPH); - EXPECT_EQ(read.Init.Direction, DMA_PERIPH_TO_MEMORY); + EXPECT_EQ(m2m.Init.Direction, DMA_MEMORY_TO_MEMORY); + EXPECT_EQ(write.Init.Direction, DMA_MEMORY_TO_PERIPH); + EXPECT_EQ(read.Init.Direction, DMA_PERIPH_TO_MEMORY); - EXPECT_EQ(m2m.Init.FIFOMode, DMA_FIFOMODE_ENABLE); - EXPECT_EQ(write.Init.FIFOMode, DMA_FIFOMODE_ENABLE); - EXPECT_EQ(read.Init.FIFOMode, DMA_FIFOMODE_ENABLE); + EXPECT_EQ(m2m.Init.FIFOMode, DMA_FIFOMODE_ENABLE); + EXPECT_EQ(write.Init.FIFOMode, DMA_FIFOMODE_ENABLE); + EXPECT_EQ(read.Init.FIFOMode, DMA_FIFOMODE_ENABLE); - EXPECT_EQ(m2m.Init.Priority, DMA_PRIORITY_HIGH); - EXPECT_EQ(write.Init.Priority, DMA_PRIORITY_HIGH); - EXPECT_EQ(read.Init.Priority, DMA_PRIORITY_HIGH); + EXPECT_EQ(m2m.Init.Priority, DMA_PRIORITY_HIGH); + EXPECT_EQ(write.Init.Priority, DMA_PRIORITY_HIGH); + EXPECT_EQ(read.Init.Priority, DMA_PRIORITY_HIGH); } TEST_F(DMA2Test, IRQHandlersDispatchMappedDMAHandles) { - ST_LIB::DMA_Domain::Init<2>::init(irq_cfg); - auto &dma0 = ST_LIB::DMA_Domain::Init<2>::instances[0].dma; - auto &dma1 = ST_LIB::DMA_Domain::Init<2>::instances[1].dma; - - DMA1_Stream0_IRQHandler(); - EXPECT_EQ(ST_LIB::MockedHAL::dma_get_call_count(ST_LIB::MockedHAL::DMAOperation::IRQHandler), - 1U); - EXPECT_EQ(ST_LIB::MockedHAL::dma_get_last_irq_handle(), &dma0); - - DMA2_Stream7_IRQHandler(); - EXPECT_EQ(ST_LIB::MockedHAL::dma_get_call_count(ST_LIB::MockedHAL::DMAOperation::IRQHandler), - 2U); - EXPECT_EQ(ST_LIB::MockedHAL::dma_get_last_irq_handle(), &dma1); + ST_LIB::DMA_Domain::Init<2>::init(irq_cfg); + auto& dma0 = ST_LIB::DMA_Domain::Init<2>::instances[0].dma; + auto& dma1 = ST_LIB::DMA_Domain::Init<2>::instances[1].dma; + + DMA1_Stream0_IRQHandler(); + EXPECT_EQ( + ST_LIB::MockedHAL::dma_get_call_count(ST_LIB::MockedHAL::DMAOperation::IRQHandler), + 1U + ); + EXPECT_EQ(ST_LIB::MockedHAL::dma_get_last_irq_handle(), &dma0); + + DMA2_Stream7_IRQHandler(); + EXPECT_EQ( + ST_LIB::MockedHAL::dma_get_call_count(ST_LIB::MockedHAL::DMAOperation::IRQHandler), + 2U + ); + EXPECT_EQ(ST_LIB::MockedHAL::dma_get_last_irq_handle(), &dma1); } diff --git a/Tests/spi2_test.cpp b/Tests/spi2_test.cpp index c235930e9..46f13dbfa 100644 --- a/Tests/spi2_test.cpp +++ b/Tests/spi2_test.cpp @@ -18,34 +18,31 @@ extern int call_count; namespace { struct SPIDomainInstanceLayout { - SPI_HandleTypeDef hspi; - SPI_TypeDef *instance; - volatile bool *operation_flag; + SPI_HandleTypeDef hspi; + SPI_TypeDef* instance; + volatile bool* operation_flag; }; struct SPIRequestMock { - ST_LIB::SPIDomain::SPIMode mode; - ST_LIB::SPIConfigTypes::SPIConfig config; + ST_LIB::SPIDomain::SPIMode mode; + ST_LIB::SPIConfigTypes::SPIConfig config; }; -consteval SPIRequestMock make_request(ST_LIB::SPIDomain::SPIMode mode, - ST_LIB::SPIConfigTypes::DataSize data_size) { - ST_LIB::SPIConfigTypes::SPIConfig cfg{}; - cfg.data_size = data_size; - cfg.nss_mode = ST_LIB::SPIConfigTypes::NSSMode::SOFTWARE; - cfg.direction = ST_LIB::SPIConfigTypes::Direction::FULL_DUPLEX; - return {.mode = mode, .config = cfg}; +consteval SPIRequestMock +make_request(ST_LIB::SPIDomain::SPIMode mode, ST_LIB::SPIConfigTypes::DataSize data_size) { + ST_LIB::SPIConfigTypes::SPIConfig cfg{}; + cfg.data_size = data_size; + cfg.nss_mode = ST_LIB::SPIConfigTypes::NSSMode::SOFTWARE; + cfg.direction = ST_LIB::SPIConfigTypes::Direction::FULL_DUPLEX; + return {.mode = mode, .config = cfg}; } inline constexpr SPIRequestMock master8_request = - make_request(ST_LIB::SPIDomain::SPIMode::MASTER, - ST_LIB::SPIConfigTypes::DataSize::SIZE_8BIT); + make_request(ST_LIB::SPIDomain::SPIMode::MASTER, ST_LIB::SPIConfigTypes::DataSize::SIZE_8BIT); inline constexpr SPIRequestMock master16_request = - make_request(ST_LIB::SPIDomain::SPIMode::MASTER, - ST_LIB::SPIConfigTypes::DataSize::SIZE_16BIT); + make_request(ST_LIB::SPIDomain::SPIMode::MASTER, ST_LIB::SPIConfigTypes::DataSize::SIZE_16BIT); inline constexpr SPIRequestMock slave8_request = - make_request(ST_LIB::SPIDomain::SPIMode::SLAVE, - ST_LIB::SPIConfigTypes::DataSize::SIZE_8BIT); + make_request(ST_LIB::SPIDomain::SPIMode::SLAVE, ST_LIB::SPIConfigTypes::DataSize::SIZE_8BIT); constexpr std::array compile_time_entry{{ {.peripheral = ST_LIB::SPIDomain::SPIPeripheral::spi2, @@ -60,10 +57,9 @@ constexpr std::array compile_time_entry{{ .config = {}}, }}; -constexpr auto compile_time_cfg = ST_LIB::SPIDomain::build<1>( - std::span{compile_time_entry}); -static_assert(compile_time_cfg[0].peripheral == - ST_LIB::SPIDomain::SPIPeripheral::spi2); +constexpr auto compile_time_cfg = + ST_LIB::SPIDomain::build<1>(std::span{compile_time_entry}); +static_assert(compile_time_cfg[0].peripheral == ST_LIB::SPIDomain::SPIPeripheral::spi2); static_assert(compile_time_cfg[0].mode == ST_LIB::SPIDomain::SPIMode::MASTER); constexpr std::array dma_entries{{ @@ -77,282 +73,310 @@ constexpr std::array dma_entries{{ .id = 1}, }}; -constexpr auto dma_cfg = ST_LIB::DMA_Domain::build<2>( - std::span{dma_entries}); +constexpr auto dma_cfg = + ST_LIB::DMA_Domain::build<2>(std::span{dma_entries}); void clear_nvic_enables() { - for (auto ® : NVIC->ISER) { - reg = 0U; - } + for (auto& reg : NVIC->ISER) { + reg = 0U; + } } void clear_dma_irq_table() { - for (auto &slot : dma_irq_table) { - slot = nullptr; - } + for (auto& slot : dma_irq_table) { + slot = nullptr; + } } } // namespace class SPI2Test : public ::testing::Test { protected: - void SetUp() override { - SystemCoreClock = 64'000'000U; - ST_LIB::MockedHAL::spi_reset(); - ST_LIB::MockedHAL::dma_reset(); - ST_LIB::TestErrorHandler::reset(); - clear_nvic_enables(); - clear_dma_irq_table(); - for (auto &inst : ST_LIB::SPIDomain::spi_instances) { - inst = nullptr; + void SetUp() override { + SystemCoreClock = 64'000'000U; + ST_LIB::MockedHAL::spi_reset(); + ST_LIB::MockedHAL::dma_reset(); + ST_LIB::TestErrorHandler::reset(); + clear_nvic_enables(); + clear_dma_irq_table(); + for (auto& inst : ST_LIB::SPIDomain::spi_instances) { + inst = nullptr; + } + ST_LIB::DMA_Domain::Init<2>::init(dma_cfg); + } + + template + ST_LIB::SPIDomain::Instance& init_spi(uint32_t max_baudrate) { + ST_LIB::SPIConfigTypes::SPIConfig config{}; + config.nss_mode = ST_LIB::SPIConfigTypes::NSSMode::SOFTWARE; + config.data_size = DataSize; + config.direction = ST_LIB::SPIConfigTypes::Direction::FULL_DUPLEX; + + const std::array cfgs{{ + {.peripheral = ST_LIB::SPIDomain::SPIPeripheral::spi2, + .mode = Mode, + .sck_gpio_idx = 0, + .miso_gpio_idx = 1, + .mosi_gpio_idx = 2, + .nss_gpio_idx = std::nullopt, + .dma_rx_idx = 0, + .dma_tx_idx = 1, + .max_baudrate = max_baudrate, + .config = config}, + }}; + + ST_LIB::SPIDomain::Init<1>::init( + cfgs, + std::span{}, + std::span(ST_LIB::DMA_Domain::Init<2>::instances) + ); + return ST_LIB::SPIDomain::Init<1>::instances[0]; } - ST_LIB::DMA_Domain::Init<2>::init(dma_cfg); - } - - template - ST_LIB::SPIDomain::Instance &init_spi(uint32_t max_baudrate) { - ST_LIB::SPIConfigTypes::SPIConfig config{}; - config.nss_mode = ST_LIB::SPIConfigTypes::NSSMode::SOFTWARE; - config.data_size = DataSize; - config.direction = ST_LIB::SPIConfigTypes::Direction::FULL_DUPLEX; - - const std::array cfgs{{ - {.peripheral = ST_LIB::SPIDomain::SPIPeripheral::spi2, - .mode = Mode, - .sck_gpio_idx = 0, - .miso_gpio_idx = 1, - .mosi_gpio_idx = 2, - .nss_gpio_idx = std::nullopt, - .dma_rx_idx = 0, - .dma_tx_idx = 1, - .max_baudrate = max_baudrate, - .config = config}, - }}; - - ST_LIB::SPIDomain::Init<1>::init( - cfgs, std::span{}, - std::span(ST_LIB::DMA_Domain::Init<2>::instances)); - return ST_LIB::SPIDomain::Init<1>::instances[0]; - } }; TEST_F(SPI2Test, InitMasterConfiguresPeripheralDMAAndNVIC) { - auto &instance = - init_spi( - 20'000'000U); - - auto *hspi = ST_LIB::MockedHAL::spi_get_last_handle(); - ASSERT_NE(hspi, nullptr); - EXPECT_EQ(hspi->Instance, reinterpret_cast(SPI2_BASE)); - EXPECT_EQ(hspi->Init.Mode, SPI_MODE_MASTER); - EXPECT_EQ(hspi->Init.NSS, SPI_NSS_SOFT); - EXPECT_EQ(hspi->Init.DataSize, 7U); - EXPECT_EQ(hspi->Init.BaudRatePrescaler, SPI_BAUDRATEPRESCALER_4); - - EXPECT_EQ(hspi->hdmarx, &ST_LIB::DMA_Domain::Init<2>::instances[0].dma); - EXPECT_EQ(hspi->hdmatx, &ST_LIB::DMA_Domain::Init<2>::instances[1].dma); - EXPECT_EQ(hspi->hdmarx->Parent, hspi); - EXPECT_EQ(hspi->hdmatx->Parent, hspi); - - EXPECT_EQ(ST_LIB::DMA_Domain::Init<2>::instances[0].dma.Init.PeriphDataAlignment, - DMA_PDATAALIGN_BYTE); - EXPECT_EQ(ST_LIB::DMA_Domain::Init<2>::instances[0].dma.Init.MemDataAlignment, - DMA_MDATAALIGN_BYTE); - EXPECT_EQ(ST_LIB::DMA_Domain::Init<2>::instances[1].dma.Init.PeriphDataAlignment, - DMA_PDATAALIGN_BYTE); - EXPECT_EQ(ST_LIB::DMA_Domain::Init<2>::instances[1].dma.Init.MemDataAlignment, - DMA_MDATAALIGN_BYTE); - - EXPECT_EQ(NVIC_GetEnableIRQ(SPI2_IRQn), 1U); - EXPECT_EQ(ST_LIB::SPIDomain::spi_instances[1], &instance); - EXPECT_EQ(ST_LIB::MockedHAL::dma_get_call_count(ST_LIB::MockedHAL::DMAOperation::Init), 4U); + auto& instance = + init_spi( + 20'000'000U + ); + + auto* hspi = ST_LIB::MockedHAL::spi_get_last_handle(); + ASSERT_NE(hspi, nullptr); + EXPECT_EQ(hspi->Instance, reinterpret_cast(SPI2_BASE)); + EXPECT_EQ(hspi->Init.Mode, SPI_MODE_MASTER); + EXPECT_EQ(hspi->Init.NSS, SPI_NSS_SOFT); + EXPECT_EQ(hspi->Init.DataSize, 7U); + EXPECT_EQ(hspi->Init.BaudRatePrescaler, SPI_BAUDRATEPRESCALER_4); + + EXPECT_EQ(hspi->hdmarx, &ST_LIB::DMA_Domain::Init<2>::instances[0].dma); + EXPECT_EQ(hspi->hdmatx, &ST_LIB::DMA_Domain::Init<2>::instances[1].dma); + EXPECT_EQ(hspi->hdmarx->Parent, hspi); + EXPECT_EQ(hspi->hdmatx->Parent, hspi); + + EXPECT_EQ( + ST_LIB::DMA_Domain::Init<2>::instances[0].dma.Init.PeriphDataAlignment, + DMA_PDATAALIGN_BYTE + ); + EXPECT_EQ( + ST_LIB::DMA_Domain::Init<2>::instances[0].dma.Init.MemDataAlignment, + DMA_MDATAALIGN_BYTE + ); + EXPECT_EQ( + ST_LIB::DMA_Domain::Init<2>::instances[1].dma.Init.PeriphDataAlignment, + DMA_PDATAALIGN_BYTE + ); + EXPECT_EQ( + ST_LIB::DMA_Domain::Init<2>::instances[1].dma.Init.MemDataAlignment, + DMA_MDATAALIGN_BYTE + ); + + EXPECT_EQ(NVIC_GetEnableIRQ(SPI2_IRQn), 1U); + EXPECT_EQ(ST_LIB::SPIDomain::spi_instances[1], &instance); + EXPECT_EQ(ST_LIB::MockedHAL::dma_get_call_count(ST_LIB::MockedHAL::DMAOperation::Init), 4U); } TEST_F(SPI2Test, InitWith32BitDataUsesWordAlignmentAndPrescaler) { - init_spi( - 8'000'000U); - - auto *hspi = ST_LIB::MockedHAL::spi_get_last_handle(); - ASSERT_NE(hspi, nullptr); - EXPECT_EQ(hspi->Init.DataSize, 31U); - EXPECT_EQ(hspi->Init.BaudRatePrescaler, SPI_BAUDRATEPRESCALER_8); - - EXPECT_EQ(ST_LIB::DMA_Domain::Init<2>::instances[0].dma.Init.PeriphDataAlignment, - DMA_PDATAALIGN_WORD); - EXPECT_EQ(ST_LIB::DMA_Domain::Init<2>::instances[0].dma.Init.MemDataAlignment, - DMA_MDATAALIGN_WORD); - EXPECT_EQ(ST_LIB::DMA_Domain::Init<2>::instances[1].dma.Init.PeriphDataAlignment, - DMA_PDATAALIGN_WORD); - EXPECT_EQ(ST_LIB::DMA_Domain::Init<2>::instances[1].dma.Init.MemDataAlignment, - DMA_MDATAALIGN_WORD); + init_spi( + 8'000'000U + ); + + auto* hspi = ST_LIB::MockedHAL::spi_get_last_handle(); + ASSERT_NE(hspi, nullptr); + EXPECT_EQ(hspi->Init.DataSize, 31U); + EXPECT_EQ(hspi->Init.BaudRatePrescaler, SPI_BAUDRATEPRESCALER_8); + + EXPECT_EQ( + ST_LIB::DMA_Domain::Init<2>::instances[0].dma.Init.PeriphDataAlignment, + DMA_PDATAALIGN_WORD + ); + EXPECT_EQ( + ST_LIB::DMA_Domain::Init<2>::instances[0].dma.Init.MemDataAlignment, + DMA_MDATAALIGN_WORD + ); + EXPECT_EQ( + ST_LIB::DMA_Domain::Init<2>::instances[1].dma.Init.PeriphDataAlignment, + DMA_PDATAALIGN_WORD + ); + EXPECT_EQ( + ST_LIB::DMA_Domain::Init<2>::instances[1].dma.Init.MemDataAlignment, + DMA_MDATAALIGN_WORD + ); } TEST_F(SPI2Test, InitFailureOnHALSPIInitDoesNotRegisterOrEnableNVIC) { - ST_LIB::TestErrorHandler::set_fail_on_error(false); - ST_LIB::MockedHAL::spi_set_status(HAL_ERROR); + ST_LIB::TestErrorHandler::set_fail_on_error(false); + ST_LIB::MockedHAL::spi_set_status(HAL_ERROR); - init_spi( - 20'000'000U); + init_spi( + 20'000'000U + ); - EXPECT_EQ(ST_LIB::TestErrorHandler::call_count, 1); - EXPECT_EQ(ST_LIB::SPIDomain::spi_instances[1], nullptr); - EXPECT_EQ(NVIC_GetEnableIRQ(SPI2_IRQn), 0U); + EXPECT_EQ(ST_LIB::TestErrorHandler::call_count, 1); + EXPECT_EQ(ST_LIB::SPIDomain::spi_instances[1], nullptr); + EXPECT_EQ(NVIC_GetEnableIRQ(SPI2_IRQn), 0U); } TEST_F(SPI2Test, SPI2IRQHandlerDispatchesToHALIRQ) { - init_spi( - 20'000'000U); - auto *hspi = ST_LIB::MockedHAL::spi_get_last_handle(); - ASSERT_NE(hspi, nullptr); - - const auto before = ST_LIB::MockedHAL::spi_get_call_count( - ST_LIB::MockedHAL::SPIOperation::IRQHandler); - SPI2_IRQHandler(); - EXPECT_EQ(ST_LIB::MockedHAL::spi_get_call_count(ST_LIB::MockedHAL::SPIOperation::IRQHandler), - before + 1U); - EXPECT_EQ(ST_LIB::MockedHAL::spi_get_last_handle(), hspi); + init_spi( + 20'000'000U + ); + auto* hspi = ST_LIB::MockedHAL::spi_get_last_handle(); + ASSERT_NE(hspi, nullptr); + + const auto before = + ST_LIB::MockedHAL::spi_get_call_count(ST_LIB::MockedHAL::SPIOperation::IRQHandler); + SPI2_IRQHandler(); + EXPECT_EQ( + ST_LIB::MockedHAL::spi_get_call_count(ST_LIB::MockedHAL::SPIOperation::IRQHandler), + before + 1U + ); + EXPECT_EQ(ST_LIB::MockedHAL::spi_get_last_handle(), hspi); } TEST_F(SPI2Test, MasterWrapperUsesFrameWordCountsAndMinTransactionSize) { - auto &instance = - init_spi( - 20'000'000U); - ST_LIB::SPIDomain::SPIWrapper spi(instance); - - std::array tx{1, 2, 3, 4}; - EXPECT_TRUE(spi.send(std::span{tx})); - EXPECT_EQ(ST_LIB::MockedHAL::spi_get_last_size_words(), 2U); - ASSERT_EQ(ST_LIB::MockedHAL::spi_get_last_tx_size_bytes(), 4U); - - std::array pattern{0xA5, 0x5A}; - ST_LIB::MockedHAL::spi_set_rx_pattern(pattern); - std::array rx{0, 0, 0, 0}; - EXPECT_TRUE(spi.receive(std::span{rx})); - EXPECT_EQ(ST_LIB::MockedHAL::spi_get_last_size_words(), 2U); - EXPECT_EQ(rx[0], 0xA5); - EXPECT_EQ(rx[1], 0x5A); - EXPECT_EQ(rx[2], 0xA5); - EXPECT_EQ(rx[3], 0x5A); - - std::array tx_long{9, 8, 7, 6, 5, 4}; - std::array rx_short{0, 0, 0, 0}; - EXPECT_TRUE(spi.transceive(std::span{tx_long}, - std::span{rx_short})); - EXPECT_EQ(ST_LIB::MockedHAL::spi_get_last_size_words(), 2U); - ASSERT_EQ(ST_LIB::MockedHAL::spi_get_last_tx_size_bytes(), 4U); + auto& instance = + init_spi( + 20'000'000U + ); + ST_LIB::SPIDomain::SPIWrapper spi(instance); + + std::array tx{1, 2, 3, 4}; + EXPECT_TRUE(spi.send(std::span{tx})); + EXPECT_EQ(ST_LIB::MockedHAL::spi_get_last_size_words(), 2U); + ASSERT_EQ(ST_LIB::MockedHAL::spi_get_last_tx_size_bytes(), 4U); + + std::array pattern{0xA5, 0x5A}; + ST_LIB::MockedHAL::spi_set_rx_pattern(pattern); + std::array rx{0, 0, 0, 0}; + EXPECT_TRUE(spi.receive(std::span{rx})); + EXPECT_EQ(ST_LIB::MockedHAL::spi_get_last_size_words(), 2U); + EXPECT_EQ(rx[0], 0xA5); + EXPECT_EQ(rx[1], 0x5A); + EXPECT_EQ(rx[2], 0xA5); + EXPECT_EQ(rx[3], 0x5A); + + std::array tx_long{9, 8, 7, 6, 5, 4}; + std::array rx_short{0, 0, 0, 0}; + EXPECT_TRUE(spi.transceive(std::span{tx_long}, std::span{rx_short})); + EXPECT_EQ(ST_LIB::MockedHAL::spi_get_last_size_words(), 2U); + ASSERT_EQ(ST_LIB::MockedHAL::spi_get_last_tx_size_bytes(), 4U); } TEST_F(SPI2Test, DMACompletionCallbacksSetOperationFlag) { - auto &instance = - init_spi( - 20'000'000U); - ST_LIB::SPIDomain::SPIWrapper spi(instance); - - std::array tx{1, 2, 3, 4}; - std::array rx{0, 0, 0, 0}; - volatile bool done = false; - - EXPECT_TRUE(spi.send_DMA(std::span{tx}, &done)); - EXPECT_FALSE(done); - HAL_SPI_TxCpltCallback(ST_LIB::MockedHAL::spi_get_last_handle()); - EXPECT_TRUE(done); - - done = false; - EXPECT_TRUE(spi.receive_DMA(std::span{rx}, &done)); - EXPECT_FALSE(done); - HAL_SPI_RxCpltCallback(ST_LIB::MockedHAL::spi_get_last_handle()); - EXPECT_TRUE(done); - - done = false; - EXPECT_TRUE( - spi.transceive_DMA(std::span{tx}, std::span{rx}, &done)); - EXPECT_FALSE(done); - HAL_SPI_TxRxCpltCallback(ST_LIB::MockedHAL::spi_get_last_handle()); - EXPECT_TRUE(done); + auto& instance = + init_spi( + 20'000'000U + ); + ST_LIB::SPIDomain::SPIWrapper spi(instance); + + std::array tx{1, 2, 3, 4}; + std::array rx{0, 0, 0, 0}; + volatile bool done = false; + + EXPECT_TRUE(spi.send_DMA(std::span{tx}, &done)); + EXPECT_FALSE(done); + HAL_SPI_TxCpltCallback(ST_LIB::MockedHAL::spi_get_last_handle()); + EXPECT_TRUE(done); + + done = false; + EXPECT_TRUE(spi.receive_DMA(std::span{rx}, &done)); + EXPECT_FALSE(done); + HAL_SPI_RxCpltCallback(ST_LIB::MockedHAL::spi_get_last_handle()); + EXPECT_TRUE(done); + + done = false; + EXPECT_TRUE(spi.transceive_DMA(std::span{tx}, std::span{rx}, &done)); + EXPECT_FALSE(done); + HAL_SPI_TxRxCpltCallback(ST_LIB::MockedHAL::spi_get_last_handle()); + EXPECT_TRUE(done); } TEST_F(SPI2Test, CallbacksWithUnknownHandleTriggerErrorPath) { - ST_LIB::TestErrorHandler::set_fail_on_error(false); + ST_LIB::TestErrorHandler::set_fail_on_error(false); - SPI_HandleTypeDef unknown{}; - HAL_SPI_TxCpltCallback(&unknown); - EXPECT_EQ(ST_LIB::TestErrorHandler::call_count, 1); + SPI_HandleTypeDef unknown{}; + HAL_SPI_TxCpltCallback(&unknown); + EXPECT_EQ(ST_LIB::TestErrorHandler::call_count, 1); } TEST_F(SPI2Test, ErrorCallbackOnKnownHandleTriggersErrorPath) { - ST_LIB::TestErrorHandler::set_fail_on_error(false); - init_spi( - 20'000'000U); + ST_LIB::TestErrorHandler::set_fail_on_error(false); + init_spi( + 20'000'000U + ); - auto *hspi = ST_LIB::MockedHAL::spi_get_last_handle(); - ASSERT_NE(hspi, nullptr); - hspi->ErrorCode = 0x55U; - HAL_SPI_ErrorCallback(hspi); + auto* hspi = ST_LIB::MockedHAL::spi_get_last_handle(); + ASSERT_NE(hspi, nullptr); + hspi->ErrorCode = 0x55U; + HAL_SPI_ErrorCallback(hspi); - EXPECT_EQ(ST_LIB::TestErrorHandler::call_count, 1); + EXPECT_EQ(ST_LIB::TestErrorHandler::call_count, 1); } TEST_F(SPI2Test, SlaveWrapperDMAOperations) { - auto &instance = - init_spi( - 1'000'000U); - ST_LIB::SPIDomain::SPIWrapper spi(instance); - - std::array tx{4, 3, 2, 1}; - std::array rx{0, 0, 0, 0}; - volatile bool done = false; - - EXPECT_TRUE(spi.listen(std::span{rx}, &done)); - EXPECT_FALSE(done); - HAL_SPI_RxCpltCallback(ST_LIB::MockedHAL::spi_get_last_handle()); - EXPECT_TRUE(done); - - done = false; - EXPECT_TRUE(spi.arm(std::span{tx}, &done)); - EXPECT_FALSE(done); - HAL_SPI_TxCpltCallback(ST_LIB::MockedHAL::spi_get_last_handle()); - EXPECT_TRUE(done); - - done = false; - EXPECT_TRUE(spi.transceive(std::span{tx}, std::span{rx}, &done)); - EXPECT_FALSE(done); - HAL_SPI_TxRxCpltCallback(ST_LIB::MockedHAL::spi_get_last_handle()); - EXPECT_TRUE(done); + auto& instance = + init_spi( + 1'000'000U + ); + ST_LIB::SPIDomain::SPIWrapper spi(instance); + + std::array tx{4, 3, 2, 1}; + std::array rx{0, 0, 0, 0}; + volatile bool done = false; + + EXPECT_TRUE(spi.listen(std::span{rx}, &done)); + EXPECT_FALSE(done); + HAL_SPI_RxCpltCallback(ST_LIB::MockedHAL::spi_get_last_handle()); + EXPECT_TRUE(done); + + done = false; + EXPECT_TRUE(spi.arm(std::span{tx}, &done)); + EXPECT_FALSE(done); + HAL_SPI_TxCpltCallback(ST_LIB::MockedHAL::spi_get_last_handle()); + EXPECT_TRUE(done); + + done = false; + EXPECT_TRUE(spi.transceive(std::span{tx}, std::span{rx}, &done)); + EXPECT_FALSE(done); + HAL_SPI_TxRxCpltCallback(ST_LIB::MockedHAL::spi_get_last_handle()); + EXPECT_TRUE(done); } TEST_F(SPI2Test, SlaveSoftwareNSSTogglesSSIWithSafePointerMapping) { - auto &instance = - init_spi( - 1'000'000U); + auto& instance = + init_spi( + 1'000'000U + ); - auto *layout = reinterpret_cast(&instance); - layout->instance = SPI2; + auto* layout = reinterpret_cast(&instance); + layout->instance = SPI2; - ST_LIB::SPIDomain::SPIWrapper spi(instance); - layout->instance->CR1 |= SPI_CR1_SSI; + ST_LIB::SPIDomain::SPIWrapper spi(instance); + layout->instance->CR1 |= SPI_CR1_SSI; - spi.set_software_nss(true); - EXPECT_EQ(layout->instance->CR1 & SPI_CR1_SSI, 0U); + spi.set_software_nss(true); + EXPECT_EQ(layout->instance->CR1 & SPI_CR1_SSI, 0U); - spi.set_software_nss(false); - EXPECT_NE(layout->instance->CR1 & SPI_CR1_SSI, 0U); + spi.set_software_nss(false); + EXPECT_NE(layout->instance->CR1 & SPI_CR1_SSI, 0U); } TEST_F(SPI2Test, BusyStatusReturnsFalseInBlockingAndDMAPaths) { - auto &instance = - init_spi( - 20'000'000U); - ST_LIB::SPIDomain::SPIWrapper spi(instance); - ST_LIB::MockedHAL::spi_set_busy(true); - - std::array tx{1, 2, 3, 4}; - std::array rx{0, 0, 0, 0}; - - EXPECT_FALSE(spi.send(std::span{tx})); - EXPECT_FALSE(spi.receive(std::span{rx})); - EXPECT_FALSE(spi.transceive(std::span{tx}, std::span{rx})); - - EXPECT_FALSE(spi.send_DMA(std::span{tx})); - EXPECT_FALSE(spi.receive_DMA(std::span{rx})); - EXPECT_FALSE(spi.transceive_DMA(std::span{tx}, std::span{rx})); + auto& instance = + init_spi( + 20'000'000U + ); + ST_LIB::SPIDomain::SPIWrapper spi(instance); + ST_LIB::MockedHAL::spi_set_busy(true); + + std::array tx{1, 2, 3, 4}; + std::array rx{0, 0, 0, 0}; + + EXPECT_FALSE(spi.send(std::span{tx})); + EXPECT_FALSE(spi.receive(std::span{rx})); + EXPECT_FALSE(spi.transceive(std::span{tx}, std::span{rx})); + + EXPECT_FALSE(spi.send_DMA(std::span{tx})); + EXPECT_FALSE(spi.receive_DMA(std::span{rx})); + EXPECT_FALSE(spi.transceive_DMA(std::span{tx}, std::span{rx})); } diff --git a/tools/install-git-hooks.sh b/tools/install-git-hooks.sh new file mode 100755 index 000000000..f308dd713 --- /dev/null +++ b/tools/install-git-hooks.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +set -euo pipefail + +if ! command -v pre-commit >/dev/null 2>&1; then + echo "pre-commit is not installed." + echo "Install with: pip install pre-commit" + exit 1 +fi + +pre-commit install --install-hooks --hook-type pre-commit --hook-type pre-push + +echo "Hooks installed (pre-commit and pre-push)."