Trying to add touchpad and motion control, based on dolphin78 works

gui/include/controllermanager.h

@@ -4,6 +4,7 @@
 #define CHIAKI_CONTROLLERMANAGER_H
 
 #include <chiaki/controller.h>
+#include <chiaki/orientation.h>
 
 #include <QObject>
 #include <QSet>
@@ -61,6 +62,7 @@ class Controller : public QObject
 
 		ControllerManager *manager;
 		int id;
+		ChiakiOrientationTracker orient_tracker;
 
 #ifdef CHIAKI_GUI_ENABLE_SDL_GAMECONTROLLER
 		SDL_GameController *controller;

gui/src/controllermanager.cpp

@@ -6,6 +6,7 @@
 #include <QMessageBox>
 #include <QByteArray>
 #include <QTimer>
+#include <chrono>
 
 #ifdef CHIAKI_GUI_ENABLE_SDL_GAMECONTROLLER
 #include <SDL.h>
@@ -95,6 +96,11 @@ static ControllerManager *instance = nullptr;
 
 #define UPDATE_INTERVAL_MS 4
 
+static float inv_sqrt(float x)
+{
+	return 1.0f / sqrt(x);
+}
+
 ControllerManager *ControllerManager::GetInstance()
 {
 	if(!instance)
@@ -187,6 +193,14 @@ void ControllerManager::HandleEvents()
 			case SDL_CONTROLLERAXISMOTION:
 				ControllerEvent(event.caxis.which);
 				break;
+			case SDL_CONTROLLERTOUCHPADDOWN:
+			case SDL_CONTROLLERTOUCHPADMOTION:
+			case SDL_CONTROLLERTOUCHPADUP:
+				ControllerEvent(event.ctouchpad.which);
+				break;
+			case SDL_CONTROLLERSENSORUPDATE:
+				ControllerEvent(event.csensor.which);
+				break;
 		}
 	}
 #endif
@@ -234,9 +248,14 @@ Controller::Controller(int device_id, ControllerManager *manager) : QObject(mana
 		if(SDL_JoystickGetDeviceInstanceID(i) == device_id)
 		{
 			controller = SDL_GameControllerOpen(i);
+			SDL_SetHint(SDL_HINT_JOYSTICK_HIDAPI_PS4_RUMBLE, "1");
+			SDL_SetHint(SDL_HINT_JOYSTICK_HIDAPI_PS5, "1");
+			SDL_GameControllerSetSensorEnabled(controller, SDL_SENSOR_ACCEL, SDL_TRUE);
+			SDL_GameControllerSetSensorEnabled(controller, SDL_SENSOR_GYRO, SDL_TRUE);
 			break;
 		}
 	}
+	chiaki_orientation_tracker_init(&orient_tracker);
 #endif
 }
 
@@ -314,13 +333,43 @@ ChiakiControllerState Controller::GetState()
 	state.buttons |= SDL_GameControllerGetButton(controller, SDL_CONTROLLER_BUTTON_START) ? CHIAKI_CONTROLLER_BUTTON_OPTIONS : 0;
 	state.buttons |= SDL_GameControllerGetButton(controller, SDL_CONTROLLER_BUTTON_BACK) ? CHIAKI_CONTROLLER_BUTTON_SHARE : 0;
 	state.buttons |= SDL_GameControllerGetButton(controller, SDL_CONTROLLER_BUTTON_GUIDE) ? CHIAKI_CONTROLLER_BUTTON_PS : 0;
+	state.buttons |= SDL_GameControllerGetButton(controller, SDL_CONTROLLER_BUTTON_TOUCHPAD) ? CHIAKI_CONTROLLER_BUTTON_TOUCHPAD : 0;
 	state.l2_state = (uint8_t)(SDL_GameControllerGetAxis(controller, SDL_CONTROLLER_AXIS_TRIGGERLEFT) >> 7);
 	state.r2_state = (uint8_t)(SDL_GameControllerGetAxis(controller, SDL_CONTROLLER_AXIS_TRIGGERRIGHT) >> 7);
 	state.left_x = SDL_GameControllerGetAxis(controller, SDL_CONTROLLER_AXIS_LEFTX);
 	state.left_y = SDL_GameControllerGetAxis(controller, SDL_CONTROLLER_AXIS_LEFTY);
 	state.right_x = SDL_GameControllerGetAxis(controller, SDL_CONTROLLER_AXIS_RIGHTX);
 	state.right_y = SDL_GameControllerGetAxis(controller, SDL_CONTROLLER_AXIS_RIGHTY);
 
