fixed gyro value report

mariotaku · Oct 6, 2023 · 875bf81 · 875bf81
1 parent 9feaf65
commit 875bf81
Show file tree

Hide file tree

Showing 3 changed files with 44 additions and 6 deletions.
diff --git a/src/app/input/app_input.h b/src/app/input/app_input.h
@@ -10,6 +10,12 @@
 
 typedef struct app_t app_t;
 
+typedef struct app_gamepad_sensor_state_t {
+    uint32_t periodMs;
+    uint32_t lastTimestamp;
+    float data[3];
+} app_gamepad_sensor_state_t;
+
 typedef struct app_gamepad_state_t {
     char leftTrigger, rightTrigger;
     short leftStickX, leftStickY;
@@ -20,6 +26,10 @@ typedef struct app_gamepad_state_t {
 #if !SDL_VERSION_ATLEAST(2, 0, 9)
     SDL_Haptic *haptic;
     int haptic_effect_id;
+#endif
+#if SDL_VERSION_ATLEAST(2, 0, 14)
+    app_gamepad_sensor_state_t accelState;
+    app_gamepad_sensor_state_t gyroState;
 #endif
     short id;
     bool initialized;

diff --git a/src/app/input/input_gamepad.c b/src/app/input/input_gamepad.c
@@ -169,22 +169,24 @@ void app_input_gamepad_rumble_triggers(app_input_t *input, unsigned short contro
 void app_input_gamepad_set_motion_event_state(app_input_t *input, unsigned short controllerNumber, uint8_t motionType,
                                               uint16_t reportRateHz) {
 #if SDL_VERSION_ATLEAST(2, 0, 14)
+    app_gamepad_state_t *gamepad = &input->gamepads[controllerNumber];
     SDL_SensorType sensor_type = SDL_SENSOR_INVALID;
     switch (motionType) {
         case LI_MOTION_TYPE_ACCEL:
             sensor_type = SDL_SENSOR_ACCEL;
+            gamepad->accelState.periodMs = reportRateHz > 0 ? 1000 / reportRateHz : 0;
             break;
         case LI_MOTION_TYPE_GYRO:
             sensor_type = SDL_SENSOR_GYRO;
+            gamepad->gyroState.periodMs = reportRateHz > 0 ? 1000 / reportRateHz : 0;
             break;
         default:
             break;
     }
     if (sensor_type == SDL_SENSOR_INVALID) {
         return;
     }
-    SDL_GameController *controller = input->gamepads[controllerNumber].controller;
-    SDL_GameControllerSetSensorEnabled(controller, sensor_type, reportRateHz > 0 ? SDL_TRUE : SDL_FALSE);
+    SDL_GameControllerSetSensorEnabled(gamepad->controller, sensor_type, reportRateHz > 0 ? SDL_TRUE : SDL_FALSE);
     commons_log_info("Input", "Setting motion event state for controller %d, motionType: %d, reportRateHz: %d",
                      controllerNumber, motionType, reportRateHz);
 #endif

diff --git a/src/app/stream/input/session_gamepad.c b/src/app/stream/input/session_gamepad.c
@@ -21,6 +21,8 @@ static void release_buttons(stream_input_t *input, app_gamepad_state_t *gamepad)
 
 static bool gamepad_combo_check(int buttons, short combo);
 
+static bool sensor_state_needs_update(const app_gamepad_sensor_state_t *state, uint32_t timestamp,
+                                      const float data[3]);
 
 void stream_input_handle_cbutton(stream_input_t *input, const SDL_ControllerButtonEvent *event) {
     int button = 0;
@@ -195,21 +197,32 @@ void stream_input_handle_csensor(stream_input_t *input, const SDL_ControllerSens
         return;
     }
     app_gamepad_state_t *gamepad = app_input_gamepad_get(input->input, event->which);
-    uint8_t event_type;
     switch (event->sensor) {
         case SDL_SENSOR_ACCEL: {
-            event_type = LI_MOTION_TYPE_ACCEL;
+            if (sensor_state_needs_update(&gamepad->accelState, event->timestamp, event->data)) {
+                gamepad->accelState.lastTimestamp = event->timestamp;
+                memcpy(gamepad->accelState.data, event->data, sizeof(gamepad->accelState.data));
+                LiSendControllerMotionEvent(gamepad->id, LI_MOTION_TYPE_ACCEL, event->data[0], event->data[1],
+                                            event->data[2]);
+            }
             break;
         }
         case SDL_SENSOR_GYRO: {
-            event_type = LI_MOTION_TYPE_GYRO;
+            if (sensor_state_needs_update(&gamepad->gyroState, event->timestamp, event->data)) {
+                gamepad->gyroState.lastTimestamp = event->timestamp;
+                memcpy(gamepad->gyroState.data, event->data, sizeof(gamepad->gyroState.data));
+                // Convert rad/s to deg/s
+                LiSendControllerMotionEvent(gamepad->id, LI_MOTION_TYPE_GYRO,
+                                            event->data[0] * 57.2957795f,
+                                            event->data[1] * 57.2957795f,
+                                            event->data[2] * 57.2957795f);
+            }
             break;
         }
         default: {
             return;
         }
     }
-    LiSendControllerMotionEvent(gamepad->id, event_type, event->data[0], event->data[1], event->data[2]);
 }
 
 void stream_input_handle_ctouchpad(stream_input_t *input, const SDL_ControllerTouchpadEvent *event) {
@@ -328,4 +341,17 @@ static void release_buttons(stream_input_t *input, app_gamepad_state_t *gamepad)
 
 static bool gamepad_combo_check(int buttons, short combo) {
     return (buttons & combo) == combo;
+}
+
+static bool sensor_state_needs_update(const app_gamepad_sensor_state_t *state, uint32_t timestamp,
+                                      const float data[3]) {
+    if (state->periodMs == 0 || !SDL_TICKS_PASSED(timestamp, state->lastTimestamp + state->periodMs)) {
+        return false;
+    }
+    for (int i = 0; i < 3; i++) {
+        if (state->data[i] != data[i]) {
+            return true;
+        }
+    }
+    return false;
 }