Commit a186a503e769f21f4637434dca203192b59f0a09

Sam Lantinga 2021-07-29T06:43:39

Added SDL_GameControllerGetSensorDataRate() to get the sensor update rate for a controller.

diff --git a/include/SDL_gamecontroller.h b/include/SDL_gamecontroller.h
index 720a75a..4c07d8f 100644
--- a/include/SDL_gamecontroller.h
+++ b/include/SDL_gamecontroller.h
@@ -759,6 +759,15 @@ extern DECLSPEC int SDLCALL SDL_GameControllerSetSensorEnabled(SDL_GameControlle
 extern DECLSPEC SDL_bool SDLCALL SDL_GameControllerIsSensorEnabled(SDL_GameController *gamecontroller, SDL_SensorType type);
 
 /**
+ * Get the data rate (number of events per second) of a game controller sensor.
+ *
+ * \param gamecontroller The controller to query
+ * \param type The type of sensor to query
+ * \return the data rate, or 0.0f if the data rate is not available.
+ */
+extern DECLSPEC float SDLCALL SDL_GameControllerGetSensorDataRate(SDL_GameController *gamecontroller, SDL_SensorType type);
+
+/**
  * Get the current state of a game controller sensor.
  *
  * The number of values and interpretation of the data is sensor dependent.
diff --git a/src/dynapi/SDL_dynapi_overrides.h b/src/dynapi/SDL_dynapi_overrides.h
index 165fa09..1e7d8bb 100644
--- a/src/dynapi/SDL_dynapi_overrides.h
+++ b/src/dynapi/SDL_dynapi_overrides.h
@@ -814,3 +814,4 @@
 #define SDL_FlashWindow SDL_FlashWindow_REAL
 #define SDL_GameControllerSendEffect SDL_GameControllerSendEffect_REAL
 #define SDL_JoystickSendEffect SDL_JoystickSendEffect_REAL
+#define SDL_GameControllerGetSensorDataRate SDL_GameControllerGetSensorDataRate_REAL
diff --git a/src/dynapi/SDL_dynapi_procs.h b/src/dynapi/SDL_dynapi_procs.h
index f878b5e..9fe7649 100644
--- a/src/dynapi/SDL_dynapi_procs.h
+++ b/src/dynapi/SDL_dynapi_procs.h
@@ -879,3 +879,4 @@ SDL_DYNAPI_PROC(void,SDL_SetWindowAlwaysOnTop,(SDL_Window *a, SDL_bool b),(a,b),
 SDL_DYNAPI_PROC(int,SDL_FlashWindow,(SDL_Window *a, SDL_FlashOperation b),(a,b),return)
 SDL_DYNAPI_PROC(int,SDL_GameControllerSendEffect,(SDL_GameController *a, const void *b, int c),(a,b,c),return)
 SDL_DYNAPI_PROC(int,SDL_JoystickSendEffect,(SDL_Joystick *a, const void *b, int c),(a,b,c),return)
+SDL_DYNAPI_PROC(float,SDL_GameControllerGetSensorDataRate,(SDL_GameController *a, SDL_SensorType b),(a,b),return)
diff --git a/src/joystick/SDL_gamecontroller.c b/src/joystick/SDL_gamecontroller.c
index f4e7a6f..1842afb 100644
--- a/src/joystick/SDL_gamecontroller.c
+++ b/src/joystick/SDL_gamecontroller.c
@@ -2182,6 +2182,29 @@ SDL_bool SDL_GameControllerIsSensorEnabled(SDL_GameController *gamecontroller, S
 }
 
 /*
+ *  Get the data rate of a game controller sensor.
+ */
+float
+SDL_GameControllerGetSensorDataRate(SDL_GameController *gamecontroller, SDL_SensorType type)
+{
+    SDL_Joystick *joystick = SDL_GameControllerGetJoystick(gamecontroller);
+    int i;
+
+    if (!joystick) {
+        return 0.0f;
+    }
+
+    for (i = 0; i < joystick->nsensors; ++i) {
+        SDL_JoystickSensorInfo *sensor = &joystick->sensors[i];
+
+        if (sensor->type == type) {
+            return sensor->rate;
+        }
+    }
+    return 0.0f;
+}
+
+/*
  *  Get the current state of a game controller sensor.
  */
 int
diff --git a/src/joystick/SDL_joystick.c b/src/joystick/SDL_joystick.c
index c48e27f..e0f27a6 100644
--- a/src/joystick/SDL_joystick.c
+++ b/src/joystick/SDL_joystick.c
@@ -1162,7 +1162,7 @@ void SDL_PrivateJoystickAddTouchpad(SDL_Joystick *joystick, int nfingers)
     }
 }
 
