Add helpers and tests for PS4 sensors

Author: cgutman

WinUHidDevs/WinUHidDevs.vcxproj

@@ -51,6 +51,7 @@
   </PropertyGroup>
   <ItemDefinitionGroup>
     <ClCompile>
+      <LanguageStandard>stdcpp20</LanguageStandard>
       <WarningLevel>Level3</WarningLevel>
       <SDLCheck>true</SDLCheck>
       <PreprocessorDefinitions>WINUHID_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>

WinUHidDevs/WinUHidPS4.cpp

@@ -1,6 +1,8 @@
 #include "pch.h"
 #include "WinUHidPS4.h"
 
+#include <algorithm>
+
 #include <wrl/wrappers/corewrappers.h>
 using namespace Microsoft::WRL;
 
@@ -700,6 +702,11 @@ WINUHID_API VOID WinUHidPS4InitializeInputReport(PWINUHID_PS4_INPUT_REPORT Repor
 	WinUHidPS4SetTouchState(Report, 1, FALSE, 0, 0);
 
 	WinUHidPS4SetBatteryState(Report, TRUE, 100);
+
+	//
+	// Initialize accelerometer in neutral upright position
+	//
+	WinUHidPS4SetAccelState(Report, 0, 9.80665f, 0);
 }
 
 WINUHID_API VOID WinUHidPS4SetTouchState(PWINUHID_PS4_INPUT_REPORT Report, UCHAR TouchIndex, BOOL TouchDown, USHORT TouchX, USHORT TouchY)
@@ -719,6 +726,24 @@ WINUHID_API VOID WinUHidPS4SetTouchState(PWINUHID_PS4_INPUT_REPORT Report, UCHAR
 	Report->TouchReports[0].Timestamp++;
 }
 
