diff --git a/include/sta/debug/spatz.hpp b/include/sta/debug/spatz.hpp index eb3ec98..2411b69 100644 --- a/include/sta/debug/spatz.hpp +++ b/include/sta/debug/spatz.hpp @@ -3,6 +3,7 @@ #include #include +#include #include #ifdef STA_SPATZ_ENABLED @@ -21,7 +22,7 @@ namespace sta * * @param mutex A mutex used to make SPATZ thread-safe. */ - void init(Mutex * mutex, Signal * signal); + void init(Mutex * mutex, Event * signal); /** * @brief Request bytes for a specific sensor id via the printable. @@ -49,6 +50,8 @@ namespace sta * @param length The number of doubles to request. */ void request(uint8_t id, double * buffer, size_t length); + + void notify(uint8_t id); } // namespace spatz } // namespace sta diff --git a/include/sta/event.hpp b/include/sta/event.hpp index 56b05a9..7ef5c44 100644 --- a/include/sta/event.hpp +++ b/include/sta/event.hpp @@ -40,7 +40,7 @@ namespace sta * @param flags flag nr. to wait for. * @param timeout timeout in ms., default to forever. */ - virtual uint32_t wait(uint32_t flags, uint32_t timeout = osWaitForever) = 0; + virtual uint32_t wait(uint32_t flags, uint32_t timeout = 0xFFFFFFFF) = 0; }; } // namespace sta diff --git a/src/debug/spatz.cpp b/src/debug/spatz.cpp index 6bf5880..ccff7e7 100644 --- a/src/debug/spatz.cpp +++ b/src/debug/spatz.cpp @@ -12,18 +12,16 @@ namespace sta { namespace spatz { - sta::Mutex * mutex_ = nullptr; - sta::Signal * signal_ = nullptr; + Mutex * mutex_ = nullptr; + Event * signal_ = nullptr; - void init(sta::Mutex * mutex, Signal * signal) + void init(sta::Mutex * mutex, Event * signal) { mutex_ = mutex; signal_ = signal; uint8_t msg = 0xFF; - sta::lock_guard lock(*mutex_); - // Wait until SPATZ sends the init byte. while (msg != STA_INIT_SPATZ_ID) { @@ -41,13 +39,11 @@ namespace sta void request(uint8_t id, float * buffer, size_t length) { - mutex_->acquire(); + sta::lock_guard lock(*mutex_); HAL_UART_Receive_IT(&huart1, reinterpret_cast(buffer), length * sizeof(float)); Debug->printf("%d", id); - signal_->wait(); - - mutex_->release(); + signal_->wait(0x01); } void request(uint8_t id, double * buffer, size_t length) @@ -57,6 +53,11 @@ namespace sta Debug->printf("%d", id); Debug->read(buffer, length); } + + void notify(uint8_t id) + { + Debug->printf("%d", id); + } } // namespace spatz } // namespace sta @@ -64,7 +65,11 @@ namespace sta void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { if (sta::spatz::signal_ != nullptr) - sta::spatz::signal_->notify(); + { + sta::spatz::signal_->set(0x01); + sta::spatz::signal_->clear(0x01); + } + }