From 53a572a4a459bc85ebc26b33ea09c591cc8b9cc7 Mon Sep 17 00:00:00 2001
From: Cameron Gutman <aicommander@gmail.com>
Date: Sun, 2 Jul 2023 19:22:00 -0500
Subject: [PATCH] Add controller motion deduplication and fix gyro units

---
 app/streaming/input/gamepad.cpp | 20 ++++++++++++++++----
 app/streaming/input/input.h     |  2 ++
 2 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/app/streaming/input/gamepad.cpp b/app/streaming/input/gamepad.cpp
index 2463c800..16bdd8c9 100644
--- a/app/streaming/input/gamepad.cpp
+++ b/app/streaming/input/gamepad.cpp
@@ -364,15 +364,27 @@ void SdlInputHandler::handleControllerSensorEvent(SDL_ControllerSensorEvent* eve
 
     switch (event->sensor) {
     case SDL_SENSOR_ACCEL:
-        if (state->accelReportPeriodMs && SDL_TICKS_PASSED(event->timestamp, state->lastAccelEventTime + state->accelReportPeriodMs)) {
-            LiSendControllerMotionEvent((uint8_t)state->index, LI_MOTION_TYPE_ACCEL, event->data[0], event->data[1], event->data[2]);
+        if (state->accelReportPeriodMs &&
+                SDL_TICKS_PASSED(event->timestamp, state->lastAccelEventTime + state->accelReportPeriodMs) &&
+                memcmp(event->data, state->lastAccelEventData, sizeof(event->data)) != 0) {
+            memcpy(state->lastAccelEventData, event->data, sizeof(event->data));
             state->lastAccelEventTime = event->timestamp;
+
+            LiSendControllerMotionEvent((uint8_t)state->index, LI_MOTION_TYPE_ACCEL, event->data[0], event->data[1], event->data[2]);
         }
         break;
     case SDL_SENSOR_GYRO:
-        if (state->gyroReportPeriodMs && SDL_TICKS_PASSED(event->timestamp, state->lastGyroEventTime + state->gyroReportPeriodMs)) {
-            LiSendControllerMotionEvent((uint8_t)state->index, LI_MOTION_TYPE_GYRO, event->data[0], event->data[1], event->data[2]);
+        if (state->gyroReportPeriodMs &&
+                SDL_TICKS_PASSED(event->timestamp, state->lastGyroEventTime + state->gyroReportPeriodMs) &&
+                memcmp(event->data, state->lastGyroEventData, sizeof(event->data)) != 0) {
+            memcpy(state->lastGyroEventData, event->data, sizeof(event->data));
             state->lastGyroEventTime = event->timestamp;
+
+            // Convert rad/s to deg/s
+            LiSendControllerMotionEvent((uint8_t)state->index, LI_MOTION_TYPE_GYRO,
+                                        event->data[0] * 57.2957795f,
+                                        event->data[1] * 57.2957795f,
+                                        event->data[2] * 57.2957795f);
         }
         break;
     }
diff --git a/app/streaming/input/input.h b/app/streaming/input/input.h
index c146a8f2..b8d569e6 100644
--- a/app/streaming/input/input.h
+++ b/app/streaming/input/input.h
@@ -20,9 +20,11 @@ struct GamepadState {
     uint32_t lastStartDownTime;
 
     uint8_t gyroReportPeriodMs;
+    float lastGyroEventData[SDL_arraysize(SDL_ControllerSensorEvent::data)];
     uint32_t lastGyroEventTime;
 
     uint8_t accelReportPeriodMs;
+    float lastAccelEventData[SDL_arraysize(SDL_ControllerSensorEvent::data)];
     uint32_t lastAccelEventTime;
 
     int buttons;