-void SDL_PrivateJoystickAddSensor(SDL_Joystick *joystick, SDL_SensorType type)
+void SDL_PrivateJoystickAddSensor(SDL_Joystick *joystick, SDL_SensorType type, float rate)
 {
     int nsensors = joystick->nsensors + 1;
     SDL_JoystickSensorInfo *sensors = (SDL_JoystickSensorInfo *)SDL_realloc(joystick->sensors, (nsensors * sizeof(SDL_JoystickSensorInfo)));
@@ -1171,6 +1171,7 @@ void SDL_PrivateJoystickAddSensor(SDL_Joystick *joystick, SDL_SensorType type)
 
         SDL_zerop(sensor);
         sensor->type = type;
+        sensor->rate = rate;
 
         joystick->nsensors = nsensors;
         joystick->sensors = sensors;
@@ -2707,25 +2708,23 @@ int SDL_PrivateJoystickSensor(SDL_Joystick *joystick, SDL_SensorType type, const
         if (sensor->type == type) {
             if (sensor->enabled) {
                 num_values = SDL_min(num_values, SDL_arraysize(sensor->data));
-                if (SDL_memcmp(data, sensor->data, num_values*sizeof(*data)) != 0) {
 
-                    /* Update internal sensor state */
-                    SDL_memcpy(sensor->data, data, num_values*sizeof(*data));
+                /* Update internal sensor state */
+                SDL_memcpy(sensor->data, data, num_values*sizeof(*data));
 
-                    /* Post the event, if desired */
+                /* Post the event, if desired */
 #if !SDL_EVENTS_DISABLED
-                    if (SDL_GetEventState(SDL_CONTROLLERSENSORUPDATE) == SDL_ENABLE) {
-                        SDL_Event event;
-                        event.type = SDL_CONTROLLERSENSORUPDATE;
-                        event.csensor.which = joystick->instance_id;
-                        event.csensor.sensor = type;
-                        num_values = SDL_min(num_values, SDL_arraysize(event.csensor.data));
-                        SDL_memset(event.csensor.data, 0, sizeof(event.csensor.data));
-                        SDL_memcpy(event.csensor.data, data, num_values*sizeof(*data));
-                        posted = SDL_PushEvent(&event) == 1;
-                    }
-#endif /* !SDL_EVENTS_DISABLED */
+                if (SDL_GetEventState(SDL_CONTROLLERSENSORUPDATE) == SDL_ENABLE) {
+                    SDL_Event event;
+                    event.type = SDL_CONTROLLERSENSORUPDATE;
+                    event.csensor.which = joystick->instance_id;
+                    event.csensor.sensor = type;
+                    num_values = SDL_min(num_values, SDL_arraysize(event.csensor.data));
+                    SDL_memset(event.csensor.data, 0, sizeof(event.csensor.data));
+                    SDL_memcpy(event.csensor.data, data, num_values*sizeof(*data));
+                    posted = SDL_PushEvent(&event) == 1;
                 }
+#endif /* !SDL_EVENTS_DISABLED */
             }
             break;
         }
diff --git a/src/joystick/SDL_joystick_c.h b/src/joystick/SDL_joystick_c.h
index 6db09a6..b26e529 100644
--- a/src/joystick/SDL_joystick_c.h
+++ b/src/joystick/SDL_joystick_c.h
@@ -116,7 +116,7 @@ extern void SDL_GameControllerHandleDelayedGuideButton(SDL_Joystick *joystick);
 
 /* Internal event queueing functions */
 extern void SDL_PrivateJoystickAddTouchpad(SDL_Joystick *joystick, int nfingers);
-extern void SDL_PrivateJoystickAddSensor(SDL_Joystick *joystick, SDL_SensorType type);
+extern void SDL_PrivateJoystickAddSensor(SDL_Joystick *joystick, SDL_SensorType type, float rate);
 extern void SDL_PrivateJoystickAdded(SDL_JoystickID device_instance);
 extern void SDL_PrivateJoystickRemoved(SDL_JoystickID device_instance);
 extern int SDL_PrivateJoystickAxis(SDL_Joystick *joystick,
diff --git a/src/joystick/SDL_sysjoystick.h b/src/joystick/SDL_sysjoystick.h
index bf63615..91c9a7c 100644
--- a/src/joystick/SDL_sysjoystick.h
+++ b/src/joystick/SDL_sysjoystick.h
@@ -56,6 +56,7 @@ typedef struct _SDL_JoystickSensorInfo
 {
     SDL_SensorType type;
     SDL_bool enabled;
+    float rate;
     float data[3];      /* If this needs to expand, update SDL_ControllerSensorEvent */
 } SDL_JoystickSensorInfo;
 
diff --git a/src/joystick/hidapi/SDL_hidapi_ps4.c b/src/joystick/hidapi/SDL_hidapi_ps4.c
index 0cc51d0..319dec4 100644
--- a/src/joystick/hidapi/SDL_hidapi_ps4.c
+++ b/src/joystick/hidapi/SDL_hidapi_ps4.c
@@ -432,8 +432,8 @@ HIDAPI_DriverPS4_SetEnhancedMode(SDL_HIDAPI_Device *device, SDL_Joystick *joysti
         ctx->enhanced_mode = SDL_TRUE;
 
         SDL_PrivateJoystickAddTouchpad(joystick, 2);
-        SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_GYRO);
-        SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_ACCEL);
+        SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_GYRO, 250.0f);
+        SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_ACCEL, 250.0f);
 
         HIDAPI_DriverPS4_UpdateEffects(device);
     }
