Skip to content

Commit 3ef6220

Browse files
saturnskygerman77
saturnsky
and
german77
authored
modules/SceMotion: Improve motion support (#3441)
* modules/SceMotion: Improve motion support * motion: Set sensor limits and make use of deadband and tilt correction * motion: Stub set angle threshold * motion: improve basic_orientation calculation * motion: optimize gyro deadband and basic orientation calculations Co-authored-by: Narr the Reg <5944268+german77@users.noreply.github.com> --------- Co-authored-by: Narr the Reg <5944268+german77@users.noreply.github.com>
1 parent 7c04fd2 commit 3ef6220

5 files changed

Lines changed: 214 additions & 49 deletions

File tree

‎vita3k/modules/SceDriverUser/SceMotion.cpp‎

Lines changed: 23 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -24,9 +24,9 @@
2424
#include <util/tracy.h>
2525
TRACY_MODULE_NAME(SceMotion);
2626

27-
EXPORT(int, sceMotionGetAngleThreshold) {
27+
EXPORT(SceFloat, sceMotionGetAngleThreshold) {
2828
TRACY_FUNC(sceMotionGetAngleThreshold);
29-
return UNIMPLEMENTED();
29+
return get_angle_threshold(emuenv.motion);
3030
}
3131

3232
EXPORT(int, sceMotionGetBasicOrientation, SceFVector3 *basicOrientation) {
@@ -35,26 +35,16 @@ EXPORT(int, sceMotionGetBasicOrientation, SceFVector3 *basicOrientation) {
3535
return RET_ERROR(SCE_MOTION_ERROR_NULL_PARAMETER);
3636

3737
std::lock_guard<std::mutex> guard(emuenv.motion.mutex);
38-
Util::Quaternion quat = get_orientation(emuenv.motion);
39-
40-
*basicOrientation = { 0.f, 0.f, 0.f };
41-
// get the basic orientation, only one component is not zero and will be 1 or -1
42-
// TODO: this is probably wrong
43-
float max_val = std::max({ std::abs(quat.xyz.x), std::abs(quat.xyz.y), std::abs(quat.xyz.z) });
44-
if (max_val == std::abs(quat.xyz.x)) {
45-
basicOrientation->x = quat.xyz.x > 0.0f ? 1.0f : -1.0f;
46-
} else if (max_val == std::abs(quat.xyz.y)) {
47-
basicOrientation->y = quat.xyz.y > 0.0f ? 1.0f : -1.0f;
48-
} else {
49-
basicOrientation->z = quat.xyz.z > 0.0f ? 1.0f : -1.0f;
50-
}
38+
SceFVector3 accelerometer = get_acceleration(emuenv.motion);
39+
40+
*basicOrientation = get_basic_orientation(emuenv.motion);
5141

5242
return 0;
5343
}
5444

5545
EXPORT(SceBool, sceMotionGetDeadband) {
5646
TRACY_FUNC(sceMotionGetDeadband);
57-
return UNIMPLEMENTED();
47+
return get_deadband(emuenv.motion);
5848
}
5949

6050
EXPORT(int, sceMotionGetDeadbandExt) {
@@ -123,6 +113,7 @@ EXPORT(int, sceMotionGetState, SceMotionState *motionState) {
123113
motionState->angularVelocity = get_gyroscope(emuenv.motion);
124114

125115
Util::Quaternion dev_quat = get_orientation(emuenv.motion);
116+
motionState->basicOrientation = get_basic_orientation(emuenv.motion);
126117

127118
static_assert(sizeof(motionState->deviceQuat) == sizeof(dev_quat));
128119
memcpy(&motionState->deviceQuat, &dev_quat, sizeof(motionState->deviceQuat));
@@ -168,7 +159,7 @@ EXPORT(int, sceMotionGetStateInternal) {
168159

169160
EXPORT(SceBool, sceMotionGetTiltCorrection) {
170161
TRACY_FUNC(sceMotionGetTiltCorrection);
171-
return UNIMPLEMENTED();
162+
return get_tilt_correction(emuenv.motion);
172163
}
173164

174165
EXPORT(int, sceMotionGetTiltCorrectionExt) {
@@ -193,7 +184,9 @@ EXPORT(int, sceMotionMagnetometerOn) {
193184

194185
EXPORT(int, sceMotionReset) {
195186
TRACY_FUNC(sceMotionReset);
196-
return UNIMPLEMENTED();
187+
std::lock_guard<std::mutex> guard(emuenv.motion.mutex);
188+
emuenv.motion.motion_data.SetQuaternion({ { 0.0f, 0.0f, -1.0f }, 0.0f });
189+
return 0;
197190
}
198191

199192
EXPORT(int, sceMotionResetExt) {
@@ -203,17 +196,22 @@ EXPORT(int, sceMotionResetExt) {
203196

204197
EXPORT(int, sceMotionRotateYaw, const float radians) {
205198
TRACY_FUNC(sceMotionRotateYaw, radians);
206-
return UNIMPLEMENTED();
199+
emuenv.motion.motion_data.RotateYaw(radians);
200+
return 0;
207201
}
208202

209-
EXPORT(int, sceMotionSetAngleThreshold, const float angle) {
203+
EXPORT(int, sceMotionSetAngleThreshold, SceFloat angle) {
210204
TRACY_FUNC(sceMotionSetAngleThreshold, angle);
211-
return UNIMPLEMENTED();
205+
set_angle_threshold(emuenv.motion, angle);
206+
return 0;
212207
}
213208

214209
EXPORT(int, sceMotionSetDeadband, SceBool setValue) {
215210
TRACY_FUNC(sceMotionSetDeadband, setValue);
216-
return UNIMPLEMENTED();
211+
STUBBED("only set value");
212+
set_deadband(emuenv.motion, setValue);
213+
214+
return 0;
217215
}
218216

219217
EXPORT(int, sceMotionSetDeadbandExt) {
@@ -230,7 +228,9 @@ EXPORT(int, sceMotionSetGyroBiasCorrection, SceBool setValue) {
230228

231229
EXPORT(int, sceMotionSetTiltCorrection, SceBool setValue) {
232230
TRACY_FUNC(sceMotionSetTiltCorrection, setValue);
233-
return UNIMPLEMENTED();
231+
STUBBED("only set value");
232+
set_tilt_correction(emuenv.motion, setValue);
233+
return 0;
234234
}
235235

236236
EXPORT(int, sceMotionSetTiltCorrectionExt) {

‎vita3k/motion/include/motion/functions.h‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,5 +25,12 @@ SceFVector3 get_gyroscope(const MotionState &state);
2525
Util::Quaternion<SceFloat> get_orientation(const MotionState &state);
2626
SceBool get_gyro_bias_correction(const MotionState &state);
2727
void set_gyro_bias_correction(MotionState &state, SceBool setValue);
28+
SceBool get_tilt_correction(MotionState &state);
29+
void set_tilt_correction(MotionState &state, SceBool setValue);
30+
SceBool get_deadband(MotionState &state);
31+
void set_deadband(MotionState &state, SceBool setValue);
32+
SceFloat get_angle_threshold(const MotionState &state);
33+
void set_angle_threshold(MotionState &state, SceFloat setValue);
34+
SceFVector3 get_basic_orientation(const MotionState &state);
2835

2936
void refresh_motion(MotionState &state, CtrlState &ctrl_state);

‎vita3k/motion/include/motion/motion_input.h‎

Lines changed: 36 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,9 @@
1010

1111
class MotionInput {
1212
public:
13+
static constexpr float GyroMaxValue = 5.0f;
14+
static constexpr float AccelMaxValue = 7.0f;
15+
1316
explicit MotionInput();
1417

1518
MotionInput(const MotionInput &) = default;
@@ -22,23 +25,33 @@ class MotionInput {
2225
void SetAcceleration(const Util::Vec3f &acceleration);
2326
void SetGyroscope(const Util::Vec3f &gyroscope);
2427
void SetQuaternion(const Util::Quaternion<SceFloat> &quaternion);
25-
void SetGyroThreshold(SceFloat threshold);
28+
void SetDeadband(SceFloat threshold);
29+
void SetAngleThreshold(SceFloat threshold);
30+
void RotateYaw(SceFloat radians);
2631

2732
void EnableGyroBias(bool enable);
33+
void EnableTiltCorrection(bool enable);
34+
void EnableDeadband(bool enable);
2835
void EnableReset(bool reset);
2936
void ResetRotations();
37+
void ResetQuaternion();
3038

3139
void UpdateRotation(SceULong64 elapsed_time);
3240
void UpdateOrientation(SceULong64 elapsed_time);
41+
void UpdateBasicOrientation();
3342

3443
[[nodiscard]] Util::Quaternion<SceFloat> GetOrientation() const;
3544
[[nodiscard]] Util::Vec3f GetAcceleration() const;
3645
[[nodiscard]] Util::Vec3f GetGyroscope() const;
3746
[[nodiscard]] Util::Vec3f GetRotations() const;
47+
[[nodiscard]] SceFVector3 GetBasicOrientation() const;
48+
[[nodiscard]] SceFloat GetAngleThreshold() const;
3849

3950
[[nodiscard]] bool IsMoving(SceFloat sensitivity) const;
4051
[[nodiscard]] bool IsCalibrated(SceFloat sensitivity) const;
4152
SceBool IsGyroBiasEnabled() const;
53+
SceBool IsTiltCorrectionEnabled() const;
54+
SceBool IsDeadbandEnabled() const;
4255

4356
private:
4457
void ResetOrientation();
@@ -55,22 +68,32 @@ class MotionInput {
5568
Util::Vec3f derivative_error;
5669

5770
// Quaternion containing the device orientation
58-
Util::Quaternion<SceFloat> quat{ { 0.0f, 0.0f, 1.0f }, 0.0f };
71+
Util::Quaternion<SceFloat> quat{ { 0.0f, 0.0f, -1.0f }, 0.0f };
5972

6073
// Number of full rotations in each axis
61-
Util::Vec3f rotations;
74+
Util::Vec3f rotations{};
6275

6376
// Acceleration vector measurement in G force
64-
Util::Vec3f accel;
77+
Util::Vec3f accel{};
6578

6679
// Gyroscope vector measurement in radians/s.
67-
Util::Vec3f gyro;
80+
Util::Vec3f gyro{};
6881

6982
// Vector to be subtracted from gyro measurements
70-
Util::Vec3f gyro_bias;
83+
Util::Vec3f gyro_bias{};
7184

7285
// Minimum gyro amplitude to detect if the device is moving
73-
SceFloat gyro_threshold = 0.0f;
86+
SceFloat gyro_deadband = 0.0f;
87+
SceFloat gyro_deadband2 = 0.0f;
88+
89+
// Minimum angle which basic motion orientation changes value
90+
SceFloat angle_threshold = 20.0f;
91+
92+
// Current basic orientation of the device
93+
SceFVector3 basic_orientation = {};
94+
95+
// It seems that the basic orientation is determined by gravity
96+
SceFVector3 basic_orientation_base = {};
7497

7598
// Number of invalid sequential data
7699
SceFloat reset_counter = 0;
@@ -81,6 +104,12 @@ class MotionInput {
81104
// If the provided gyro uses bias correction
82105
bool bias_enabled = true;
83106

107+
// If the provided gyro uses gyro deadband
108+
bool deadband_enabled = true;
109+
110+
// Corrects tilt based on accelerometer measurements
111+
bool tilt_correction_enabled = true;
112+
84113
// Use accelerometer values to calculate position
85114
bool only_accelerometer = true;
86115
};

‎vita3k/motion/src/motion.cpp‎

Lines changed: 30 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ SceFVector3 get_gyroscope(const MotionState &state) {
4444
Util::Quaternion<SceFloat> get_orientation(const MotionState &state) {
4545
auto quat = state.motion_data.GetOrientation();
4646
return {
47-
{ quat.xyz[1], quat.xyz[0], -quat.w },
47+
{ -quat.xyz[1], -quat.w, quat.xyz[0] },
4848
-quat.xyz[2],
4949
};
5050
}
@@ -57,6 +57,34 @@ void set_gyro_bias_correction(MotionState &state, SceBool setValue) {
5757
state.motion_data.EnableGyroBias(setValue);
5858
}
5959

60+
SceBool get_tilt_correction(MotionState &state) {
61+
return state.motion_data.IsTiltCorrectionEnabled();
62+
}
63+
64+
void set_tilt_correction(MotionState &state, SceBool setValue) {
65+
state.motion_data.EnableTiltCorrection(setValue);
66+
}
67+
68+
SceBool get_deadband(MotionState &state) {
69+
return state.motion_data.IsDeadbandEnabled();
70+
}
71+
72+
void set_deadband(MotionState &state, SceBool setValue) {
73+
state.motion_data.EnableDeadband(setValue);
74+
}
75+
76+
SceFloat get_angle_threshold(const MotionState &state) {
77+
return state.motion_data.GetAngleThreshold();
78+
}
79+
80+
void set_angle_threshold(MotionState &state, SceFloat setValue) {
81+
state.motion_data.SetAngleThreshold(setValue);
82+
}
83+
84+
SceFVector3 get_basic_orientation(const MotionState &state) {
85+
return state.motion_data.GetBasicOrientation();
86+
}
87+
6088
void refresh_motion(MotionState &state, CtrlState &ctrl_state) {
6189
if (!state.is_sampling)
6290
return;
@@ -128,6 +156,7 @@ void refresh_motion(MotionState &state, CtrlState &ctrl_state) {
128156

129157
state.motion_data.UpdateRotation(gyro_timestamp - state.last_gyro_timestamp);
130158
state.motion_data.UpdateOrientation(accel_timestamp - state.last_accel_timestamp);
159+
state.motion_data.UpdateBasicOrientation();
131160

132161
state.last_gyro_timestamp = gyro_timestamp;
133162
state.last_accel_timestamp = accel_timestamp;

0 commit comments

Comments
 (0)