Merge pull request #13 from rogy-AquaLab/timeout-suspend

Author: H1rono

Diff for: include/umiusi/defered_delay.hpp

@@ -7,6 +7,7 @@
 #include <DigitalOut.h>
 #include <Mutex.h>
 #include <PwmOut.h>
+#include <events/EventQueue.h>
 
 #include "umiusi/state.hpp"
 
@@ -81,11 +82,7 @@ class Outputs {
     Outputs();
     void activate();
     void deactivate();
-    /// BLDC(に繋がっているESC)を起動する。完了までに2秒を要する。
-    /// 完了時点でbldcのパルス幅は各100usとなる
-    void wake_up();
-    /// activate->wake_up
-    void setup();
+    void prepare_wake_up();
     void set_powers(
         const std::array<std::pair<uint16_t, uint16_t>, THRUSTER_NUM>& pulsewidths_us
     );
@@ -107,7 +104,7 @@ class OutputMachine {
         const std::array<std::pair<uint16_t, uint16_t>, THRUSTER_NUM>& pulsewidths_us
     );
     void suspend();
-    void initialize();
+    void initialize_with_equeue(events::EventQueue& equeue);
 };
 
 #endif

Diff for: include/umiusi/outputs.hpp

@@ -1,69 +1,40 @@
 #include <array>
+#include <atomic>
 #include <cstdint>
 #include <mutex>
 
 #include "BufferedSerial.h"
-#include "EventFlags.h"
-#include "ThisThread.h"
-#include "Thread.h"
+#include "events/EventQueue.h"
+#include "events/Event.h"
 