diff --git a/src/joystick/hidapi/SDL_hidapi_ps5.c b/src/joystick/hidapi/SDL_hidapi_ps5.c
index 82eab6a..b0aa3c9 100644
--- a/src/joystick/hidapi/SDL_hidapi_ps5.c
+++ b/src/joystick/hidapi/SDL_hidapi_ps5.c
@@ -488,8 +488,8 @@ HIDAPI_DriverPS5_SetEnhancedMode(SDL_HIDAPI_Device *device, SDL_Joystick *joysti
         ctx->enhanced_mode = SDL_TRUE;
 
         SDL_PrivateJoystickAddTouchpad(joystick, 2);
-        SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_GYRO);
-        SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_ACCEL);
+        SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_GYRO, 250.0f);
+        SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_ACCEL, 250.0f);
 
         /* Switch into enhanced report mode */
         HIDAPI_DriverPS5_UpdateEffects(device, 0);
diff --git a/src/joystick/hidapi/SDL_hidapi_switch.c b/src/joystick/hidapi/SDL_hidapi_switch.c
index 68dcbfe..0be289f 100644
--- a/src/joystick/hidapi/SDL_hidapi_switch.c
+++ b/src/joystick/hidapi/SDL_hidapi_switch.c
@@ -898,8 +898,8 @@ HIDAPI_DriverSwitch_OpenJoystick(SDL_HIDAPI_Device *device, SDL_Joystick *joysti
         }
         
         if (input_mode == k_eSwitchInputReportIDs_FullControllerState) {
-            SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_GYRO);
-            SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_ACCEL);
+            SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_GYRO, 200.0f);
+            SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_ACCEL, 200.0f);
             ctx->m_bHasSensors = SDL_TRUE;
         }
 
