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 new file mode 100644 index 0000000..bb59b3e --- /dev/null +++ b/include/umiusi/trylock_guard.hpp @@ -0,0 +1,25 @@ +#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 8f0b442..ec734a8 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -4,11 +4,12 @@ #include #include "BufferedSerial.h" -#include "events/EventQueue.h" #include "events/Event.h" +#include "events/EventQueue.h" #include "umiusi/inputs.hpp" #include "umiusi/outputs.hpp" +#include "umiusi/trylock_guard.hpp" using namespace std::chrono_literals; @@ -20,6 +21,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(); @@ -34,58 +36,62 @@ 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, &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) { + // 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, &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, &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, + &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; } }; - equeue.call_every(30ms, [&equeue, &pc, &received_order, &process_order]() { + // FIXME: なぜか30msより短いと挙動がおかしくなる + 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); + ssize_t res = pc.read(&header, 1); if (res < 1) { return; }