+	Uint8 touch_state;
+	float x, y, pressure;
+	SDL_GameControllerGetTouchpadFinger(controller, 0, 0, &touch_state, &x, &y, &pressure);
+
+	state.touches[0].x = x * 1920;
+	state.touches[0].y = y * 1080;
+	state.touches[0].id = touch_state - 1;
+
+	if(SDL_GameControllerHasSensor(controller, SDL_SENSOR_ACCEL) && SDL_GameControllerHasSensor(controller, SDL_SENSOR_GYRO))
+	{
+		float gyro_data[3], accel_data[3];
+		SDL_GameControllerGetSensorData(controller, SDL_SENSOR_GYRO, &gyro_data[0], 3);
+		SDL_GameControllerGetSensorData(controller, SDL_SENSOR_ACCEL, &accel_data[0], 3);
+
+		uint64_t microsec_since_epoch = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+
+		// Normalize accelerometer data
+		float recip_norm = inv_sqrt(accel_data[0] * accel_data[0] + accel_data[1] * accel_data[1] + accel_data[2] * accel_data[2]);
+		accel_data[0] *= recip_norm;
+		accel_data[1] *= recip_norm;
+		accel_data[2] *= recip_norm;
+
+		chiaki_orientation_tracker_update(&orient_tracker,
+			gyro_data[0], gyro_data[1], gyro_data[2],
+			accel_data[0], accel_data[1], accel_data[2],
+			microsec_since_epoch);
+
+		chiaki_orientation_tracker_apply_to_controller_state(&orient_tracker, &state);
+	}
 #endif
 	return state;
 }

gui/src/streamsession.cpp

@@ -384,8 +384,10 @@ void StreamSession::SendFeedbackState()
 
 	for(auto controller : controllers)
 	{
-		auto controller_state = controller->GetState();
-		chiaki_controller_state_or(&state, &state, &controller_state);
+		state = controller->GetState();
+		break;
+		// auto controller_state = controller->GetState();
+		// chiaki_controller_state_or(&state, &state, &controller_state);
 	}
 
 	chiaki_controller_state_or(&state, &state, &keyboard_state);

lib/include/chiaki/orientation.h

@@ -32,13 +32,13 @@ typedef struct chiaki_orientation_tracker_t
 	float gyro_x, gyro_y, gyro_z;
 	float accel_x, accel_y, accel_z;
 	ChiakiOrientation orient;
-	uint32_t timestamp;
+	uint64_t timestamp;
 	uint64_t sample_index;
 } ChiakiOrientationTracker;
 
 CHIAKI_EXPORT void chiaki_orientation_tracker_init(ChiakiOrientationTracker *tracker);
 CHIAKI_EXPORT void chiaki_orientation_tracker_update(ChiakiOrientationTracker *tracker,
-		float gx, float gy, float gz, float ax, float ay, float az, uint32_t timestamp_us);
+		float gx, float gy, float gz, float ax, float ay, float az, uint64_t timestamp_us);
 CHIAKI_EXPORT void chiaki_orientation_tracker_apply_to_controller_state(ChiakiOrientationTracker *tracker,
 		ChiakiControllerState *state);
 

lib/src/orientation.c

@@ -135,7 +135,7 @@ CHIAKI_EXPORT void chiaki_orientation_tracker_init(ChiakiOrientationTracker *tra
 }
 
 CHIAKI_EXPORT void chiaki_orientation_tracker_update(ChiakiOrientationTracker *tracker,
-		float gx, float gy, float gz, float ax, float ay, float az, uint32_t timestamp_us)
+		float gx, float gy, float gz, float ax, float ay, float az, uint64_t timestamp_us)
 {
 	tracker->gyro_x = gx;
 	tracker->gyro_y = gy;