@@ -1333,6 +1333,27 @@ static void HandleSimpleControllerState(SDL_Joystick *joystick, SDL_DriverSwitch
     ctx->m_lastSimpleState = *packet;
 }
 
+static void SendSensorUpdate(SDL_Joystick *joystick, SDL_DriverSwitch_Context *ctx, SDL_SensorType type, Sint16 *values)
+{
+    float data[3];
+
+    /* Note the order of components has been shuffled to match PlayStation controllers,
+     * since that's our de facto standard from already supporting those controllers, and
+     * users will want consistent axis mappings across devices.
+     */
+    data[0] = -HIDAPI_DriverSwitch_ScaleGyro(values[1]);
+    data[1] = HIDAPI_DriverSwitch_ScaleGyro(values[2]);
+    data[2] = -HIDAPI_DriverSwitch_ScaleGyro(values[0]);
+
+    /* Right Joy-Con flips some axes, so let's flip them back for consistency */
+    if (ctx->m_eControllerType == k_eSwitchDeviceInfoControllerType_JoyConRight) {
+        data[0] = -data[0];
+        data[1] = -data[1];
+    }
+
+    SDL_PrivateJoystickSensor(joystick, type, data, 3);
+}
+
 static void HandleFullControllerState(SDL_Joystick *joystick, SDL_DriverSwitch_Context *ctx, SwitchStatePacket_t *packet)
 {
     Sint16 axis;
@@ -1416,51 +1437,13 @@ static void HandleFullControllerState(SDL_Joystick *joystick, SDL_DriverSwitch_C
     }
 
     if (ctx->m_bReportSensors) {
-        float data[3];
-
-        /* Note the order of components has been shuffled to match PlayStation controllers,
-         * since that's our de facto standard from already supporting those controllers, and
-         * users will want consistent axis mappings across devices.
-         */
-        data[0] = HIDAPI_DriverSwitch_ScaleGyro(packet->imuState[0].sGyroY);
-        data[1] = HIDAPI_DriverSwitch_ScaleGyro(packet->imuState[0].sGyroZ);
-        data[2] = HIDAPI_DriverSwitch_ScaleGyro(packet->imuState[0].sGyroX);
-        data[0] += HIDAPI_DriverSwitch_ScaleGyro(packet->imuState[1].sGyroY);
-        data[1] += HIDAPI_DriverSwitch_ScaleGyro(packet->imuState[1].sGyroZ);
-        data[2] += HIDAPI_DriverSwitch_ScaleGyro(packet->imuState[1].sGyroX);
-        data[0] += HIDAPI_DriverSwitch_ScaleGyro(packet->imuState[2].sGyroY);
-        data[1] += HIDAPI_DriverSwitch_ScaleGyro(packet->imuState[2].sGyroZ);
-        data[2] += HIDAPI_DriverSwitch_ScaleGyro(packet->imuState[2].sGyroX);
-        data[0] /= -3.f;
-        data[1] /= 3.f;
-        data[2] /= -3.f;
-        /* Right Joy-Con flips some axes, so let's flip them back for consistency */
-        if (ctx->m_eControllerType == k_eSwitchDeviceInfoControllerType_JoyConRight) {
-            data[0] = -data[0];
-            data[1] = -data[1];
-        }
-
-        SDL_PrivateJoystickSensor(joystick, SDL_SENSOR_GYRO, data, 3);
-
-        data[0] = HIDAPI_DriverSwitch_ScaleAccel(packet->imuState[0].sAccelY);
-        data[1] = HIDAPI_DriverSwitch_ScaleAccel(packet->imuState[0].sAccelZ);
-        data[2] = HIDAPI_DriverSwitch_ScaleAccel(packet->imuState[0].sAccelX);
-        data[0] += HIDAPI_DriverSwitch_ScaleAccel(packet->imuState[1].sAccelY);
-        data[1] += HIDAPI_DriverSwitch_ScaleAccel(packet->imuState[1].sAccelZ);
-        data[2] += HIDAPI_DriverSwitch_ScaleAccel(packet->imuState[1].sAccelX);
-        data[0] += HIDAPI_DriverSwitch_ScaleAccel(packet->imuState[2].sAccelY);
-        data[1] += HIDAPI_DriverSwitch_ScaleAccel(packet->imuState[2].sAccelZ);
-        data[2] += HIDAPI_DriverSwitch_ScaleAccel(packet->imuState[2].sAccelX);
-        data[0] /= -3.f;
-        data[1] /= 3.f;
-        data[2] /= -3.f;
-        /* Right Joy-Con flips some axes, so let's flip them back for consistency */
-        if (ctx->m_eControllerType == k_eSwitchDeviceInfoControllerType_JoyConRight) {
-            data[0] = -data[0];
-            data[1] = -data[1];
-        }
+        SendSensorUpdate(joystick, ctx, SDL_SENSOR_GYRO, &packet->imuState[2].sGyroX);
+        SendSensorUpdate(joystick, ctx, SDL_SENSOR_GYRO, &packet->imuState[1].sGyroX);
+        SendSensorUpdate(joystick, ctx, SDL_SENSOR_GYRO, &packet->imuState[0].sGyroX);
 
-        SDL_PrivateJoystickSensor(joystick, SDL_SENSOR_ACCEL, data, 3);
+        SendSensorUpdate(joystick, ctx, SDL_SENSOR_ACCEL, &packet->imuState[2].sAccelX);
+        SendSensorUpdate(joystick, ctx, SDL_SENSOR_ACCEL, &packet->imuState[1].sAccelX);
+        SendSensorUpdate(joystick, ctx, SDL_SENSOR_ACCEL, &packet->imuState[0].sAccelX);
     }
 
     ctx->m_lastFullState = *packet;
diff --git a/src/joystick/iphoneos/SDL_mfijoystick.m b/src/joystick/iphoneos/SDL_mfijoystick.m
index 7aee4f5..1a2c28e 100644
--- a/src/joystick/iphoneos/SDL_mfijoystick.m
+++ b/src/joystick/iphoneos/SDL_mfijoystick.m
@@ -720,10 +720,10 @@ IOS_JoystickOpen(SDL_Joystick *joystick, int device_index)
                 GCController *controller = joystick->hwdata->controller;
                 GCMotion *motion = controller.motion;
                 if (motion && motion.hasRotationRate) {
-                    SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_GYRO);
+                    SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_GYRO, 0.0f);
                 }
                 if (motion && motion.hasGravityAndUserAcceleration) {
-                    SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_ACCEL);
+                    SDL_PrivateJoystickAddSensor(joystick, SDL_SENSOR_ACCEL, 0.0f);
                 }
             }
 #endif /* ENABLE_MFI_SENSORS */
