Use threading in Bluetooth mode to improve responsiveness

tracking/main_loop.cc

@@ -43,7 +43,7 @@ static inline float highpass(float oldVal, float newVal)
     return newVal + alpha * delta;
 }
 
-void sendCurrentState(MouseMoveCallback mouse_move)
+void sendCurrentState(MouseMoveCallback mouse_move, void *context)
 {
     float dX = g_dYaw * CURSOR_SPEED;
     float dY = g_dPitch * CURSOR_SPEED;
@@ -69,7 +69,7 @@ void sendCurrentState(MouseMoveCallback mouse_move)
     const int8_t x = (int8_t)std::floor(dX + 0.5);
     const int8_t y = (int8_t)std::floor(dY + 0.5);
 
-    mouse_move(x, y);
+    mouse_move(x, y, context);
 
     // Only subtract the part of the error that was already sent.
     if (x != 0) {
@@ -160,7 +160,7 @@ void tracking_begin() {
     tracker.Resume();
 }
 
-void tracking_step(MouseMoveCallback mouse_move) {
+void tracking_step(MouseMoveCallback mouse_move, void *context) {
     double vec[6];
     int ret = imu_read(vec);
     if (ret != 0) {
@@ -177,7 +177,7 @@ void tracking_step(MouseMoveCallback mouse_move) {
                     .data = cardboard::Vector3(vec[3], vec[4], vec[5]) };
             cardboard::Vector4 pose = tracker.OnGyroscopeData(gdata);
             onOrientation(pose);
-            sendCurrentState(mouse_move);
+            sendCurrentState(mouse_move, context);
         }
     }
 }

tracking/main_loop.h

@@ -6,14 +6,14 @@
 extern "C" {
 #endif
 
-typedef bool (*MouseMoveCallback)(int8_t x, int8_t y);
+typedef bool (*MouseMoveCallback)(int8_t x, int8_t y, void *context);
 
 void calibration_begin();
 bool calibration_step();
 void calibration_end();
 
 void tracking_begin();
-void tracking_step(MouseMoveCallback mouse_move);
+void tracking_step(MouseMoveCallback mouse_move, void *context);
 void tracking_end();
 
 #ifdef __cplusplus

views/bt_mouse.c

@@ -10,16 +10,49 @@
 #include <notification/notification.h>
 #include <notification/notification_messages.h>
 
+typedef struct ButtonEvent {
+    int8_t button;
+    bool state;
+} ButtonEvent;
+
+#define BTN_EVT_QUEUE_SIZE 32
+
 struct BtMouse {
     View* view;
     ViewDispatcher* view_dispatcher;
     Bt* bt;
     NotificationApp* notifications;
+    FuriMutex* mutex;
+    FuriThread* thread;
+    bool connected;
+
+    // Current mouse state
+    uint8_t btn;
+    int dx;
+    int dy;
+    int wheel;
+
+    // Circular buffer;
+    // (qhead == qtail) means either empty or overflow.
+    // We'll ignore overflow and treat it as empty.
+    int qhead;
+    int qtail;
+    ButtonEvent queue[BTN_EVT_QUEUE_SIZE];
 };
 
+#define BT_MOUSE_FLAG_INPUT_EVENT (1UL << 0)
+#define BT_MOUSE_FLAG_KILL_THREAD (1UL << 1)
+#define BT_MOUSE_FLAG_ALL (BT_MOUSE_FLAG_INPUT_EVENT | BT_MOUSE_FLAG_KILL_THREAD)
+
 #define MOUSE_MOVE_SHORT 5
 #define MOUSE_MOVE_LONG 20
 
+static void bt_mouse_notify_event(BtMouse* bt_mouse) {
+    FuriThreadId thread_id = furi_thread_get_id(bt_mouse->thread);
+    furi_assert(thread_id);
+    furi_thread_flags_set(thread_id, BT_MOUSE_FLAG_INPUT_EVENT);
+}
+
 static void bt_mouse_draw_callback(Canvas* canvas, void* context) {
     UNUSED(context);
     canvas_clear(canvas);
@@ -29,6 +62,20 @@ static void bt_mouse_draw_callback(Canvas* canvas, void* context) {
     canvas_draw_str(canvas, 0, 63, "Hold [back] to exit");
 }
 
+static void bt_mouse_button_state(BtMouse* bt_mouse, int8_t button, bool state) {
+    ButtonEvent event;
+    event.button = button;
+    event.state = state;
+
+    if (bt_mouse->connected) {
+        furi_mutex_acquire(bt_mouse->mutex, FuriWaitForever);
+        bt_mouse->queue[bt_mouse->qtail++] = event;
+        bt_mouse->qtail %= BTN_EVT_QUEUE_SIZE;
+        furi_mutex_release(bt_mouse->mutex);
+        bt_mouse_notify_event(bt_mouse);
+    }
+}
+
 static void bt_mouse_process(BtMouse* bt_mouse, InputEvent* event) {
     with_view_model(
         bt_mouse->view,
@@ -37,21 +84,21 @@ static void bt_mouse_process(BtMouse* bt_mouse, InputEvent* event) {
             UNUSED(model);
             if(event->key == InputKeyUp) {
                 if(event->type == InputTypePress) {
-                    furi_hal_bt_hid_mouse_press(HID_MOUSE_BTN_LEFT);
+                    bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_LEFT, true);
                 } else if(event->type == InputTypeRelease) {
-                    furi_hal_bt_hid_mouse_release(HID_MOUSE_BTN_LEFT);
+                    bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_LEFT, false);
                 }
             } else if(event->key == InputKeyDown) {
                 if(event->type == InputTypePress) {
-                    furi_hal_bt_hid_mouse_press(HID_MOUSE_BTN_RIGHT);
+                    bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_RIGHT, true);
                 } else if(event->type == InputTypeRelease) {
-                    furi_hal_bt_hid_mouse_release(HID_MOUSE_BTN_RIGHT);
+                    bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_RIGHT, false);
                 }
             } else if(event->key == InputKeyOk) {
                 if(event->type == InputTypePress) {
-                    furi_hal_bt_hid_mouse_press(HID_MOUSE_BTN_WHEEL);
+                    bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_WHEEL, true);
                 } else if(event->type == InputTypeRelease) {
-                    furi_hal_bt_hid_mouse_release(HID_MOUSE_BTN_WHEEL);
+                    bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_WHEEL, false);
                 }
             }
         },
