Merge pull request #579 from wheremyfoodat/hle-dsp

Add accelerometer support to Qt/SDL

CMakeLists.txt

@@ -260,7 +260,7 @@ set(HEADER_FILES include/emulator.hpp include/helpers.hpp include/termcolor.hpp
                  include/audio/miniaudio_device.hpp include/ring_buffer.hpp include/bitfield.hpp include/audio/dsp_shared_mem.hpp
                  include/audio/hle_core.hpp include/capstone.hpp include/audio/aac.hpp include/PICA/pica_frag_config.hpp
                  include/PICA/pica_frag_uniforms.hpp include/PICA/shader_gen_types.hpp include/PICA/shader_decompiler.hpp
-                 include/sdl_gyro.hpp
+                 include/sdl_sensors.hpp
 )
 
 cmrc_add_resource_library(

include/sdl_gyro.hpp

@@ -1,17 +0,0 @@
-#pragma once
-
-#include <glm/glm.hpp>
-#include <numbers>
-
-#include "services/hid.hpp"
-
-namespace Gyro::SDL {
-	// Convert the rotation data we get from SDL sensor events to rotation data we can feed right to HID
-	// Returns [pitch, roll, yaw]
-	static glm::vec3 convertRotation(glm::vec3 rotation) {
-		// Convert the rotation from rad/s to deg/s and scale by the gyroscope coefficient in HID
-		constexpr float scale = 180.f / std::numbers::pi * HIDService::gyroscopeCoeff;
-		// The axes are also inverted, so invert scale before the multiplication.
-		return rotation * -scale;
-	}
-}  // namespace Gyro::SDL

include/sdl_sensors.hpp

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <cmath>
+#include <glm/glm.hpp>
+#include <numbers>
+
+#include "helpers.hpp"
+#include "services/hid.hpp"
+
+namespace Sensors::SDL {
+	// Convert the rotation data we get from SDL sensor events to rotation data we can feed right to HID
+	// Returns [pitch, roll, yaw]
+	static glm::vec3 convertRotation(glm::vec3 rotation) {
+		// Convert the rotation from rad/s to deg/s and scale by the gyroscope coefficient in HID
+		constexpr float scale = 180.f / std::numbers::pi * HIDService::gyroscopeCoeff;
+		// The axes are also inverted, so invert scale before the multiplication.
+		return rotation * -scale;
+	}
+
+	static glm::vec3 convertAcceleration(float* data) {
+		// Set our cap to ~9 m/s^2. The 3DS sensors cap at -930 and +930, so values above this value will get clamped to 930
+		// At rest (3DS laid flat on table), hardware reads around ~0 for x and z axis, and around ~480 for y axis due to gravity.
+		// This code tries to mimic this approximately, with offsets based on measurements from my DualShock 4.
+		static constexpr float accelMax = 9.f;
+
+		s16 x = std::clamp<s16>(s16(data[0] / accelMax * 930.f), -930, +930);
+		s16 y = std::clamp<s16>(s16(data[1] / (SDL_STANDARD_GRAVITY * accelMax) * 930.f - 350.f), -930, +930);
+		s16 z = std::clamp<s16>(s16((data[2] - 2.1f) / accelMax * 930.f), -930, +930);
+
+		return glm::vec3(x, y, z);
+	}
+}  // namespace Sensors::SDL

include/services/hid.hpp

@@ -56,6 +56,7 @@ class HIDService {
 	s16 circlePadX, circlePadY;      // Circlepad state
 	s16 touchScreenX, touchScreenY;  // Touchscreen state
 	s16 roll, pitch, yaw;            // Gyroscope state
+	s16 accelX, accelY, accelZ;      // Accelerometer state
 
 	bool accelerometerEnabled;
 	bool eventsInitialized;
@@ -87,6 +88,11 @@ class HIDService {
 		*(T*)&sharedMem[offset] = value;
 	}
 
+	template <typename T>
+	T* getSharedMemPointer(size_t offset) {
+		return (T*)&sharedMem[offset];
+	}
+
   public:
 	static constexpr float gyroscopeCoeff = 14.375f;  // Same as retail 3DS
 
@@ -130,6 +136,12 @@ class HIDService {
 	void setPitch(s16 value) { pitch = value; }
 	void setYaw(s16 value) { yaw = value; }
 
+	void setAccel(s16 x, s16 y, s16 z) {
+		accelX = x;
+		accelY = y;
+		accelZ = z;
+	}
+
 	void updateInputs(u64 currentTimestamp);
 
 	void setSharedMem(u8* ptr) {

src/core/services/hid.cpp

@@ -35,6 +35,7 @@ void HIDService::reset() {
 	circlePadX = circlePadY = 0;
 	touchScreenX = touchScreenY = 0;
 	roll = pitch = yaw = 0;
+	accelX = accelY = accelZ = 0;
 }
 
 void HIDService::handleSyncRequest(u32 messagePointer) {
@@ -189,6 +190,20 @@ void HIDService::updateInputs(u64 currentTick) {
 			writeSharedMem<u64>(0x108, currentTick);             // Write new tick count
 		}
 		writeSharedMem<u32>(0x118, nextAccelerometerIndex); // Index last updated by the HID module
+		const size_t accelEntryOffset = 0x128 + (nextAccelerometerIndex * 6);  // Offset in the array of 8 accelerometer entries
+
+		// Raw data of current accelerometer entry
+		// TODO: How is the "raw" data actually calculated?
+		s16* accelerometerDataRaw = getSharedMemPointer<s16>(0x120);
+		accelerometerDataRaw[0] = accelX;
+		accelerometerDataRaw[1] = accelY;
+		accelerometerDataRaw[2] = accelZ;
+
+		// Accelerometer entry in entry table
+		s16* accelerometerData = getSharedMemPointer<s16>(accelEntryOffset);
+		accelerometerData[0] = accelX;
+		accelerometerData[1] = accelY;
+		accelerometerData[2] = accelZ;
 		nextAccelerometerIndex = (nextAccelerometerIndex + 1) % 8; // Move to next entry
 
 		// Next, update gyro state
@@ -197,9 +212,10 @@ void HIDService::updateInputs(u64 currentTick) {
 			writeSharedMem<u64>(0x158, currentTick);             // Write new tick count
 		}
 		const size_t gyroEntryOffset = 0x178 + (nextGyroIndex * 6);  // Offset in the array of 8 touchscreen entries
-		writeSharedMem<u16>(gyroEntryOffset, pitch);
-		writeSharedMem<u16>(gyroEntryOffset + 2, yaw);
-		writeSharedMem<u16>(gyroEntryOffset + 4, roll);
+		s16* gyroData = getSharedMemPointer<s16>(gyroEntryOffset);
+		gyroData[0] = pitch;
+		gyroData[1] = yaw;
+		gyroData[2] = roll;
 
 		// Since gyroscope euler angles are relative, we zero them out here and the frontend will update them again when we receive a new rotation
 		roll = pitch = yaw = 0;

src/panda_qt/main_window.cpp

@@ -9,7 +9,7 @@
 
 #include "cheats.hpp"
 #include "input_mappings.hpp"
-#include "sdl_gyro.hpp"
+#include "sdl_sensors.hpp"
 #include "services/dsp.hpp"
 
 MainWindow::MainWindow(QApplication* app, QWidget* parent) : QMainWindow(parent), keyboardMappings(InputMappings::defaultKeyboardMappings()) {
@@ -606,7 +606,7 @@ void MainWindow::pollControllers() {
 
 			case SDL_CONTROLLERSENSORUPDATE: {
 				if (event.csensor.sensor == SDL_SENSOR_GYRO) {
-					auto rotation = Gyro::SDL::convertRotation({
+					auto rotation = Sensors::SDL::convertRotation({
 						event.csensor.data[0],
 						event.csensor.data[1],
 						event.csensor.data[2],
@@ -615,6 +615,9 @@ void MainWindow::pollControllers() {
 					hid.setPitch(s16(rotation.x));
 					hid.setRoll(s16(rotation.y));
 					hid.setYaw(s16(rotation.z));
+				} else if (event.csensor.sensor == SDL_SENSOR_ACCEL) {
+					auto accel = Sensors::SDL::convertAcceleration(event.csensor.data);
+					hid.setAccel(accel.x, accel.y, accel.z);
 				}
 				break;
 			}
@@ -624,8 +627,13 @@ void MainWindow::pollControllers() {
 
 void MainWindow::setupControllerSensors(SDL_GameController* controller) {
 	bool haveGyro = SDL_GameControllerHasSensor(controller, SDL_SENSOR_GYRO) == SDL_TRUE;
+	bool haveAccelerometer = SDL_GameControllerHasSensor(controller, SDL_SENSOR_ACCEL) == SDL_TRUE;
 
 	if (haveGyro) {
 		SDL_GameControllerSetSensorEnabled(controller, SDL_SENSOR_GYRO, SDL_TRUE);
 	}
+
+	if (haveAccelerometer) {
+		SDL_GameControllerSetSensorEnabled(controller, SDL_SENSOR_ACCEL, SDL_TRUE);
+	}
 }

src/panda_sdl/frontend_sdl.cpp

@@ -2,7 +2,7 @@
 
 #include <glad/gl.h>
 
-#include "sdl_gyro.hpp"
+#include "sdl_sensors.hpp"
 
 FrontendSDL::FrontendSDL() : keyboardMappings(InputMappings::defaultKeyboardMappings()) {
 	if (SDL_Init(SDL_INIT_VIDEO | SDL_INIT_EVENTS) < 0) {
@@ -289,7 +289,7 @@ void FrontendSDL::run() {
 									
 				case SDL_CONTROLLERSENSORUPDATE: {
 					if (event.csensor.sensor == SDL_SENSOR_GYRO) {
-						auto rotation = Gyro::SDL::convertRotation({
+						auto rotation = Sensors::SDL::convertRotation({
 							event.csensor.data[0],
 							event.csensor.data[1],
 							event.csensor.data[2],
@@ -298,6 +298,9 @@ void FrontendSDL::run() {
 						hid.setPitch(s16(rotation.x));
 						hid.setRoll(s16(rotation.y));
 						hid.setYaw(s16(rotation.z));
+					} else if (event.csensor.sensor == SDL_SENSOR_ACCEL) {
+						auto accel = Sensors::SDL::convertAcceleration(event.csensor.data);
+						hid.setAccel(accel.x, accel.y, accel.z);
 					}
 					break;
 				}
@@ -366,8 +369,13 @@ void FrontendSDL::run() {
 
 void FrontendSDL::setupControllerSensors(SDL_GameController* controller) {
 	bool haveGyro = SDL_GameControllerHasSensor(controller, SDL_SENSOR_GYRO) == SDL_TRUE;
+	bool haveAccelerometer = SDL_GameControllerHasSensor(controller, SDL_SENSOR_ACCEL) == SDL_TRUE;
 
 	if (haveGyro) {
 		SDL_GameControllerSetSensorEnabled(controller, SDL_SENSOR_GYRO, SDL_TRUE);
 	}
+
+	if (haveAccelerometer) {
+		SDL_GameControllerSetSensorEnabled(controller, SDL_SENSOR_ACCEL, SDL_TRUE);
+	}
 }