From bdac62c8087495e28bb0ec23cdd2e39c36439360 Mon Sep 17 00:00:00 2001 From: H1rono Date: Sat, 24 Aug 2024 21:57:23 +0900 Subject: [PATCH 1/6] Split events --- src/main.cpp | 85 +++++++++++++++++++++++++--------------------------- 1 file changed, 40 insertions(+), 45 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 8f0b442..8904714 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -4,8 +4,8 @@ #include #include "BufferedSerial.h" -#include "events/EventQueue.h" #include "events/Event.h" +#include "events/EventQueue.h" #include "umiusi/inputs.hpp" #include "umiusi/outputs.hpp" @@ -34,50 +34,45 @@ int main() { output.suspend(); })); - const auto process_order = [&pc, &inputs, &output, &initialize_event, &suspend_event](std::uint8_t header) { + auto write_order_event = equeue.event(mbed::callback([&pc, &output]() { + std::array buffer{}; // FIXME: 16 == THRUSTER_NUM * 2 * 2 + pc.read(buffer.data(), 16); + std::array, THRUSTER_NUM> pulsewidths_us{}; + for (size_t i = 0; i < THRUSTER_NUM; ++i) { + // bldc + uint16_t pulsewidth_us_lsb = static_cast(buffer[i * 2 + 0]); + uint16_t pulsewidth_us_msb = static_cast(buffer[i * 2 + 1]); + pulsewidths_us[i].first = (pulsewidth_us_lsb << 0) | (pulsewidth_us_msb << 8); + // servo + pulsewidth_us_lsb = static_cast(buffer[i * 2 + 0 + THRUSTER_NUM * 2] + ); + pulsewidth_us_msb = static_cast(buffer[i * 2 + 1 + THRUSTER_NUM * 2] + ); + pulsewidths_us[i].second = (pulsewidth_us_lsb << 0) + | (pulsewidth_us_msb << 8); + } + output.set_powers(pulsewidths_us); + })); + auto read_order_event = equeue.event(mbed::callback([&pc, &inputs]() { + std::array buffer = inputs.get().packet_data(); + pc.write(buffer.data(), 8); + })); + auto read_state_order_event = equeue.event(mbed::callback([&pc, &output]() { + uint8_t state_val = static_cast(output.state()); + pc.write(&state_val, 1); + })); + + const auto process_order = [&write_order_event, + &read_order_event, + &read_state_order_event, + &initialize_event, + &suspend_event](std::uint8_t header) { switch (header) { - case 0: { - // write - std::array buffer{}; // FIXME: 16 == THRUSTER_NUM * 2 * 2 - pc.read(buffer.data(), 16); - std::array, THRUSTER_NUM> pulsewidths_us{}; - for (size_t i = 0; i < THRUSTER_NUM; ++i) { - // bldc - uint16_t pulsewidth_us_lsb = static_cast(buffer[i * 2 + 0]); - uint16_t pulsewidth_us_msb = static_cast(buffer[i * 2 + 1]); - pulsewidths_us[i].first = (pulsewidth_us_lsb << 0) - | (pulsewidth_us_msb << 8); - // servo - pulsewidth_us_lsb - = static_cast(buffer[i * 2 + 0 + THRUSTER_NUM * 2]); - pulsewidth_us_msb - = static_cast(buffer[i * 2 + 1 + THRUSTER_NUM * 2]); - pulsewidths_us[i].second = (pulsewidth_us_lsb << 0) - | (pulsewidth_us_msb << 8); - } - output.set_powers(pulsewidths_us); - } break; - case 1: { - // read - std::array buffer = inputs.get().packet_data(); - pc.write(buffer.data(), 8); - } break; - case 2: { - // read state - uint8_t state_val = static_cast(output.state()); - pc.write(&state_val, 1); - } break; - case 0xFE: { - // (re)start - if (output.state() == State::INITIALIZING) { - return; - } - initialize_event.call(); - } break; - case 0xFF: - // suspend - suspend_event.call(); - break; + case 0: write_order_event.call(); break; + case 1: read_order_event.call(); break; + case 2: read_state_order_event.call(); break; + case 0xFE: initialize_event.call(); break; + case 0xFF: suspend_event.call(); break; default: // unexpected return; @@ -85,7 +80,7 @@ int main() { }; equeue.call_every(30ms, [&equeue, &pc, &received_order, &process_order]() { std::uint8_t header = 0; - ssize_t res = pc.read(&header, 1); + ssize_t res = pc.read(&header, 1); if (res < 1) { return; } From b0792785840917b13030cb296624a9e19f522b6a Mon Sep 17 00:00:00 2001 From: H1rono Date: Sat, 24 Aug 2024 23:53:57 +0900 Subject: [PATCH 2/6] Add `TrylockGuard` --- src/main.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/main.cpp b/src/main.cpp index 8904714..f4ee5fe 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -12,6 +12,27 @@ using namespace std::chrono_literals; +class TrylockGuard { +private: + rtos::Mutex& _mutex; + bool _locked; + +public: + TrylockGuard(rtos::Mutex& mutex) : _mutex(mutex) { + this->_locked = this->_mutex.trylock(); + } + + ~TrylockGuard() { + if (this->_locked) { + this->_mutex.unlock(); + } + } + + auto locked() const -> bool { + return this->_locked; + } +}; + int main() { constexpr std::size_t EQUEUE_BUFFER_SIZE = 32 * EVENTS_EVENT_SIZE; From f0eb9f6650374c1f82cdfc17be4bd869122b8a14 Mon Sep 17 00:00:00 2001 From: H1rono Date: Sun, 25 Aug 2024 00:50:53 +0900 Subject: [PATCH 3/6] Add `pc_mutex` --- src/main.cpp | 27 ++++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index f4ee5fe..711f227 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -41,6 +41,7 @@ int main() { CachedInputs inputs{}; OutputMachine output{}; mbed::BufferedSerial pc(USBTX, USBRX); + rtos::Mutex pc_mutex{}; std::atomic_bool received_order; pc.set_blocking(false); inputs.read(); @@ -55,8 +56,9 @@ int main() { output.suspend(); })); - auto write_order_event = equeue.event(mbed::callback([&pc, &output]() { - std::array buffer{}; // FIXME: 16 == THRUSTER_NUM * 2 * 2 + auto write_order_event = equeue.event(mbed::callback([&pc, &pc_mutex, &output]() { + std::lock_guard _guard(pc_mutex); + std::array buffer{}; // FIXME: 16 == THRUSTER_NUM * 2 * 2 pc.read(buffer.data(), 16); std::array, THRUSTER_NUM> pulsewidths_us{}; for (size_t i = 0; i < THRUSTER_NUM; ++i) { @@ -74,14 +76,17 @@ int main() { } output.set_powers(pulsewidths_us); })); - auto read_order_event = equeue.event(mbed::callback([&pc, &inputs]() { - std::array buffer = inputs.get().packet_data(); + auto read_order_event = equeue.event(mbed::callback([&pc, &pc_mutex, &inputs]() { + std::lock_guard _guard(pc_mutex); + std::array buffer = inputs.get().packet_data(); pc.write(buffer.data(), 8); })); - auto read_state_order_event = equeue.event(mbed::callback([&pc, &output]() { - uint8_t state_val = static_cast(output.state()); - pc.write(&state_val, 1); - })); + auto read_state_order_event + = equeue.event(mbed::callback([&pc, &pc_mutex, &output]() { + std::lock_guard _guard(pc_mutex); + uint8_t state_val = static_cast(output.state()); + pc.write(&state_val, 1); + })); const auto process_order = [&write_order_event, &read_order_event, @@ -99,7 +104,11 @@ int main() { return; } }; - equeue.call_every(30ms, [&equeue, &pc, &received_order, &process_order]() { + equeue.call_every(30ms, [&equeue, &pc, &pc_mutex, &received_order, &process_order]() { + TrylockGuard pc_guard(pc_mutex); + if (!pc_guard.locked()) { + return; + } std::uint8_t header = 0; ssize_t res = pc.read(&header, 1); if (res < 1) { From 6b67f2a3fddb055c12d7803c80e4bb5eecaf2568 Mon Sep 17 00:00:00 2001 From: H1rono Date: Sun, 25 Aug 2024 03:19:12 +0900 Subject: [PATCH 4/6] Move `TrylockGuard` --- include/umiusi/trylock_guard.hpp | 26 ++++++++++++++++++++++++++ src/main.cpp | 22 +--------------------- 2 files changed, 27 insertions(+), 21 deletions(-) create mode 100644 include/umiusi/trylock_guard.hpp diff --git a/include/umiusi/trylock_guard.hpp b/include/umiusi/trylock_guard.hpp new file mode 100644 index 0000000..1f1b6e6 --- /dev/null +++ b/include/umiusi/trylock_guard.hpp @@ -0,0 +1,26 @@ +#ifndef UMIUSI_TRYLOCK_GUARD_HPP +#define UMIUSI_TRYLOCK_GUARD_HPP + +template +class TrylockGuard { +private: + Mutex& _mutex; + bool _locked; + +public: + TrylockGuard(Mutex& mutex) : _mutex(mutex) { + this->_locked = this->_mutex.trylock(); + } + + ~TrylockGuard() { + if (this->_locked) { + this->_mutex.unlock(); + } + } + + auto locked() const -> bool { + return this->_locked; + } +}; + +#endif // UMIUSI_TRYLOCK_GUARD_HPP diff --git a/src/main.cpp b/src/main.cpp index 711f227..3f82e95 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -9,30 +9,10 @@ #include "umiusi/inputs.hpp" #include "umiusi/outputs.hpp" +#include "umiusi/trylock_guard.hpp" using namespace std::chrono_literals; -class TrylockGuard { -private: - rtos::Mutex& _mutex; - bool _locked; - -public: - TrylockGuard(rtos::Mutex& mutex) : _mutex(mutex) { - this->_locked = this->_mutex.trylock(); - } - - ~TrylockGuard() { - if (this->_locked) { - this->_mutex.unlock(); - } - } - - auto locked() const -> bool { - return this->_locked; - } -}; - int main() { constexpr std::size_t EQUEUE_BUFFER_SIZE = 32 * EVENTS_EVENT_SIZE; From 67d2cd64618b35837f9bec628613c73484ddc144 Mon Sep 17 00:00:00 2001 From: H1rono Date: Sun, 25 Aug 2024 03:53:38 +0900 Subject: [PATCH 5/6] Fmt --- include/umiusi/outputs.hpp | 2 +- include/umiusi/trylock_guard.hpp | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/include/umiusi/outputs.hpp b/include/umiusi/outputs.hpp index 292a78a..22ee669 100644 --- a/include/umiusi/outputs.hpp +++ b/include/umiusi/outputs.hpp @@ -5,9 +5,9 @@ #include #include +#include #include #include -#include #include "umiusi/state.hpp" diff --git a/include/umiusi/trylock_guard.hpp b/include/umiusi/trylock_guard.hpp index 1f1b6e6..bb59b3e 100644 --- a/include/umiusi/trylock_guard.hpp +++ b/include/umiusi/trylock_guard.hpp @@ -1,11 +1,10 @@ #ifndef UMIUSI_TRYLOCK_GUARD_HPP #define UMIUSI_TRYLOCK_GUARD_HPP -template -class TrylockGuard { +template class TrylockGuard { private: Mutex& _mutex; - bool _locked; + bool _locked; public: TrylockGuard(Mutex& mutex) : _mutex(mutex) { From 28a992c41e52f05e1fcc6d565411da3c8f41a9c2 Mon Sep 17 00:00:00 2001 From: H1rono Date: Sun, 25 Aug 2024 04:09:27 +0900 Subject: [PATCH 6/6] Add comment --- src/main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main.cpp b/src/main.cpp index 3f82e95..ec734a8 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -84,6 +84,7 @@ int main() { return; } }; + // FIXME: なぜか30msより短いと挙動がおかしくなる equeue.call_every(30ms, [&equeue, &pc, &pc_mutex, &received_order, &process_order]() { TrylockGuard pc_guard(pc_mutex); if (!pc_guard.locked()) {