core: hid: Restore motion state on refresh and clamp motion values

src/core/hid/emulated_controller.cpp

@@ -363,7 +363,17 @@ void EmulatedController::ReloadInput() {
                     SetMotion(callback, index);
                 },
         });
-        motion_devices[index]->ForceUpdate();
+
+        // Restore motion state
+        auto& emulated_motion = controller.motion_values[index].emulated;
+        auto& motion = controller.motion_state[index];
+        emulated_motion.ResetRotations();
+        emulated_motion.ResetQuaternion();
+        motion.accel = emulated_motion.GetAcceleration();
+        motion.gyro = emulated_motion.GetGyroscope();
+        motion.rotation = emulated_motion.GetRotations();
+        motion.orientation = emulated_motion.GetOrientation();
+        motion.is_at_rest = !emulated_motion.IsMoving(motion_sensitivity);
     }
 
     for (std::size_t index = 0; index < camera_devices.size(); ++index) {

src/core/hid/motion_input.cpp

@@ -10,6 +10,8 @@ MotionInput::MotionInput() {
     // Initialize PID constants with default values
     SetPID(0.3f, 0.005f, 0.0f);
     SetGyroThreshold(ThresholdStandard);
+    ResetQuaternion();
+    ResetRotations();
 }
 
 void MotionInput::SetPID(f32 new_kp, f32 new_ki, f32 new_kd) {
@@ -20,11 +22,19 @@ void MotionInput::SetPID(f32 new_kp, f32 new_ki, f32 new_kd) {
 
 void MotionInput::SetAcceleration(const Common::Vec3f& acceleration) {
     accel = acceleration;
+
+    accel.x = std::clamp(accel.x, -AccelMaxValue, AccelMaxValue);
+    accel.y = std::clamp(accel.y, -AccelMaxValue, AccelMaxValue);
+    accel.z = std::clamp(accel.z, -AccelMaxValue, AccelMaxValue);
 }
 
 void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) {
     gyro = gyroscope - gyro_bias;
 
+    gyro.x = std::clamp(gyro.x, -GyroMaxValue, GyroMaxValue);
+    gyro.y = std::clamp(gyro.y, -GyroMaxValue, GyroMaxValue);
+    gyro.z = std::clamp(gyro.z, -GyroMaxValue, GyroMaxValue);
+
     // Auto adjust drift to minimize drift
     if (!IsMoving(IsAtRestRelaxed)) {
         gyro_bias = (gyro_bias * 0.9999f) + (gyroscope * 0.0001f);
@@ -61,6 +71,10 @@ void MotionInput::ResetRotations() {
     rotations = {};
 }
 
+void MotionInput::ResetQuaternion() {
+    quat = {{0.0f, 0.0f, -1.0f}, 0.0f};
+}
+
 bool MotionInput::IsMoving(f32 sensitivity) const {
     return gyro.Length() >= sensitivity || accel.Length() <= 0.9f || accel.Length() >= 1.1f;
 }

src/core/hid/motion_input.h

@@ -20,6 +20,9 @@ public:
     static constexpr float IsAtRestStandard = 0.01f;
     static constexpr float IsAtRestThight = 0.005f;
 
+    static constexpr float GyroMaxValue = 5.0f;
+    static constexpr float AccelMaxValue = 7.0f;
+
     explicit MotionInput();
 
     MotionInput(const MotionInput&) = default;
@@ -40,6 +43,7 @@ public:
 
     void EnableReset(bool reset);
     void ResetRotations();
+    void ResetQuaternion();
 
     void UpdateRotation(u64 elapsed_time);
     void UpdateOrientation(u64 elapsed_time);
@@ -69,7 +73,7 @@ private:
     Common::Vec3f derivative_error;
 
     // Quaternion containing the device orientation
-    Common::Quaternion<f32> quat{{0.0f, 0.0f, -1.0f}, 0.0f};
+    Common::Quaternion<f32> quat;
 
     // Number of full rotations in each axis
     Common::Vec3f rotations;