diff --git a/test/testgamecontroller.c b/test/testgamecontroller.c
index b134e0e..a413df9 100644
--- a/test/testgamecontroller.c
+++ b/test/testgamecontroller.c
@@ -156,14 +156,14 @@ static void AddController(int device_index, SDL_bool verbose)
 
     if (SDL_GameControllerHasSensor(gamecontroller, SDL_SENSOR_ACCEL)) {
         if (verbose) {
-            SDL_Log("Enabling accelerometer\n");
+            SDL_Log("Enabling accelerometer at %.2f Hz\n", SDL_GameControllerGetSensorDataRate(gamecontroller, SDL_SENSOR_ACCEL));
         }
         SDL_GameControllerSetSensorEnabled(gamecontroller, SDL_SENSOR_ACCEL, SDL_TRUE);
     }
 
     if (SDL_GameControllerHasSensor(gamecontroller, SDL_SENSOR_GYRO)) {
         if (verbose) {
-            SDL_Log("Enabling gyro\n");
+            SDL_Log("Enabling gyro at %.2f Hz\n", SDL_GameControllerGetSensorDataRate(gamecontroller, SDL_SENSOR_ACCEL));
         }
         SDL_GameControllerSetSensorEnabled(gamecontroller, SDL_SENSOR_GYRO, SDL_TRUE);
     }