+WINUHID_API VOID WinUHidPS4SetAccelState(PWINUHID_PS4_INPUT_REPORT Report, float AccelX, float AccelY, float AccelZ)
+{
+	static const float k_AccelSensitivity = 0.000980664976f;
+
+	Report->AccelX = std::clamp((int)(AccelX / k_AccelSensitivity), SHRT_MIN, SHRT_MAX);
+	Report->AccelY = std::clamp((int)(AccelY / k_AccelSensitivity), SHRT_MIN, SHRT_MAX);
+	Report->AccelZ = std::clamp((int)(AccelZ / k_AccelSensitivity), SHRT_MIN, SHRT_MAX);
+}
+
+WINUHID_API VOID WinUHidPS4SetGyroState(PWINUHID_PS4_INPUT_REPORT Report, float GyroX, float GyroY, float GyroZ)
+{
+	static const float k_GyroSensitivity = 0.000872664619f;
+
+	Report->GyroX = std::clamp((int)(GyroX / k_GyroSensitivity), SHRT_MIN, SHRT_MAX);
+	Report->GyroY = std::clamp((int)(GyroY / k_GyroSensitivity), SHRT_MIN, SHRT_MAX);
+	Report->GyroZ = std::clamp((int)(GyroZ / k_GyroSensitivity), SHRT_MIN, SHRT_MAX);
+}
+
 WINUHID_API VOID WinUHidPS4SetBatteryState(PWINUHID_PS4_INPUT_REPORT Report, BOOL Wired, UCHAR Percentage)
 {
 	if (Percentage == 100 && Wired) {

WinUHidDevs/WinUHidPS4.h

@@ -131,6 +131,20 @@ WINUHID_API VOID WinUHidPS4SetBatteryState(PWINUHID_PS4_INPUT_REPORT Report, BOO
 //
 WINUHID_API VOID WinUHidPS4SetTouchState(PWINUHID_PS4_INPUT_REPORT Report, UCHAR TouchIndex, BOOL TouchDown, USHORT TouchX, USHORT TouchY);
 
+//
+// Sets the accelerometer state in the input report.
+//
+// The values provided should be in meters per second squared.
+//
+WINUHID_API VOID WinUHidPS4SetAccelState(PWINUHID_PS4_INPUT_REPORT Report, float AccelX, float AccelY, float AccelZ);
+
+//
+// Sets the gyroscope state in the input report.
+//
+// The values provided should be in radians per second.
+//
+WINUHID_API VOID WinUHidPS4SetGyroState(PWINUHID_PS4_INPUT_REPORT Report, float GyroX, float GyroY, float GyroZ);
+
 //
 // Submits an input report to the device.
 //

WinUHidUnitTests/PS4.cpp

@@ -182,6 +182,73 @@ TEST(PS4, AxisMapping) {
 	WinUHidPS4Destroy(gamepad);
 }
 
+TEST(PS4, SensorMapping) {
+	auto gamepad = WinUHidPS4Create(NULL, NULL, NULL, NULL);
+	ASSERT_TRUE(gamepad) << "Failed to create PS4 gamepad";
+
+	SDLGamepadManager gm;
+	ASSERT_EQ(gm.GetGamepadCount(), 1) << "Unable to detect PS4 gamepad with SDL";
+
+	//
+	// Sensors must be enabled before they can be read
+	//
+	ASSERT_TRUE(SDL_SetGamepadSensorEnabled(gm.GetGamepad(0), SDL_SENSOR_GYRO, true));
+	ASSERT_TRUE(SDL_SetGamepadSensorEnabled(gm.GetGamepad(0), SDL_SENSOR_ACCEL, true));
+
+	WINUHID_PS4_INPUT_REPORT report;
+	WinUHidPS4InitializeInputReport(&report);
+
+	for (float value = -28; value <= 28; value += 0.25) {
+		WinUHidPS4SetGyroState(&report, value, 0, 0);
+		ASSERT_EQ(WinUHidPS4ReportInput(gamepad, &report), TRUE);
+
+		float expected[3] = { value, 0, 0 };
+		gm.ExpectSensorData(SDL_SENSOR_GYRO, expected);
+	}
+
+	for (float value = -28; value <= 28; value += 0.25) {
+		WinUHidPS4SetGyroState(&report, 0, value, 0);
+		ASSERT_EQ(WinUHidPS4ReportInput(gamepad, &report), TRUE);
+
+		float expected[3] = { 0, value, 0 };
+		gm.ExpectSensorData(SDL_SENSOR_GYRO, expected);
+	}
+
+	for (float value = -28; value <= 28; value += 0.25) {
+		WinUHidPS4SetGyroState(&report, 0, 0, value);
+		ASSERT_EQ(WinUHidPS4ReportInput(gamepad, &report), TRUE);
+
+		float expected[3] = { 0, 0, value };
+		gm.ExpectSensorData(SDL_SENSOR_GYRO, expected);
+	}
+
+	for (float value = -32; value <= 32; value += 0.25) {
+		WinUHidPS4SetAccelState(&report, value, 0, 0);
+		ASSERT_EQ(WinUHidPS4ReportInput(gamepad, &report), TRUE);
+
+		float expected[3] = { value, 0, 0 };
+		gm.ExpectSensorData(SDL_SENSOR_ACCEL, expected);
+	}
+
+	for (float value = -32; value <= 32; value += 0.25) {
+		WinUHidPS4SetAccelState(&report, 0, value, 0);
+		ASSERT_EQ(WinUHidPS4ReportInput(gamepad, &report), TRUE);
+
+		float expected[3] = { 0, value, 0 };
+		gm.ExpectSensorData(SDL_SENSOR_ACCEL, expected);
+	}
+
+	for (float value = -32; value <= 32; value += 0.25) {
+		WinUHidPS4SetAccelState(&report, 0, 0, value);
+		ASSERT_EQ(WinUHidPS4ReportInput(gamepad, &report), TRUE);
+
+		float expected[3] = { 0, 0, value };
+		gm.ExpectSensorData(SDL_SENSOR_ACCEL, expected);
+	}
+
+	WinUHidPS4Destroy(gamepad);
+}
+
 //
 // The touchpad is 943 points wide, but SDL treats it as 920. Since we want to
 // validate against SDL's representation, we must also use 920 as the width.

WinUHidUnitTests/Utilities.cpp

@@ -144,4 +144,24 @@ void SDLGamepadManager::ExpectTouchpadFingerState(int Finger, bool Down, float T
 		EXPECT_FLOAT_EQ(actualX, TouchX) << "Finger " << Finger << " has unexpected X coordinate";
 		EXPECT_FLOAT_EQ(actualY, TouchY) << "Finger " << Finger << " has unexpected Y coordinate";
 	}
+}
+
+void SDLGamepadManager::ExpectSensorData(SDL_SensorType sensor, float* values) {
+	for (int i = 0; i < 100; i++) {
+		float actualValues[3];
+		EXPECT_TRUE(SDL_GetGamepadSensorData(GetGamepad(0), sensor, actualValues, ARRAYSIZE(actualValues)));
+
+		if (std::fabs(actualValues[0] - values[0]) < 0.01 &&
+			std::fabs(actualValues[1] - values[1]) < 0.01 &&
+			std::fabs(actualValues[2] - values[2]) < 0.01) {
+			break;
+		}
+		SDL_Delay(1);
+	}
+
+	float actualValues[3];
+	EXPECT_TRUE(SDL_GetGamepadSensorData(GetGamepad(0), sensor, actualValues, ARRAYSIZE(actualValues)));
+	ASSERT_NEAR(actualValues[0], values[0], 0.01) << "Sensor " << sensor << "has unexpected X coordinate";
+	ASSERT_NEAR(actualValues[1], values[1], 0.01) << "Sensor " << sensor << "has unexpected Y coordinate";
+	ASSERT_NEAR(actualValues[2], values[2], 0.01) << "Sensor " << sensor << "has unexpected Z coordinate";
 }

WinUHidUnitTests/Utilities.h

@@ -22,6 +22,7 @@ class SDLGamepadManager
 	void ExpectHatState(int HatX, int HatY);
 	void ExpectAxisValue(SDL_GamepadAxis Axis, Sint16 Value);
 	void ExpectTouchpadFingerState(int Finger, bool Down, float TouchX, float TouchY);
+	void ExpectSensorData(SDL_SensorType sensor, float* values);
 
 private:
 	static int EventPollThread(void* ptr);