-#include "umiusi/defered_delay.hpp"
 #include "umiusi/inputs.hpp"
 #include "umiusi/outputs.hpp"
 
 using namespace std::chrono_literals;
 
 int main() {
-    constexpr size_t INPUTS_THREAD_STACK_SIZE = 1024;
-    constexpr size_t SETUP_THREAD_STACK_SIZE  = 512;
+    constexpr std::size_t EQUEUE_BUFFER_SIZE = 32 * EVENTS_EVENT_SIZE;
+
+    unsigned char equeue_buffer[EQUEUE_BUFFER_SIZE] = {};
 
     CachedInputs         inputs{};
     OutputMachine        output{};
     mbed::BufferedSerial pc(USBTX, USBRX);
+    std::atomic_bool     received_order;
+    pc.set_blocking(false);
+    inputs.read();
 
-    rtos::EventFlags trigger_setup{};
-    unsigned char    setup_thread_stack[SETUP_THREAD_STACK_SIZE]   = {};
-    unsigned char    inputs_thread_stack[INPUTS_THREAD_STACK_SIZE] = {};
-
-    rtos::Thread setup_thread(
-        osPriorityBelowNormal, SETUP_THREAD_STACK_SIZE, setup_thread_stack
-    );
-    rtos::Thread inputs_thread(
-        osPriorityBelowNormal, INPUTS_THREAD_STACK_SIZE, inputs_thread_stack
-    );
-
-    // TODO: handle osStatus
-    setup_thread.start([&output, &trigger_setup]() {
-        while (true) {
-            trigger_setup.wait_any(1, osWaitForever, false);
-            output.initialize();
-            trigger_setup.clear();
-        }
-    });
-    osStatus inputs_thread_status = inputs_thread.start([&inputs]() {
-        while (true) {
-            inputs.read();
-            rtos::ThisThread::sleep_for(10ms);
+    events::EventQueue equeue(EQUEUE_BUFFER_SIZE, equeue_buffer);
+    auto initialize_event = equeue.event(mbed::callback([&output, &equeue]() {
+        if (output.state() != State::INITIALIZING) {
+            output.initialize_with_equeue(equeue);
         }
-    });
-    if (inputs_thread_status != osOK) {
-        // 本来ここに入ることはあってはならないが、一応書いておく
-        // スレッドを開始することができなかったので、入力が読み取られなくなる
-        // そのため、一度だけ値を読んでおくことにする
-        // 得られる値を見て実行状況を判断すること
-        inputs.read();
-    }
-    while (true) {
-        DeferedDelay _delay(10ms);
-        pc.sync();
-        uint8_t header = 0;
-        // TODO: timeout
-        ssize_t read = pc.read(&header, 1);
-        if (read < 1) {
-            continue;
-        }
-        // なぜかこれがないと動かない
-        rtos::ThisThread::sleep_for(20ms);
+    }));
+    auto suspend_event = equeue.event(mbed::callback([&output]() {
+        output.suspend();
+    }));
+
+    const auto process_order = [&pc, &inputs, &output, &initialize_event, &suspend_event](std::uint8_t header) {
         switch (header) {
         case 0: {
             // write
@@ -74,7 +45,7 @@ int main() {
                 // 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)
+                pulsewidths_us[i].first = (pulsewidth_us_lsb << 0)
                                           | (pulsewidth_us_msb << 8);
                 // servo
                 pulsewidth_us_lsb
@@ -99,18 +70,41 @@ int main() {
         case 0xFE: {
             // (re)start
             if (output.state() == State::INITIALIZING) {
-                continue;
+                return;
             }
-            trigger_setup.set(1);
+            initialize_event.call();
         } break;
         case 0xFF:
             // suspend
-            output.suspend();
+            suspend_event.call();
             break;
         default:
             // unexpected
-            continue;
+            return;
         }
-    }
+    };
+    equeue.call_every(10ms, [&equeue, &pc, &received_order, &process_order]() {
+        std::uint8_t header = 0;
+        ssize_t res = pc.read(&header, 1);
+        if (res < 1) {
+            return;
+        }
+        received_order.store(true, std::memory_order_release);
+        equeue.call(process_order, header);
+    });
+
+    equeue.call_every(10ms, [&inputs]() {
+        inputs.read();
+    });
+
+    equeue.call_every(1s, [&received_order, &suspend_event]() {
+        const bool received = received_order.load(std::memory_order_acquire);
+        received_order.store(false, std::memory_order_release);
+        if (!received) {
+            suspend_event.call();
+        }
+    });
+
+    equeue.dispatch_forever();
     return 0;
 }

Diff for: src/main.cpp

@@ -1,5 +1,4 @@
 #include "umiusi/outputs.hpp"
-#include "umiusi/defered_delay.hpp"
 
 using namespace std::chrono_literals;
 
@@ -29,19 +28,12 @@ void Outputs::deactivate() {
     this->init_status.write(0);
 }
 
-/// ESC の起動待ち
-void Outputs::wake_up() {
-    DeferedDelay _(2s);
+void Outputs::prepare_wake_up(){
     for (mbed::PwmOut& bldc : this->bldcs) {
         bldc.pulsewidth_us(100);
     }
 }
 
-void Outputs::setup() {
-    this->activate();
-    this->wake_up();
-}
-
 void Outputs::set_powers(
     const std::array<std::pair<uint16_t, uint16_t>, THRUSTER_NUM>& pulsewidths_us
 ) {
@@ -85,15 +77,18 @@ void OutputMachine::suspend() {
     this->outputs.deactivate();
 }
 
-void OutputMachine::initialize() {
+void OutputMachine::initialize_with_equeue(events::EventQueue& equeue){
     if (this->state() == State::INITIALIZING) {
         return;
     }
     this->set_state(State::INITIALIZING);
     this->outputs.reset();
-    this->outputs.setup();
-    if (this->state() == State::INITIALIZING) {
-        // setup前後で値が変化する可能性がある
-        this->set_state(State::RUNNING);
-    }
+    this->outputs.activate();
+    this->outputs.prepare_wake_up();
+    equeue.call_in(2s, [this]() {
+        if (this->state() == State::INITIALIZING) {
+            this->set_state(State::RUNNING);
+        }
+    });
 }
+