pcのmutexを追加

include/umiusi/outputs.hpp

@@ -5,9 +5,9 @@
 #include <mutex>
 
 #include <DigitalOut.h>
+#include <events/EventQueue.h>
 #include <Mutex.h>
 #include <PwmOut.h>
-#include <events/EventQueue.h>
 
 #include "umiusi/state.hpp"
 

include/umiusi/trylock_guard.hpp

@@ -0,0 +1,25 @@
+#ifndef UMIUSI_TRYLOCK_GUARD_HPP
+#define UMIUSI_TRYLOCK_GUARD_HPP
+
+template<typename Mutex> 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

src/main.cpp

@@ -4,11 +4,12 @@
 #include <mutex>
 
 #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<rtos::Mutex> _guard(pc_mutex);
+        std::array<uint8_t, 16>      buffer{}; // FIXME: 16 == THRUSTER_NUM * 2 * 2
+        pc.read(buffer.data(), 16);
+        std::array<std::pair<uint16_t, uint16_t>, THRUSTER_NUM> pulsewidths_us{};
+        for (size_t i = 0; i < THRUSTER_NUM; ++i) {
+            // bldc
+            uint16_t pulsewidth_us_lsb = static_cast<uint16_t>(buffer[i * 2 + 0]);
+            uint16_t pulsewidth_us_msb = static_cast<uint16_t>(buffer[i * 2 + 1]);
+            pulsewidths_us[i].first = (pulsewidth_us_lsb << 0) | (pulsewidth_us_msb << 8);
+            // servo
+            pulsewidth_us_lsb = static_cast<uint16_t>(buffer[i * 2 + 0 + THRUSTER_NUM * 2]
+            );
+            pulsewidth_us_msb = static_cast<uint16_t>(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<rtos::Mutex> _guard(pc_mutex);
+        std::array<uint8_t, 8>       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<rtos::Mutex> _guard(pc_mutex);
+              uint8_t state_val = static_cast<uint8_t>(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<uint8_t, 16> buffer{}; // FIXME: 16 == THRUSTER_NUM * 2 * 2
-            pc.read(buffer.data(), 16);
-            std::array<std::pair<uint16_t, uint16_t>, THRUSTER_NUM> pulsewidths_us{};
-            for (size_t i = 0; i < THRUSTER_NUM; ++i) {
-                // bldc
-                uint16_t pulsewidth_us_lsb = static_cast<uint16_t>(buffer[i * 2 + 0]);
-                uint16_t pulsewidth_us_msb = static_cast<uint16_t>(buffer[i * 2 + 1]);
-                pulsewidths_us[i].first = (pulsewidth_us_lsb << 0)
-                                          | (pulsewidth_us_msb << 8);
-                // servo
-                pulsewidth_us_lsb
-                    = static_cast<uint16_t>(buffer[i * 2 + 0 + THRUSTER_NUM * 2]);
-                pulsewidth_us_msb
-                    = static_cast<uint16_t>(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<uint8_t, 8> buffer = inputs.get().packet_data();
-            pc.write(buffer.data(), 8);
-        } break;
-        case 2: {
-            // read state
-            uint8_t state_val = static_cast<uint8_t>(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;
         }