@@ -77,10 +124,13 @@ void bt_mouse_connection_status_changed_callback(BtStatus status, void* context)
     furi_assert(context);
     BtMouse* bt_mouse = context;
 
-    bool connected = (status == BtStatusConnected);
-    if(connected) {
+    bt_mouse->connected = (status == BtStatusConnected);
+    if(bt_mouse->connected) {
         notification_internal_message(bt_mouse->notifications, &sequence_set_blue_255);
+        tracking_begin();
+        view_dispatcher_send_custom_event(bt_mouse->view_dispatcher, 0);
     } else {
+        tracking_end();
         notification_internal_message(bt_mouse->notifications, &sequence_reset_blue);
     }
 
@@ -88,6 +138,21 @@ void bt_mouse_connection_status_changed_callback(BtStatus status, void* context)
     //    bt_mouse->view, void * model, { model->connected = connected; }, true);
 }
 
+bool bt_mouse_move(int8_t dx, int8_t dy, void *context) {
+    furi_assert(context);
+    BtMouse* bt_mouse = context;
+
+    if (bt_mouse->connected) {
+        furi_mutex_acquire(bt_mouse->mutex, FuriWaitForever);
+        bt_mouse->dx += dx;
+        bt_mouse->dy += dy;
+        furi_mutex_release(bt_mouse->mutex);
+        bt_mouse_notify_event(bt_mouse);
+    }
+
+    return true;
+}
+
 void bt_mouse_enter_callback(void* context) {
     furi_assert(context);
     BtMouse* bt_mouse = context;
@@ -98,10 +163,6 @@ void bt_mouse_enter_callback(void* context) {
         bt_mouse->bt, bt_mouse_connection_status_changed_callback, bt_mouse);
     furi_assert(bt_set_profile(bt_mouse->bt, BtProfileHidKeyboard));
     furi_hal_bt_start_advertising();
-
-    tracking_begin();
-
-    view_dispatcher_send_custom_event(bt_mouse->view_dispatcher, 0);
 }
 
 bool bt_mouse_custom_callback(uint32_t event, void* context) {
@@ -109,7 +170,8 @@ bool bt_mouse_custom_callback(uint32_t event, void* context) {
     furi_assert(context);
     BtMouse* bt_mouse = context;
 
-    tracking_step(furi_hal_bt_hid_mouse_move);
+    tracking_step(bt_mouse_move, context);
+    furi_delay_ms(3); // Magic! Removing this will break the buttons
 
     view_dispatcher_send_custom_event(bt_mouse->view_dispatcher, 0);
     return true;
@@ -120,7 +182,6 @@ void bt_mouse_exit_callback(void* context) {
     BtMouse* bt_mouse = context;
 
     tracking_end();
-
     notification_internal_message(bt_mouse->notifications, &sequence_reset_blue);
 
     furi_hal_bt_stop_advertising();
@@ -132,8 +193,91 @@ void bt_mouse_exit_callback(void* context) {
     bt_mouse->bt = NULL;
 }
 
+static int8_t clamp(int t) {
+    if (t < -128) {
+        return -128;
+    }
+    else if (t > 127) {
+        return 127;
+    }
+    return t;
+}
+
+static int32_t bt_mouse_thread_callback(void* context) {
+    furi_assert(context);
+    BtMouse* bt_mouse = (BtMouse*)context;
+
+    while(1) {
+        uint32_t flags = furi_thread_flags_wait(BT_MOUSE_FLAG_ALL, FuriFlagWaitAny, FuriWaitForever);
+        if(flags & BT_MOUSE_FLAG_KILL_THREAD) {
+            break;
+        }
+        if(flags & BT_MOUSE_FLAG_INPUT_EVENT) {
+            furi_mutex_acquire(bt_mouse->mutex, FuriWaitForever);
+
+            ButtonEvent event;
+            bool send_buttons = false;
+            if (bt_mouse->qhead != bt_mouse->qtail) {
+                event = bt_mouse->queue[bt_mouse->qhead++];
+                bt_mouse->qhead %= BTN_EVT_QUEUE_SIZE;
+                send_buttons = true;
+            }
+
+            int8_t dx = clamp(bt_mouse->dx);
+            bt_mouse->dx -= dx;
+            int8_t dy = clamp(bt_mouse->dy);
+            bt_mouse->dy -= dy;
+            int8_t wheel = clamp(bt_mouse->wheel);
+            bt_mouse->wheel -= wheel;
+
+            furi_mutex_release(bt_mouse->mutex);
+
+            if (bt_mouse->connected && send_buttons) {
+                if (event.state) {
+                    furi_hal_bt_hid_mouse_press(event.button);
+                } else {
+                    furi_hal_bt_hid_mouse_release(event.button);
+                }
+            }
+
+            if (bt_mouse->connected && (dx != 0 || dy != 0)) {
+                furi_hal_bt_hid_mouse_move(dx, dy);
+            }
+
+            if (bt_mouse->connected && wheel != 0) {
+                furi_hal_bt_hid_mouse_scroll(wheel);
+            }
+        }
+    }
+
+    return 0;
+}
+
+void bt_mouse_thread_start(BtMouse* bt_mouse) {
+    furi_assert(bt_mouse);
+    bt_mouse->mutex = furi_mutex_alloc(FuriMutexTypeNormal);
+    bt_mouse->thread = furi_thread_alloc();
+    furi_thread_set_name(bt_mouse->thread, "BtSender");
+    furi_thread_set_stack_size(bt_mouse->thread, 1024);
+    furi_thread_set_context(bt_mouse->thread, bt_mouse);
+    furi_thread_set_callback(bt_mouse->thread, bt_mouse_thread_callback);
+    furi_thread_start(bt_mouse->thread);
+}
+
+void bt_mouse_thread_stop(BtMouse* bt_mouse) {
+    furi_assert(bt_mouse);
+    FuriThreadId thread_id = furi_thread_get_id(bt_mouse->thread);
+    furi_assert(thread_id);
+    furi_thread_flags_set(thread_id, BT_MOUSE_FLAG_KILL_THREAD);
+    furi_thread_join(bt_mouse->thread);
+    furi_thread_free(bt_mouse->thread);
+    furi_mutex_free(bt_mouse->mutex);
+}
+
 BtMouse* bt_mouse_alloc(ViewDispatcher* view_dispatcher) {
     BtMouse* bt_mouse = malloc(sizeof(BtMouse));
+    memset(bt_mouse, 0, sizeof(BtMouse));
+
     bt_mouse->view = view_alloc();
     bt_mouse->view_dispatcher = view_dispatcher;
     view_set_context(bt_mouse->view, bt_mouse);
@@ -142,11 +286,13 @@ BtMouse* bt_mouse_alloc(ViewDispatcher* view_dispatcher) {
     view_set_enter_callback(bt_mouse->view, bt_mouse_enter_callback);
     view_set_custom_callback(bt_mouse->view, bt_mouse_custom_callback);
     view_set_exit_callback(bt_mouse->view, bt_mouse_exit_callback);
+    bt_mouse_thread_start(bt_mouse);
     return bt_mouse;
 }
 
 void bt_mouse_free(BtMouse* bt_mouse) {
     furi_assert(bt_mouse);
+    bt_mouse_thread_stop(bt_mouse);
     view_free(bt_mouse->view);
     free(bt_mouse);
 }

views/usb_mouse.c

@@ -78,13 +78,18 @@ void usb_mouse_enter_callback(void* context) {
     view_dispatcher_send_custom_event(usb_mouse->view_dispatcher, 0);
 }
 
+bool usb_mouse_move(int8_t dx, int8_t dy, void *context) {
+    UNUSED(context);
+    return furi_hal_hid_mouse_move(dx, dy);
+}
+
 bool usb_mouse_custom_callback(uint32_t event, void* context) {
     UNUSED(event);
     furi_assert(context);
     UsbMouse* usb_mouse = context;
 
-    tracking_step(furi_hal_hid_mouse_move);
-    furi_delay_ms(1); // Magic! Removing this will break the buttons
+    tracking_step(usb_mouse_move, context);
+    furi_delay_ms(3); // Magic! Removing this will break the buttons
 
     view_dispatcher_send_custom_event(usb_mouse->view_dispatcher, 0);
     return true;