Cleaner SBB scheduling.

Author: Mysticial

SerialPrograms/Source/NintendoSwitch/Controllers/SysbotBase/SysbotBase_ProController.cpp

@@ -29,7 +29,7 @@ ProController_SysbotBase::ProController_SysbotBase(
     , m_stopping(false)
     , m_replace_on_next(false)
     , m_command_queue(QUEUE_SIZE)
-    , m_is_active(false)
+    , m_next_state_change(WallClock::max())
 {
     if (!connection.is_ready()){
         return;
@@ -74,11 +74,9 @@ void ProController_SysbotBase::cancel_all_commands(){
 //    cout << "ProController_SysbotBase::cancel_all_commands()" << endl;
     std::lock_guard<std::mutex> lg(m_state_lock);
     size_t queue_size = m_command_queue.size();
-    if (queue_size > 0){
-        m_command_queue.clear();
-        m_is_active = false;
-        m_cv.notify_all();
-    }
+    m_next_state_change = WallClock::min();
+    m_command_queue.clear();
+    m_cv.notify_all();
     this->clear_on_next();
     m_logger.log("cancel_all_commands(): Command Queue Size = " + std::to_string(queue_size), COLOR_DARKGREEN);
 }
@@ -99,7 +97,7 @@ void ProController_SysbotBase::wait_for_all(const Cancellable* cancellable){
     m_logger.log("wait_for_all(): Command Queue Size = " + std::to_string(m_command_queue.size()), COLOR_DARKGREEN);
     this->issue_wait_for_all(cancellable);
     m_cv.wait(lg1, [this]{
-        return m_command_queue.empty() || m_replace_on_next;
+        return m_next_state_change == WallClock::max() || m_replace_on_next;
     });
     if (cancellable){
         cancellable->throw_if_cancelled();
@@ -151,12 +149,16 @@ void ProController_SysbotBase::push_state(const Cancellable* cancellable, WallDu
     }
 
     if (m_replace_on_next){
-        m_command_queue.clear();
-        m_is_active = false;
+//        cout << "executing replace" << endl;
         m_replace_on_next = false;
+        m_command_queue.clear();
+        m_next_state_change = WallClock::min();
+        m_cv.notify_all();
     }
 
-    if (m_command_queue.empty()){
+    //  Enqueuing into empty+idle queue.
+    if (m_next_state_change == WallClock::max()){
+        m_next_state_change = WallClock::min();
         m_cv.notify_all();
     }
 
@@ -298,52 +300,34 @@ void ProController_SysbotBase::thread_body(){
 
     std::unique_lock<std::mutex> lg(m_state_lock);
     while (!m_stopping.load(std::memory_order_relaxed)){
-        if (m_command_queue.empty()){
-            m_is_active = false;
-            if (current_state.is_neutral()){
-                m_cv.wait(lg);
-                continue;
-            }
-
-            send_diff(current_state, ProControllerState());
-            current_state.clear();
-            continue;
-        }
-
         WallClock now = current_time();
 
-        //  Check the next item in the schedule.
-        Command& command = m_command_queue.front();
-
-        //  Waking up from idle.
-        if (!m_is_active){
-            m_is_active = true;
-            m_queue_start_time = now;
-
-            send_diff(current_state, command.state);
-            current_state = command.state;
-
-            WallClock expiration = m_queue_start_time + command.duration;
-            m_cv.wait_until(lg, expiration - EARLY_WAKE);
-            pause();
+        //  State change.
+        if (now >= m_next_state_change){
+            if (m_command_queue.empty()){
+                send_diff(current_state, ProControllerState());
+                current_state.clear();
+                m_next_state_change = WallClock::max();
+            }else{
+                Command& command = m_command_queue.front();
+                send_diff(current_state, command.state);
+                current_state = command.state;
+                if (m_next_state_change == WallClock::min()){
+                    m_next_state_change = now;
+                }
+                m_next_state_change += command.duration;
+                m_command_queue.pop_front();
+            }
+            m_cv.notify_all();
             continue;
         }
 
-        //  Already running.
-        WallClock expiration = m_queue_start_time + command.duration;
-
-        //  Current command hasn't expired yet.
-        if (now < expiration){
-            m_cv.wait_until(lg, expiration - EARLY_WAKE);
+        if (now + EARLY_WAKE >= m_next_state_change){
             pause();
             continue;
         }
 
-        //  Command has expired. We can pop it.
-        m_is_active = false;
-        m_queue_start_time = expiration;
-        m_command_queue.pop_front();
-        m_cv.notify_all();
+        m_cv.wait_until(lg, m_next_state_change - EARLY_WAKE);
 
         //  Check how much we shot passed the expiration time.
 //        WallDuration delay = now - expiration;

SerialPrograms/Source/NintendoSwitch/Controllers/SysbotBase/SysbotBase_ProController.h

@@ -236,8 +236,10 @@ class ProController_SysbotBase final :
         Milliseconds duration;
     };
     CircularBuffer<Command> m_command_queue;
-    WallClock m_queue_start_time;
-    bool m_is_active;
+
+    //  WallClock::max() means the queue is empty.
+    //  WallClock::min() means the state has suddently changed.
+    WallClock m_next_state_change;
 
     std::condition_variable m_cv;
     std::thread m_dispatch_thread;

SerialPrograms/Source/NintendoSwitch/DevPrograms/TestProgramComputer.cpp

@@ -225,7 +225,6 @@ class WatchdogTest1 : public WatchdogCallback{
 
 
 
-
 template <typename Type>
 class CheckedObject{
 public:
@@ -282,41 +281,6 @@ struct RequestManagerConfig{
 
 
 
-struct Command{
-    uint64_t seqnum;
-    uint64_t milliseconds;
-    uint64_t buttons;
-    uint16_t left_joystick_x;
-    uint16_t left_joystick_y;
-    uint16_t right_joystick_x;
-    uint16_t right_joystick_y;
-
-    void write_to_hex(char str[64]) const{
-        const char HEX_DIGITS[] = "0123456789abcdef";
-        const char* ptr = (const char*)this;
-        for (size_t c = 0; c < 64; c += 2){
-            uint8_t hi = (uint8_t)ptr[0] >> 4;
-            uint8_t lo = (uint8_t)ptr[0] & 0x0f;
-            str[c + 0] = HEX_DIGITS[hi];
-            str[c + 1] = HEX_DIGITS[lo];
-            ptr++;
-        }
-    }
-    void parse_from_hex(const char str[64]){
-        char* ptr = (char*)this;
-        for (size_t c = 0; c < 64; c += 2){
-            char hi = str[c + 0];
-            char lo = str[c + 1];
-            hi = hi < 'a' ? hi - '0' : hi - 'a' + 10;
-            lo = lo < 'a' ? lo - '0' : lo - 'a' + 10;
-            ptr[0] = hi << 4 | lo;
-            ptr++;
-        }
-    }
-};
-
-
-
 
 
 
@@ -330,6 +294,47 @@ void TestProgramComputer::program(ProgramEnvironment& env, CancellableScope& sco
 
     using namespace std::chrono_literals;
 
+
+#if 0
+    {
+        CommandQueue queue;
+
+        queue.enqueue_command(ControllerCommand{
+            .seqnum = 10,
+            .milliseconds = 1000,
+            .state = ControllerState{
+                .buttons = 123,
+            }
+        });
+        queue.enqueue_command(ControllerCommand{
+            .seqnum = 11,
+            .milliseconds = 2000,
+            .state = ControllerState{
+                .buttons = 456,
+            }
+        });
+
+        scope.wait_for(60s);
+    }
+#endif
+
+#if 0
+    HeapCircularBuffer<std::string> buffer(10);
+
+    buffer.try_push_back("asdf");
+    buffer.try_push_back("qwer");
+    buffer.try_push_back("zxcv");
+
+    cout << buffer[0] << endl;
+    cout << buffer[1] << endl;
+    cout << buffer[2] << endl;
+    cout << "--------------------" << endl;
+    buffer.pop_front();
+    cout << buffer[0] << endl;
+    cout << buffer[1] << endl;
+#endif
+
+#if 0
     Command command{
         123,
         100,
@@ -351,7 +356,7 @@ void TestProgramComputer::program(ProgramEnvironment& env, CancellableScope& sco
     cout << command.left_joystick_y << endl;
     cout << command.right_joystick_x << endl;
     cout << command.right_joystick_y << endl;
-
+#endif
 
 #if 0
     ImageRGB32 image("20250503-121259857603.png");