From c772b1c7026ced40084e63c1fe5366f39e5b4bcd Mon Sep 17 00:00:00 2001
From: wheremyfoodat <44909372+wheremyfoodat@users.noreply.github.com>
Date: Thu, 15 Aug 2024 17:31:55 +0300
Subject: [PATCH] Initial accelerometer support

---
 include/services/hid.hpp       | 12 ++++++++++++
 src/core/services/hid.cpp      | 22 +++++++++++++++++++---
 src/panda_qt/main_window.cpp   |  7 +++++++
 src/panda_sdl/frontend_sdl.cpp |  7 +++++++
 4 files changed, 45 insertions(+), 3 deletions(-)

diff --git a/include/services/hid.hpp b/include/services/hid.hpp
index bce2cc1b..a0eefb1c 100644
--- a/include/services/hid.hpp
+++ b/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) {
diff --git a/src/core/services/hid.cpp b/src/core/services/hid.cpp
index aa13096c..a7b9b13b 100644
--- a/src/core/services/hid.cpp
+++ b/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;
diff --git a/src/panda_qt/main_window.cpp b/src/panda_qt/main_window.cpp
index f1949da7..fab77d2e 100644
--- a/src/panda_qt/main_window.cpp
+++ b/src/panda_qt/main_window.cpp
@@ -615,6 +615,8 @@ 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) {
+					hid.setAccel(s16(event.csensor.data[0]), s16(-event.csensor.data[1]), s16(event.csensor.data[2]));
 				}
 				break;
 			}
@@ -624,8 +626,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);
+	}
 }
\ No newline at end of file
diff --git a/src/panda_sdl/frontend_sdl.cpp b/src/panda_sdl/frontend_sdl.cpp
index 8f9f4240..80014884 100644
--- a/src/panda_sdl/frontend_sdl.cpp
+++ b/src/panda_sdl/frontend_sdl.cpp
@@ -298,6 +298,8 @@ 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) {
+						hid.setAccel(s16(event.csensor.data[0]), s16(-event.csensor.data[1]), s16(event.csensor.data[2]));
 					}
 					break;
 				}
@@ -366,8 +368,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);
+	}
 }
\ No newline at end of file