diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/core/CMakeLists.txt | 5 | ||||
| -rw-r--r-- | src/core/hid/hid_types.h | 388 | ||||
| -rw-r--r-- | src/core/hid/input_converter.cpp | 345 | ||||
| -rw-r--r-- | src/core/hid/input_converter.h | 77 | ||||
| -rw-r--r-- | src/core/hid/motion_input.cpp | 278 | ||||
| -rw-r--r-- | src/core/hid/motion_input.h | 71 |
6 files changed, 1164 insertions, 0 deletions
diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index 14ba986d3..eddf455c0 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt | |||
| @@ -135,8 +135,13 @@ add_library(core STATIC | |||
| 135 | frontend/input.h | 135 | frontend/input.h |
| 136 | hardware_interrupt_manager.cpp | 136 | hardware_interrupt_manager.cpp |
| 137 | hardware_interrupt_manager.h | 137 | hardware_interrupt_manager.h |
| 138 | hid/hid_types.h | ||
| 139 | hid/input_converter.cpp | ||
| 140 | hid/input_converter.h | ||
| 138 | hid/input_interpreter.cpp | 141 | hid/input_interpreter.cpp |
| 139 | hid/input_interpreter.h | 142 | hid/input_interpreter.h |
| 143 | hid/motion_input.cpp | ||
| 144 | hid/motion_input.h | ||
| 140 | hle/api_version.h | 145 | hle/api_version.h |
| 141 | hle/ipc.h | 146 | hle/ipc.h |
| 142 | hle/ipc_helpers.h | 147 | hle/ipc_helpers.h |
diff --git a/src/core/hid/hid_types.h b/src/core/hid/hid_types.h new file mode 100644 index 000000000..d3f7930c9 --- /dev/null +++ b/src/core/hid/hid_types.h | |||
| @@ -0,0 +1,388 @@ | |||
| 1 | // Copyright 2021 yuzu Emulator Project | ||
| 2 | // Licensed under GPLv2 or any later version | ||
| 3 | // Refer to the license.txt file included. | ||
| 4 | |||
| 5 | #pragma once | ||
| 6 | |||
| 7 | #include "common/bit_field.h" | ||
| 8 | #include "common/common_funcs.h" | ||
| 9 | #include "common/common_types.h" | ||
| 10 | #include "common/point.h" | ||
| 11 | #include "common/uuid.h" | ||
| 12 | |||
| 13 | namespace Core::HID { | ||
| 14 | |||
| 15 | // This is nn::hid::NpadIdType | ||
| 16 | enum class NpadIdType : u8 { | ||
| 17 | Player1 = 0x0, | ||
| 18 | Player2 = 0x1, | ||
| 19 | Player3 = 0x2, | ||
| 20 | Player4 = 0x3, | ||
| 21 | Player5 = 0x4, | ||
| 22 | Player6 = 0x5, | ||
| 23 | Player7 = 0x6, | ||
| 24 | Player8 = 0x7, | ||
| 25 | Other = 0x10, | ||
| 26 | Handheld = 0x20, | ||
| 27 | |||
| 28 | Invalid = 0xFF, | ||
| 29 | }; | ||
| 30 | |||
| 31 | /// Converts a NpadIdType to an array index. | ||
| 32 | constexpr size_t NpadIdTypeToIndex(NpadIdType npad_id_type) { | ||
| 33 | switch (npad_id_type) { | ||
| 34 | case NpadIdType::Player1: | ||
| 35 | return 0; | ||
| 36 | case NpadIdType::Player2: | ||
| 37 | return 1; | ||
| 38 | case NpadIdType::Player3: | ||
| 39 | return 2; | ||
| 40 | case NpadIdType::Player4: | ||
| 41 | return 3; | ||
| 42 | case NpadIdType::Player5: | ||
| 43 | return 4; | ||
| 44 | case NpadIdType::Player6: | ||
| 45 | return 5; | ||
| 46 | case NpadIdType::Player7: | ||
| 47 | return 6; | ||
| 48 | case NpadIdType::Player8: | ||
| 49 | return 7; | ||
| 50 | case NpadIdType::Other: | ||
| 51 | return 8; | ||
| 52 | case NpadIdType::Handheld: | ||
| 53 | return 9; | ||
| 54 | default: | ||
| 55 | return 0; | ||
| 56 | } | ||
| 57 | } | ||
| 58 | |||
| 59 | /// Converts an array index to a NpadIdType | ||
| 60 | constexpr NpadIdType IndexToNpadIdType(size_t index) { | ||
| 61 | switch (index) { | ||
| 62 | case 0: | ||
| 63 | return NpadIdType::Player1; | ||
| 64 | case 1: | ||
| 65 | return NpadIdType::Player2; | ||
| 66 | case 2: | ||
| 67 | return NpadIdType::Player3; | ||
| 68 | case 3: | ||
| 69 | return NpadIdType::Player4; | ||
| 70 | case 4: | ||
| 71 | return NpadIdType::Player5; | ||
| 72 | case 5: | ||
| 73 | return NpadIdType::Player6; | ||
| 74 | case 6: | ||
| 75 | return NpadIdType::Player7; | ||
| 76 | case 7: | ||
| 77 | return NpadIdType::Player8; | ||
| 78 | case 8: | ||
| 79 | return NpadIdType::Other; | ||
| 80 | case 9: | ||
| 81 | return NpadIdType::Handheld; | ||
| 82 | default: | ||
| 83 | return NpadIdType::Invalid; | ||
| 84 | } | ||
| 85 | } | ||
| 86 | |||
| 87 | // This is nn::hid::NpadType | ||
| 88 | enum class NpadType : u8 { | ||
| 89 | None = 0, | ||
| 90 | ProController = 3, | ||
| 91 | Handheld = 4, | ||
| 92 | JoyconDual = 5, | ||
| 93 | JoyconLeft = 6, | ||
| 94 | JoyconRight = 7, | ||
| 95 | GameCube = 8, | ||
| 96 | Pokeball = 9, | ||
| 97 | MaxNpadType = 10, | ||
| 98 | }; | ||
| 99 | |||
| 100 | // This is nn::hid::NpadStyleTag | ||
| 101 | struct NpadStyleTag { | ||
| 102 | union { | ||
| 103 | u32_le raw{}; | ||
| 104 | |||
| 105 | BitField<0, 1, u32> fullkey; | ||
| 106 | BitField<1, 1, u32> handheld; | ||
| 107 | BitField<2, 1, u32> joycon_dual; | ||
| 108 | BitField<3, 1, u32> joycon_left; | ||
| 109 | BitField<4, 1, u32> joycon_right; | ||
| 110 | BitField<5, 1, u32> gamecube; | ||
| 111 | BitField<6, 1, u32> palma; | ||
| 112 | BitField<7, 1, u32> lark; | ||
| 113 | BitField<8, 1, u32> handheld_lark; | ||
| 114 | BitField<9, 1, u32> lucia; | ||
| 115 | BitField<29, 1, u32> system_ext; | ||
| 116 | BitField<30, 1, u32> system; | ||
| 117 | }; | ||
| 118 | }; | ||
| 119 | static_assert(sizeof(NpadStyleTag) == 4, "NpadStyleTag is an invalid size"); | ||
| 120 | |||
| 121 | // This is nn::hid::TouchAttribute | ||
| 122 | struct TouchAttribute { | ||
| 123 | union { | ||
| 124 | u32 raw{}; | ||
| 125 | BitField<0, 1, u32> start_touch; | ||
| 126 | BitField<1, 1, u32> end_touch; | ||
| 127 | }; | ||
| 128 | }; | ||
| 129 | static_assert(sizeof(TouchAttribute) == 0x4, "TouchAttribute is an invalid size"); | ||
| 130 | |||
| 131 | // This is nn::hid::TouchState | ||
| 132 | struct TouchState { | ||
| 133 | u64_le delta_time; | ||
| 134 | TouchAttribute attribute; | ||
| 135 | u32_le finger; | ||
| 136 | Common::Point<u32_le> position; | ||
| 137 | u32_le diameter_x; | ||
| 138 | u32_le diameter_y; | ||
| 139 | u32_le rotation_angle; | ||
| 140 | }; | ||
| 141 | static_assert(sizeof(TouchState) == 0x28, "Touchstate is an invalid size"); | ||
| 142 | |||
| 143 | // This is nn::hid::NpadControllerColor | ||
| 144 | struct NpadControllerColor { | ||
| 145 | u32_le body; | ||
| 146 | u32_le button; | ||
| 147 | }; | ||
| 148 | static_assert(sizeof(NpadControllerColor) == 8, "NpadControllerColor is an invalid size"); | ||
| 149 | |||
| 150 | // This is nn::hid::AnalogStickState | ||
| 151 | struct AnalogStickState { | ||
| 152 | s32_le x; | ||
| 153 | s32_le y; | ||
| 154 | }; | ||
| 155 | static_assert(sizeof(AnalogStickState) == 8, "AnalogStickState is an invalid size"); | ||
| 156 | |||
| 157 | // This is nn::hid::server::NpadGcTriggerState | ||
| 158 | struct NpadGcTriggerState { | ||
| 159 | s64_le sampling_number{}; | ||
| 160 | s32_le left{}; | ||
| 161 | s32_le right{}; | ||
| 162 | }; | ||
| 163 | static_assert(sizeof(NpadGcTriggerState) == 0x10, "NpadGcTriggerState is an invalid size"); | ||
| 164 | |||
| 165 | // This is nn::hid::system::NpadBatteryLevel | ||
| 166 | using BatteryLevel = u32; | ||
| 167 | static_assert(sizeof(BatteryLevel) == 0x4, "BatteryLevel is an invalid size"); | ||
| 168 | |||
| 169 | // This is nn::hid::system::NpadPowerInfo | ||
| 170 | struct NpadPowerInfo { | ||
| 171 | bool is_powered; | ||
| 172 | bool is_charging; | ||
| 173 | INSERT_PADDING_BYTES(0x6); | ||
| 174 | BatteryLevel battery_level; | ||
| 175 | }; | ||
| 176 | static_assert(sizeof(NpadPowerInfo) == 0xC, "NpadPowerInfo is an invalid size"); | ||
| 177 | |||
| 178 | // This is nn::hid::NpadButton | ||
| 179 | enum class NpadButton : u64 { | ||
| 180 | None = 0, | ||
| 181 | A = 1U << 0, | ||
| 182 | B = 1U << 1, | ||
| 183 | X = 1U << 2, | ||
| 184 | Y = 1U << 3, | ||
| 185 | StickL = 1U << 4, | ||
| 186 | StickR = 1U << 5, | ||
| 187 | L = 1U << 6, | ||
| 188 | R = 1U << 7, | ||
| 189 | ZL = 1U << 8, | ||
| 190 | ZR = 1U << 9, | ||
| 191 | Plus = 1U << 10, | ||
| 192 | Minus = 1U << 11, | ||
| 193 | |||
| 194 | Left = 1U << 12, | ||
| 195 | Up = 1U << 13, | ||
| 196 | Right = 1U << 14, | ||
| 197 | Down = 1U << 15, | ||
| 198 | |||
| 199 | StickLLeft = 1U << 16, | ||
| 200 | StickLUp = 1U << 17, | ||
| 201 | StickLRight = 1U << 18, | ||
| 202 | StickLDown = 1U << 19, | ||
| 203 | |||
| 204 | StickRLeft = 1U << 20, | ||
| 205 | StickRUp = 1U << 21, | ||
| 206 | StickRRight = 1U << 22, | ||
| 207 | StickRDown = 1U << 23, | ||
| 208 | |||
| 209 | LeftSL = 1U << 24, | ||
| 210 | LeftSR = 1U << 25, | ||
| 211 | |||
| 212 | RightSL = 1U << 26, | ||
| 213 | RightSR = 1U << 27, | ||
| 214 | |||
| 215 | Palma = 1U << 28, | ||
| 216 | HandheldLeftB = 1U << 30, | ||
| 217 | }; | ||
| 218 | DECLARE_ENUM_FLAG_OPERATORS(NpadButton); | ||
| 219 | |||
| 220 | struct NpadButtonState { | ||
| 221 | union { | ||
| 222 | NpadButton raw{}; | ||
| 223 | |||
| 224 | // Buttons | ||
| 225 | BitField<0, 1, u64> a; | ||
| 226 | BitField<1, 1, u64> b; | ||
| 227 | BitField<2, 1, u64> x; | ||
| 228 | BitField<3, 1, u64> y; | ||
| 229 | BitField<4, 1, u64> stick_l; | ||
| 230 | BitField<5, 1, u64> stick_r; | ||
| 231 | BitField<6, 1, u64> l; | ||
| 232 | BitField<7, 1, u64> r; | ||
| 233 | BitField<8, 1, u64> zl; | ||
| 234 | BitField<9, 1, u64> zr; | ||
| 235 | BitField<10, 1, u64> plus; | ||
| 236 | BitField<11, 1, u64> minus; | ||
| 237 | |||
| 238 | // D-Pad | ||
| 239 | BitField<12, 1, u64> left; | ||
| 240 | BitField<13, 1, u64> up; | ||
| 241 | BitField<14, 1, u64> right; | ||
| 242 | BitField<15, 1, u64> down; | ||
| 243 | |||
| 244 | // Left JoyStick | ||
| 245 | BitField<16, 1, u64> stick_l_left; | ||
| 246 | BitField<17, 1, u64> stick_l_up; | ||
| 247 | BitField<18, 1, u64> stick_l_right; | ||
| 248 | BitField<19, 1, u64> stick_l_down; | ||
| 249 | |||
| 250 | // Right JoyStick | ||
| 251 | BitField<20, 1, u64> stick_r_left; | ||
| 252 | BitField<21, 1, u64> stick_r_up; | ||
| 253 | BitField<22, 1, u64> stick_r_right; | ||
| 254 | BitField<23, 1, u64> stick_r_down; | ||
| 255 | |||
| 256 | BitField<24, 1, u64> left_sl; | ||
| 257 | BitField<25, 1, u64> left_sr; | ||
| 258 | |||
| 259 | BitField<26, 1, u64> right_sl; | ||
| 260 | BitField<27, 1, u64> right_sr; | ||
| 261 | |||
| 262 | BitField<28, 1, u64> palma; | ||
| 263 | BitField<30, 1, u64> handheld_left_b; | ||
| 264 | }; | ||
| 265 | }; | ||
| 266 | static_assert(sizeof(NpadButtonState) == 0x8, "NpadButtonState has incorrect size."); | ||
| 267 | |||
| 268 | // This is nn::hid::DebugPadButton | ||
| 269 | struct DebugPadButton { | ||
| 270 | union { | ||
| 271 | u32_le raw{}; | ||
| 272 | BitField<0, 1, u32> a; | ||
| 273 | BitField<1, 1, u32> b; | ||
| 274 | BitField<2, 1, u32> x; | ||
| 275 | BitField<3, 1, u32> y; | ||
| 276 | BitField<4, 1, u32> l; | ||
| 277 | BitField<5, 1, u32> r; | ||
| 278 | BitField<6, 1, u32> zl; | ||
| 279 | BitField<7, 1, u32> zr; | ||
| 280 | BitField<8, 1, u32> plus; | ||
| 281 | BitField<9, 1, u32> minus; | ||
| 282 | BitField<10, 1, u32> d_left; | ||
| 283 | BitField<11, 1, u32> d_up; | ||
| 284 | BitField<12, 1, u32> d_right; | ||
| 285 | BitField<13, 1, u32> d_down; | ||
| 286 | }; | ||
| 287 | }; | ||
| 288 | static_assert(sizeof(DebugPadButton) == 0x4, "DebugPadButton is an invalid size"); | ||
| 289 | |||
| 290 | // This is nn::hid::VibrationDeviceType | ||
| 291 | enum class VibrationDeviceType : u32 { | ||
| 292 | Unknown = 0, | ||
| 293 | LinearResonantActuator = 1, | ||
| 294 | GcErm = 2, | ||
| 295 | }; | ||
| 296 | |||
| 297 | // This is nn::hid::VibrationDevicePosition | ||
| 298 | enum class VibrationDevicePosition : u32 { | ||
| 299 | None = 0, | ||
| 300 | Left = 1, | ||
| 301 | Right = 2, | ||
| 302 | }; | ||
| 303 | |||
| 304 | // This is nn::hid::VibrationValue | ||
| 305 | struct VibrationValue { | ||
| 306 | f32 low_amplitude; | ||
| 307 | f32 low_frequency; | ||
| 308 | f32 high_amplitude; | ||
| 309 | f32 high_frequency; | ||
| 310 | }; | ||
| 311 | static_assert(sizeof(VibrationValue) == 0x10, "VibrationValue has incorrect size."); | ||
| 312 | |||
| 313 | // This is nn::hid::VibrationGcErmCommand | ||
| 314 | enum class VibrationGcErmCommand : u64 { | ||
| 315 | Stop = 0, | ||
| 316 | Start = 1, | ||
| 317 | StopHard = 2, | ||
| 318 | }; | ||
| 319 | |||
| 320 | // This is nn::hid::VibrationDeviceInfo | ||
| 321 | struct VibrationDeviceInfo { | ||
| 322 | VibrationDeviceType type{}; | ||
| 323 | VibrationDevicePosition position{}; | ||
| 324 | }; | ||
| 325 | static_assert(sizeof(VibrationDeviceInfo) == 0x8, "VibrationDeviceInfo has incorrect size."); | ||
| 326 | |||
| 327 | // This is nn::hid::KeyboardModifier | ||
| 328 | struct KeyboardModifier { | ||
| 329 | union { | ||
| 330 | u32_le raw{}; | ||
| 331 | BitField<0, 1, u32> control; | ||
| 332 | BitField<1, 1, u32> shift; | ||
| 333 | BitField<2, 1, u32> left_alt; | ||
| 334 | BitField<3, 1, u32> right_alt; | ||
| 335 | BitField<4, 1, u32> gui; | ||
| 336 | BitField<8, 1, u32> caps_lock; | ||
| 337 | BitField<9, 1, u32> scroll_lock; | ||
| 338 | BitField<10, 1, u32> num_lock; | ||
| 339 | BitField<11, 1, u32> katakana; | ||
| 340 | BitField<12, 1, u32> hiragana; | ||
| 341 | }; | ||
| 342 | }; | ||
| 343 | static_assert(sizeof(KeyboardModifier) == 0x4, "KeyboardModifier is an invalid size"); | ||
| 344 | |||
| 345 | // This is nn::hid::KeyboardKey | ||
| 346 | struct KeyboardKey { | ||
| 347 | // This should be a 256 bit flag | ||
| 348 | std::array<u8, 32> key; | ||
| 349 | }; | ||
| 350 | static_assert(sizeof(KeyboardKey) == 0x20, "KeyboardKey is an invalid size"); | ||
| 351 | |||
| 352 | // This is nn::hid::MouseButton | ||
| 353 | struct MouseButton { | ||
| 354 | union { | ||
| 355 | u32_le raw{}; | ||
| 356 | BitField<0, 1, u32> left; | ||
| 357 | BitField<1, 1, u32> right; | ||
| 358 | BitField<2, 1, u32> middle; | ||
| 359 | BitField<3, 1, u32> forward; | ||
| 360 | BitField<4, 1, u32> back; | ||
| 361 | }; | ||
| 362 | }; | ||
| 363 | static_assert(sizeof(MouseButton) == 0x4, "MouseButton is an invalid size"); | ||
| 364 | |||
| 365 | // This is nn::hid::MouseAttribute | ||
| 366 | struct MouseAttribute { | ||
| 367 | union { | ||
| 368 | u32_le raw{}; | ||
| 369 | BitField<0, 1, u32> transferable; | ||
| 370 | BitField<1, 1, u32> is_connected; | ||
| 371 | }; | ||
| 372 | }; | ||
| 373 | static_assert(sizeof(MouseAttribute) == 0x4, "MouseAttribute is an invalid size"); | ||
| 374 | |||
| 375 | // This is nn::hid::detail::MouseState | ||
| 376 | struct MouseState { | ||
| 377 | s64_le sampling_number; | ||
| 378 | s32_le x; | ||
| 379 | s32_le y; | ||
| 380 | s32_le delta_x; | ||
| 381 | s32_le delta_y; | ||
| 382 | s32_le delta_wheel_x; | ||
| 383 | s32_le delta_wheel_y; | ||
| 384 | MouseButton button; | ||
| 385 | MouseAttribute attribute; | ||
| 386 | }; | ||
| 387 | static_assert(sizeof(MouseState) == 0x28, "MouseState is an invalid size"); | ||
| 388 | } // namespace Core::HID | ||
diff --git a/src/core/hid/input_converter.cpp b/src/core/hid/input_converter.cpp new file mode 100644 index 000000000..5834622e9 --- /dev/null +++ b/src/core/hid/input_converter.cpp | |||
| @@ -0,0 +1,345 @@ | |||
| 1 | // Copyright 2021 yuzu Emulator Project | ||
| 2 | // Licensed under GPLv2 or any later version | ||
| 3 | // Refer to the license.txt file included | ||
| 4 | |||
| 5 | #include <random> | ||
| 6 | |||
| 7 | #include "common/input.h" | ||
| 8 | #include "core/hid/input_converter.h" | ||
| 9 | |||
| 10 | namespace Core::HID { | ||
| 11 | |||
| 12 | Input::BatteryStatus TransformToBattery(const Input::CallbackStatus& callback) { | ||
| 13 | Input::BatteryStatus battery{}; | ||
| 14 | switch (callback.type) { | ||
| 15 | case Input::InputType::Analog: | ||
| 16 | case Input::InputType::Trigger: { | ||
| 17 | const auto value = TransformToTrigger(callback).analog.value; | ||
| 18 | battery = Input::BatteryLevel::Empty; | ||
| 19 | if (value > 0.2f) { | ||
| 20 | battery = Input::BatteryLevel::Critical; | ||
| 21 | } | ||
| 22 | if (value > 0.4f) { | ||
| 23 | battery = Input::BatteryLevel::Low; | ||
| 24 | } | ||
| 25 | if (value > 0.6f) { | ||
| 26 | battery = Input::BatteryLevel::Medium; | ||
| 27 | } | ||
| 28 | if (value > 0.8f) { | ||
| 29 | battery = Input::BatteryLevel::Full; | ||
| 30 | } | ||
| 31 | if (value >= 1.0f) { | ||
| 32 | battery = Input::BatteryLevel::Charging; | ||
| 33 | } | ||
| 34 | break; | ||
| 35 | } | ||
| 36 | case Input::InputType::Battery: | ||
| 37 | battery = callback.battery_status; | ||
| 38 | break; | ||
| 39 | default: | ||
| 40 | LOG_ERROR(Input, "Conversion from type {} to battery not implemented", callback.type); | ||
| 41 | break; | ||
| 42 | } | ||
| 43 | |||
| 44 | return battery; | ||
| 45 | } | ||
| 46 | |||
| 47 | Input::ButtonStatus TransformToButton(const Input::CallbackStatus& callback) { | ||
| 48 | Input::ButtonStatus status{}; | ||
| 49 | switch (callback.type) { | ||
| 50 | case Input::InputType::Analog: | ||
| 51 | case Input::InputType::Trigger: | ||
| 52 | status.value = TransformToTrigger(callback).pressed; | ||
| 53 | break; | ||
| 54 | case Input::InputType::Button: | ||
| 55 | status = callback.button_status; | ||
| 56 | break; | ||
| 57 | default: | ||
| 58 | LOG_ERROR(Input, "Conversion from type {} to button not implemented", callback.type); | ||
| 59 | break; | ||
| 60 | } | ||
| 61 | |||
| 62 | if (status.inverted) { | ||
| 63 | status.value = !status.value; | ||
| 64 | } | ||
| 65 | |||
| 66 | return status; | ||
| 67 | } | ||
| 68 | |||
| 69 | Input::MotionStatus TransformToMotion(const Input::CallbackStatus& callback) { | ||
| 70 | Input::MotionStatus status{}; | ||
| 71 | switch (callback.type) { | ||
| 72 | case Input::InputType::Button: { | ||
| 73 | if (TransformToButton(callback).value) { | ||
| 74 | std::random_device device; | ||
| 75 | std::mt19937 gen(device()); | ||
| 76 | std::uniform_int_distribution<s16> distribution(-1000, 1000); | ||
| 77 | Input::AnalogProperties properties{ | ||
| 78 | .deadzone = 0.0, | ||
| 79 | .range = 1.0f, | ||
| 80 | .offset = 0.0, | ||
| 81 | }; | ||
| 82 | status.accel.x = { | ||
| 83 | .value = 0, | ||
| 84 | .raw_value = static_cast<f32>(distribution(gen)) * 0.001f, | ||
| 85 | .properties = properties, | ||
| 86 | }; | ||
| 87 | status.accel.y = { | ||
| 88 | .value = 0, | ||
| 89 | .raw_value = static_cast<f32>(distribution(gen)) * 0.001f, | ||
| 90 | .properties = properties, | ||
| 91 | }; | ||
| 92 | status.accel.z = { | ||
| 93 | .value = 0, | ||
| 94 | .raw_value = static_cast<f32>(distribution(gen)) * 0.001f, | ||
| 95 | .properties = properties, | ||
| 96 | }; | ||
| 97 | status.gyro.x = { | ||
| 98 | .value = 0, | ||
| 99 | .raw_value = static_cast<f32>(distribution(gen)) * 0.001f, | ||
| 100 | .properties = properties, | ||
| 101 | }; | ||
| 102 | status.gyro.y = { | ||
| 103 | .value = 0, | ||
| 104 | .raw_value = static_cast<f32>(distribution(gen)) * 0.001f, | ||
| 105 | .properties = properties, | ||
| 106 | }; | ||
| 107 | status.gyro.z = { | ||
| 108 | .value = 0, | ||
| 109 | .raw_value = static_cast<f32>(distribution(gen)) * 0.001f, | ||
| 110 | .properties = properties, | ||
| 111 | }; | ||
| 112 | } | ||
| 113 | break; | ||
| 114 | } | ||
| 115 | case Input::InputType::Motion: | ||
| 116 | status = callback.motion_status; | ||
| 117 | break; | ||
| 118 | default: | ||
| 119 | LOG_ERROR(Input, "Conversion from type {} to motion not implemented", callback.type); | ||
| 120 | break; | ||
| 121 | } | ||
| 122 | SanitizeAnalog(status.accel.x, false); | ||
| 123 | SanitizeAnalog(status.accel.y, false); | ||
| 124 | SanitizeAnalog(status.accel.z, false); | ||
| 125 | SanitizeAnalog(status.gyro.x, false); | ||
| 126 | SanitizeAnalog(status.gyro.y, false); | ||
| 127 | SanitizeAnalog(status.gyro.z, false); | ||
| 128 | |||
| 129 | return status; | ||
| 130 | } | ||
| 131 | |||
| 132 | Input::StickStatus TransformToStick(const Input::CallbackStatus& callback) { | ||
| 133 | Input::StickStatus status{}; | ||
| 134 | |||
| 135 | switch (callback.type) { | ||
| 136 | case Input::InputType::Stick: | ||
| 137 | status = callback.stick_status; | ||
| 138 | break; | ||
| 139 | default: | ||
| 140 | LOG_ERROR(Input, "Conversion from type {} to stick not implemented", callback.type); | ||
| 141 | break; | ||
| 142 | } | ||
| 143 | |||
| 144 | SanitizeStick(status.x, status.y, true); | ||
| 145 | const Input::AnalogProperties& properties_x = status.x.properties; | ||
| 146 | const Input::AnalogProperties& properties_y = status.y.properties; | ||
| 147 | const float x = status.x.value; | ||
| 148 | const float y = status.y.value; | ||
| 149 | |||
| 150 | // Set directional buttons | ||
| 151 | status.right = x > properties_x.threshold; | ||
| 152 | status.left = x < -properties_x.threshold; | ||
| 153 | status.up = y > properties_y.threshold; | ||
| 154 | status.down = y < -properties_y.threshold; | ||
| 155 | |||
| 156 | return status; | ||
| 157 | } | ||
| 158 | |||
| 159 | Input::TouchStatus TransformToTouch(const Input::CallbackStatus& callback) { | ||
| 160 | Input::TouchStatus status{}; | ||
| 161 | |||
| 162 | switch (callback.type) { | ||
| 163 | case Input::InputType::Touch: | ||
| 164 | status = callback.touch_status; | ||
| 165 | break; | ||
| 166 | default: | ||
| 167 | LOG_ERROR(Input, "Conversion from type {} to touch not implemented", callback.type); | ||
| 168 | break; | ||
| 169 | } | ||
| 170 | |||
| 171 | SanitizeAnalog(status.x, true); | ||
| 172 | SanitizeAnalog(status.y, true); | ||
| 173 | float& x = status.x.value; | ||
| 174 | float& y = status.y.value; | ||
| 175 | |||
| 176 | // Adjust if value is inverted | ||
| 177 | x = status.x.properties.inverted ? 1.0f + x : x; | ||
| 178 | y = status.y.properties.inverted ? 1.0f + y : y; | ||
| 179 | |||
| 180 | // clamp value | ||
| 181 | x = std::clamp(x, 0.0f, 1.0f); | ||
| 182 | y = std::clamp(y, 0.0f, 1.0f); | ||
| 183 | |||
| 184 | if (status.pressed.inverted) { | ||
| 185 | status.pressed.value = !status.pressed.value; | ||
| 186 | } | ||
| 187 | |||
| 188 | return status; | ||
| 189 | } | ||
| 190 | |||
| 191 | Input::TriggerStatus TransformToTrigger(const Input::CallbackStatus& callback) { | ||
| 192 | Input::TriggerStatus status{}; | ||
| 193 | float& raw_value = status.analog.raw_value; | ||
| 194 | bool calculate_button_value = true; | ||
| 195 | |||
| 196 | switch (callback.type) { | ||
| 197 | case Input::InputType::Analog: | ||
| 198 | status.analog.properties = callback.analog_status.properties; | ||
| 199 | raw_value = callback.analog_status.raw_value; | ||
| 200 | break; | ||
| 201 | case Input::InputType::Button: | ||
| 202 | status.analog.properties.range = 1.0f; | ||
| 203 | status.analog.properties.inverted = callback.button_status.inverted; | ||
| 204 | raw_value = callback.button_status.value ? 1.0f : 0.0f; | ||
| 205 | break; | ||
| 206 | case Input::InputType::Trigger: | ||
| 207 | status = callback.trigger_status; | ||
| 208 | calculate_button_value = false; | ||
| 209 | break; | ||
| 210 | default: | ||
| 211 | LOG_ERROR(Input, "Conversion from type {} to trigger not implemented", callback.type); | ||
| 212 | break; | ||
| 213 | } | ||
| 214 | |||
| 215 | SanitizeAnalog(status.analog, true); | ||
| 216 | const Input::AnalogProperties& properties = status.analog.properties; | ||
| 217 | float& value = status.analog.value; | ||
| 218 | |||
| 219 | // Set button status | ||
| 220 | if (calculate_button_value) { | ||
| 221 | status.pressed = value > properties.threshold; | ||
| 222 | } | ||
| 223 | |||
| 224 | // Adjust if value is inverted | ||
| 225 | value = properties.inverted ? 1.0f + value : value; | ||
| 226 | |||
| 227 | // clamp value | ||
| 228 | value = std::clamp(value, 0.0f, 1.0f); | ||
| 229 | |||
| 230 | return status; | ||
| 231 | } | ||
| 232 | |||
| 233 | void SanitizeAnalog(Input::AnalogStatus& analog, bool clamp_value) { | ||
| 234 | const Input::AnalogProperties& properties = analog.properties; | ||
| 235 | float& raw_value = analog.raw_value; | ||
| 236 | float& value = analog.value; | ||
| 237 | |||
| 238 | if (!std::isnormal(raw_value)) { | ||
| 239 | raw_value = 0; | ||
| 240 | } | ||
| 241 | |||
| 242 | // Apply center offset | ||
| 243 | raw_value -= properties.offset; | ||
| 244 | |||
| 245 | // Set initial values to be formated | ||
| 246 | value = raw_value; | ||
| 247 | |||
| 248 | // Calculate vector size | ||
| 249 | const float r = std::abs(value); | ||
| 250 | |||
| 251 | // Return zero if value is smaller than the deadzone | ||
| 252 | if (r <= properties.deadzone || properties.deadzone == 1.0f) { | ||
| 253 | analog.value = 0; | ||
| 254 | return; | ||
| 255 | } | ||
| 256 | |||
| 257 | // Adjust range of value | ||
| 258 | const float deadzone_factor = | ||
| 259 | 1.0f / r * (r - properties.deadzone) / (1.0f - properties.deadzone); | ||
| 260 | value = value * deadzone_factor / properties.range; | ||
| 261 | |||
| 262 | // Invert direction if needed | ||
| 263 | if (properties.inverted) { | ||
| 264 | value = -value; | ||
| 265 | } | ||
| 266 | |||
| 267 | // Clamp value | ||
| 268 | if (clamp_value) { | ||
| 269 | value = std::clamp(value, -1.0f, 1.0f); | ||
| 270 | } | ||
| 271 | } | ||
| 272 | |||
| 273 | void SanitizeStick(Input::AnalogStatus& analog_x, Input::AnalogStatus& analog_y, bool clamp_value) { | ||
| 274 | const Input::AnalogProperties& properties_x = analog_x.properties; | ||
| 275 | const Input::AnalogProperties& properties_y = analog_y.properties; | ||
| 276 | float& raw_x = analog_x.raw_value; | ||
| 277 | float& raw_y = analog_y.raw_value; | ||
| 278 | float& x = analog_x.value; | ||
| 279 | float& y = analog_y.value; | ||
| 280 | |||
| 281 | if (!std::isnormal(raw_x)) { | ||
| 282 | raw_x = 0; | ||
| 283 | } | ||
| 284 | if (!std::isnormal(raw_y)) { | ||
| 285 | raw_y = 0; | ||
| 286 | } | ||
| 287 | |||
| 288 | // Apply center offset | ||
| 289 | raw_x += properties_x.offset; | ||
| 290 | raw_y += properties_y.offset; | ||
| 291 | |||
| 292 | // Apply X scale correction from offset | ||
| 293 | if (std::abs(properties_x.offset) < 0.5f) { | ||
| 294 | if (raw_x > 0) { | ||
| 295 | raw_x /= 1 + properties_x.offset; | ||
| 296 | } else { | ||
| 297 | raw_x /= 1 - properties_x.offset; | ||
| 298 | } | ||
| 299 | } | ||
| 300 | |||
| 301 | // Apply Y scale correction from offset | ||
| 302 | if (std::abs(properties_y.offset) < 0.5f) { | ||
| 303 | if (raw_y > 0) { | ||
| 304 | raw_y /= 1 + properties_y.offset; | ||
| 305 | } else { | ||
| 306 | raw_y /= 1 - properties_y.offset; | ||
| 307 | } | ||
| 308 | } | ||
| 309 | |||
| 310 | // Invert direction if needed | ||
| 311 | raw_x = properties_x.inverted ? -raw_x : raw_x; | ||
| 312 | raw_y = properties_y.inverted ? -raw_y : raw_y; | ||
| 313 | |||
| 314 | // Set initial values to be formated | ||
| 315 | x = raw_x; | ||
| 316 | y = raw_y; | ||
| 317 | |||
| 318 | // Calculate vector size | ||
| 319 | float r = x * x + y * y; | ||
| 320 | r = std::sqrt(r); | ||
| 321 | |||
| 322 | // TODO(German77): Use deadzone and range of both axis | ||
| 323 | |||
| 324 | // Return zero if values are smaller than the deadzone | ||
| 325 | if (r <= properties_x.deadzone || properties_x.deadzone >= 1.0f) { | ||
| 326 | x = 0; | ||
| 327 | y = 0; | ||
| 328 | return; | ||
| 329 | } | ||
| 330 | |||
| 331 | // Adjust range of joystick | ||
| 332 | const float deadzone_factor = | ||
| 333 | 1.0f / r * (r - properties_x.deadzone) / (1.0f - properties_x.deadzone); | ||
| 334 | x = x * deadzone_factor / properties_x.range; | ||
| 335 | y = y * deadzone_factor / properties_x.range; | ||
| 336 | r = r * deadzone_factor / properties_x.range; | ||
| 337 | |||
| 338 | // Normalize joystick | ||
| 339 | if (clamp_value && r > 1.0f) { | ||
| 340 | x /= r; | ||
| 341 | y /= r; | ||
| 342 | } | ||
| 343 | } | ||
| 344 | |||
| 345 | } // namespace Core::HID | ||
diff --git a/src/core/hid/input_converter.h b/src/core/hid/input_converter.h new file mode 100644 index 000000000..3cc32e26a --- /dev/null +++ b/src/core/hid/input_converter.h | |||
| @@ -0,0 +1,77 @@ | |||
| 1 | // Copyright 2021 yuzu Emulator Project | ||
| 2 | // Licensed under GPLv2 or any later version | ||
| 3 | // Refer to the license.txt file included | ||
| 4 | |||
| 5 | #pragma once | ||
| 6 | |||
| 7 | namespace Input { | ||
| 8 | struct CallbackStatus; | ||
| 9 | }; | ||
| 10 | |||
| 11 | namespace Core::HID { | ||
| 12 | |||
| 13 | /** | ||
| 14 | * Converts raw input data into a valid battery status. | ||
| 15 | * | ||
| 16 | * @param Supported callbacks: Analog, Battery, Trigger. | ||
| 17 | * @return A valid BatteryStatus object. | ||
| 18 | */ | ||
| 19 | Input::BatteryStatus TransformToBattery(const Input::CallbackStatus& callback); | ||
| 20 | |||
| 21 | /** | ||
| 22 | * Converts raw input data into a valid button status. Applies invert properties to the output. | ||
| 23 | * | ||
| 24 | * @param Supported callbacks: Analog, Button, Trigger. | ||
| 25 | * @return A valid TouchStatus object. | ||
| 26 | */ | ||
| 27 | Input::ButtonStatus TransformToButton(const Input::CallbackStatus& callback); | ||
| 28 | |||
| 29 | /** | ||
| 30 | * Converts raw input data into a valid motion status. | ||
| 31 | * | ||
| 32 | * @param Supported callbacks: Motion. | ||
| 33 | * @return A valid TouchStatus object. | ||
| 34 | */ | ||
| 35 | Input::MotionStatus TransformToMotion(const Input::CallbackStatus& callback); | ||
| 36 | |||
| 37 | /** | ||
| 38 | * Converts raw input data into a valid stick status. Applies offset, deadzone, range and invert | ||
| 39 | * properties to the output. | ||
| 40 | * | ||
| 41 | * @param Supported callbacks: Stick. | ||
| 42 | * @return A valid StickStatus object. | ||
| 43 | */ | ||
| 44 | Input::StickStatus TransformToStick(const Input::CallbackStatus& callback); | ||
| 45 | |||
| 46 | /** | ||
| 47 | * Converts raw input data into a valid touch status. | ||
| 48 | * | ||
| 49 | * @param Supported callbacks: Touch. | ||
| 50 | * @return A valid TouchStatus object. | ||
| 51 | */ | ||
| 52 | Input::TouchStatus TransformToTouch(const Input::CallbackStatus& callback); | ||
| 53 | |||
| 54 | /** | ||
| 55 | * Converts raw input data into a valid trigger status. Applies offset, deadzone, range and | ||
| 56 | * invert properties to the output. Button status uses the threshold property if necessary. | ||
| 57 | * | ||
| 58 | * @param Supported callbacks: Analog, Button, Trigger. | ||
| 59 | * @return A valid TriggerStatus object. | ||
| 60 | */ | ||
| 61 | Input::TriggerStatus TransformToTrigger(const Input::CallbackStatus& callback); | ||
| 62 | |||
| 63 | /** | ||
| 64 | * Converts raw analog data into a valid analog value | ||
| 65 | * @param An analog object containing raw data and properties, bool that determines if the value | ||
| 66 | * needs to be clamped between -1.0f and 1.0f. | ||
| 67 | */ | ||
| 68 | void SanitizeAnalog(Input::AnalogStatus& analog, bool clamp_value); | ||
| 69 | |||
| 70 | /** | ||
| 71 | * Converts raw stick data into a valid stick value | ||
| 72 | * @param Two analog objects containing raw data and properties, bool that determines if the value | ||
| 73 | * needs to be clamped into the unit circle. | ||
| 74 | */ | ||
| 75 | void SanitizeStick(Input::AnalogStatus& analog_x, Input::AnalogStatus& analog_y, bool clamp_value); | ||
| 76 | |||
| 77 | } // namespace Core::HID | ||
diff --git a/src/core/hid/motion_input.cpp b/src/core/hid/motion_input.cpp new file mode 100644 index 000000000..93f37b77b --- /dev/null +++ b/src/core/hid/motion_input.cpp | |||
| @@ -0,0 +1,278 @@ | |||
| 1 | // Copyright 2020 yuzu Emulator Project | ||
| 2 | // Licensed under GPLv2 or any later version | ||
| 3 | // Refer to the license.txt file included | ||
| 4 | |||
| 5 | #include "common/math_util.h" | ||
| 6 | #include "core/hid/motion_input.h" | ||
| 7 | |||
| 8 | namespace Core::HID { | ||
| 9 | |||
| 10 | MotionInput::MotionInput() { | ||
| 11 | // Initialize PID constants with default values | ||
| 12 | SetPID(0.3f, 0.005f, 0.0f); | ||
| 13 | } | ||
| 14 | |||
| 15 | void MotionInput::SetPID(f32 new_kp, f32 new_ki, f32 new_kd) { | ||
| 16 | kp = new_kp; | ||
| 17 | ki = new_ki; | ||
| 18 | kd = new_kd; | ||
| 19 | } | ||
| 20 | |||
| 21 | void MotionInput::SetAcceleration(const Common::Vec3f& acceleration) { | ||
| 22 | accel = acceleration; | ||
| 23 | } | ||
| 24 | |||
| 25 | void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) { | ||
| 26 | gyro = gyroscope - gyro_drift; | ||
| 27 | |||
| 28 | // Auto adjust drift to minimize drift | ||
| 29 | if (!IsMoving(0.1f)) { | ||
| 30 | gyro_drift = (gyro_drift * 0.9999f) + (gyroscope * 0.0001f); | ||
| 31 | } | ||
| 32 | |||
| 33 | if (gyro.Length2() < gyro_threshold) { | ||
| 34 | gyro = {}; | ||
| 35 | } else { | ||
| 36 | only_accelerometer = false; | ||
| 37 | } | ||
| 38 | } | ||
| 39 | |||
| 40 | void MotionInput::SetQuaternion(const Common::Quaternion<f32>& quaternion) { | ||
| 41 | quat = quaternion; | ||
| 42 | } | ||
| 43 | |||
| 44 | void MotionInput::SetGyroDrift(const Common::Vec3f& drift) { | ||
| 45 | gyro_drift = drift; | ||
| 46 | } | ||
| 47 | |||
| 48 | void MotionInput::SetGyroThreshold(f32 threshold) { | ||
| 49 | gyro_threshold = threshold; | ||
| 50 | } | ||
| 51 | |||
| 52 | void MotionInput::EnableReset(bool reset) { | ||
| 53 | reset_enabled = reset; | ||
| 54 | } | ||
| 55 | |||
| 56 | void MotionInput::ResetRotations() { | ||
| 57 | rotations = {}; | ||
| 58 | } | ||
| 59 | |||
| 60 | bool MotionInput::IsMoving(f32 sensitivity) const { | ||
| 61 | return gyro.Length() >= sensitivity || accel.Length() <= 0.9f || accel.Length() >= 1.1f; | ||
| 62 | } | ||
| 63 | |||
| 64 | bool MotionInput::IsCalibrated(f32 sensitivity) const { | ||
| 65 | return real_error.Length() < sensitivity; | ||
| 66 | } | ||
| 67 | |||
| 68 | void MotionInput::UpdateRotation(u64 elapsed_time) { | ||
| 69 | const auto sample_period = static_cast<f32>(elapsed_time) / 1000000.0f; | ||
| 70 | if (sample_period > 0.1f) { | ||
| 71 | return; | ||
| 72 | } | ||
| 73 | rotations += gyro * sample_period; | ||
| 74 | } | ||
| 75 | |||
| 76 | void MotionInput::UpdateOrientation(u64 elapsed_time) { | ||
| 77 | if (!IsCalibrated(0.1f)) { | ||
| 78 | ResetOrientation(); | ||
| 79 | } | ||
| 80 | // Short name local variable for readability | ||
| 81 | f32 q1 = quat.w; | ||
| 82 | f32 q2 = quat.xyz[0]; | ||
| 83 | f32 q3 = quat.xyz[1]; | ||
| 84 | f32 q4 = quat.xyz[2]; | ||
| 85 | const auto sample_period = static_cast<f32>(elapsed_time) / 1000000.0f; | ||
| 86 | |||
| 87 | // Ignore invalid elapsed time | ||
| 88 | if (sample_period > 0.1f) { | ||
| 89 | return; | ||
| 90 | } | ||
| 91 | |||
| 92 | const auto normal_accel = accel.Normalized(); | ||
| 93 | auto rad_gyro = gyro * Common::PI * 2; | ||
| 94 | const f32 swap = rad_gyro.x; | ||
| 95 | rad_gyro.x = rad_gyro.y; | ||
| 96 | rad_gyro.y = -swap; | ||
| 97 | rad_gyro.z = -rad_gyro.z; | ||
| 98 | |||
| 99 | // Clear gyro values if there is no gyro present | ||
| 100 | if (only_accelerometer) { | ||
| 101 | rad_gyro.x = 0; | ||
| 102 | rad_gyro.y = 0; | ||
| 103 | rad_gyro.z = 0; | ||
| 104 | } | ||
| 105 | |||
| 106 | // Ignore drift correction if acceleration is not reliable | ||
| 107 | if (accel.Length() >= 0.75f && accel.Length() <= 1.25f) { | ||
| 108 | const f32 ax = -normal_accel.x; | ||
| 109 | const f32 ay = normal_accel.y; | ||
| 110 | const f32 az = -normal_accel.z; | ||
| 111 | |||
| 112 | // Estimated direction of gravity | ||
| 113 | const f32 vx = 2.0f * (q2 * q4 - q1 * q3); | ||
| 114 | const f32 vy = 2.0f * (q1 * q2 + q3 * q4); | ||
| 115 | const f32 vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4; | ||
| 116 | |||
| 117 | // Error is cross product between estimated direction and measured direction of gravity | ||
| 118 | const Common::Vec3f new_real_error = { | ||
| 119 | az * vx - ax * vz, | ||
| 120 | ay * vz - az * vy, | ||
| 121 | ax * vy - ay * vx, | ||
| 122 | }; | ||
| 123 | |||
| 124 | derivative_error = new_real_error - real_error; | ||
| 125 | real_error = new_real_error; | ||
| 126 | |||
| 127 | // Prevent integral windup | ||
| 128 | if (ki != 0.0f && !IsCalibrated(0.05f)) { | ||
| 129 | integral_error += real_error; | ||
| 130 | } else { | ||
| 131 | integral_error = {}; | ||
| 132 | } | ||
| 133 | |||
| 134 | // Apply feedback terms | ||
| 135 | if (!only_accelerometer) { | ||
| 136 | rad_gyro += kp * real_error; | ||
| 137 | rad_gyro += ki * integral_error; | ||
| 138 | rad_gyro += kd * derivative_error; | ||
| 139 | } else { | ||
| 140 | // Give more weight to accelerometer values to compensate for the lack of gyro | ||
| 141 | rad_gyro += 35.0f * kp * real_error; | ||
| 142 | rad_gyro += 10.0f * ki * integral_error; | ||
| 143 | rad_gyro += 10.0f * kd * derivative_error; | ||
| 144 | |||
| 145 | // Emulate gyro values for games that need them | ||
| 146 | gyro.x = -rad_gyro.y; | ||
| 147 | gyro.y = rad_gyro.x; | ||
| 148 | gyro.z = -rad_gyro.z; | ||
| 149 | UpdateRotation(elapsed_time); | ||
| 150 | } | ||
| 151 | } | ||
| 152 | |||
| 153 | const f32 gx = rad_gyro.y; | ||
| 154 | const f32 gy = rad_gyro.x; | ||
| 155 | const f32 gz = rad_gyro.z; | ||
| 156 | |||
| 157 | // Integrate rate of change of quaternion | ||
| 158 | const f32 pa = q2; | ||
| 159 | const f32 pb = q3; | ||
| 160 | const f32 pc = q4; | ||
| 161 | q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * sample_period); | ||
| 162 | q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * sample_period); | ||
| 163 | q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * sample_period); | ||
| 164 | q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * sample_period); | ||
| 165 | |||
| 166 | quat.w = q1; | ||
| 167 | quat.xyz[0] = q2; | ||
| 168 | quat.xyz[1] = q3; | ||
| 169 | quat.xyz[2] = q4; | ||
| 170 | quat = quat.Normalized(); | ||
| 171 | } | ||
| 172 | |||
| 173 | std::array<Common::Vec3f, 3> MotionInput::GetOrientation() const { | ||
| 174 | const Common::Quaternion<float> quad{ | ||
| 175 | .xyz = {-quat.xyz[1], -quat.xyz[0], -quat.w}, | ||
| 176 | .w = -quat.xyz[2], | ||
| 177 | }; | ||
| 178 | const std::array<float, 16> matrix4x4 = quad.ToMatrix(); | ||
| 179 | |||
| 180 | return {Common::Vec3f(matrix4x4[0], matrix4x4[1], -matrix4x4[2]), | ||
| 181 | Common::Vec3f(matrix4x4[4], matrix4x4[5], -matrix4x4[6]), | ||
| 182 | Common::Vec3f(-matrix4x4[8], -matrix4x4[9], matrix4x4[10])}; | ||
| 183 | } | ||
| 184 | |||
| 185 | Common::Vec3f MotionInput::GetAcceleration() const { | ||
| 186 | return accel; | ||
| 187 | } | ||
| 188 | |||
| 189 | Common::Vec3f MotionInput::GetGyroscope() const { | ||
| 190 | return gyro; | ||
| 191 | } | ||
| 192 | |||
| 193 | Common::Quaternion<f32> MotionInput::GetQuaternion() const { | ||
| 194 | return quat; | ||
| 195 | } | ||
| 196 | |||
| 197 | Common::Vec3f MotionInput::GetRotations() const { | ||
| 198 | return rotations; | ||
| 199 | } | ||
| 200 | |||
| 201 | void MotionInput::ResetOrientation() { | ||
| 202 | if (!reset_enabled || only_accelerometer) { | ||
| 203 | return; | ||
| 204 | } | ||
| 205 | if (!IsMoving(0.5f) && accel.z <= -0.9f) { | ||
| 206 | ++reset_counter; | ||
| 207 | if (reset_counter > 900) { | ||
| 208 | quat.w = 0; | ||
| 209 | quat.xyz[0] = 0; | ||
| 210 | quat.xyz[1] = 0; | ||
| 211 | quat.xyz[2] = -1; | ||
| 212 | SetOrientationFromAccelerometer(); | ||
| 213 | integral_error = {}; | ||
| 214 | reset_counter = 0; | ||
| 215 | } | ||
| 216 | } else { | ||
| 217 | reset_counter = 0; | ||
| 218 | } | ||
| 219 | } | ||
| 220 | |||
| 221 | void MotionInput::SetOrientationFromAccelerometer() { | ||
| 222 | int iterations = 0; | ||
| 223 | const f32 sample_period = 0.015f; | ||
| 224 | |||
| 225 | const auto normal_accel = accel.Normalized(); | ||
| 226 | |||
| 227 | while (!IsCalibrated(0.01f) && ++iterations < 100) { | ||
| 228 | // Short name local variable for readability | ||
| 229 | f32 q1 = quat.w; | ||
| 230 | f32 q2 = quat.xyz[0]; | ||
| 231 | f32 q3 = quat.xyz[1]; | ||
| 232 | f32 q4 = quat.xyz[2]; | ||
| 233 | |||
| 234 | Common::Vec3f rad_gyro; | ||
| 235 | const f32 ax = -normal_accel.x; | ||
| 236 | const f32 ay = normal_accel.y; | ||
| 237 | const f32 az = -normal_accel.z; | ||
| 238 | |||
| 239 | // Estimated direction of gravity | ||
| 240 | const f32 vx = 2.0f * (q2 * q4 - q1 * q3); | ||
| 241 | const f32 vy = 2.0f * (q1 * q2 + q3 * q4); | ||
| 242 | const f32 vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4; | ||
| 243 | |||
| 244 | // Error is cross product between estimated direction and measured direction of gravity | ||
| 245 | const Common::Vec3f new_real_error = { | ||
| 246 | az * vx - ax * vz, | ||
| 247 | ay * vz - az * vy, | ||
| 248 | ax * vy - ay * vx, | ||
| 249 | }; | ||
| 250 | |||
| 251 | derivative_error = new_real_error - real_error; | ||
| 252 | real_error = new_real_error; | ||
| 253 | |||
| 254 | rad_gyro += 10.0f * kp * real_error; | ||
| 255 | rad_gyro += 5.0f * ki * integral_error; | ||
| 256 | rad_gyro += 10.0f * kd * derivative_error; | ||
| 257 | |||
| 258 | const f32 gx = rad_gyro.y; | ||
| 259 | const f32 gy = rad_gyro.x; | ||
| 260 | const f32 gz = rad_gyro.z; | ||
| 261 | |||
| 262 | // Integrate rate of change of quaternion | ||
| 263 | const f32 pa = q2; | ||
| 264 | const f32 pb = q3; | ||
| 265 | const f32 pc = q4; | ||
| 266 | q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * sample_period); | ||
| 267 | q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * sample_period); | ||
| 268 | q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * sample_period); | ||
| 269 | q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * sample_period); | ||
| 270 | |||
| 271 | quat.w = q1; | ||
| 272 | quat.xyz[0] = q2; | ||
| 273 | quat.xyz[1] = q3; | ||
| 274 | quat.xyz[2] = q4; | ||
| 275 | quat = quat.Normalized(); | ||
| 276 | } | ||
| 277 | } | ||
| 278 | } // namespace Core::HID | ||
diff --git a/src/core/hid/motion_input.h b/src/core/hid/motion_input.h new file mode 100644 index 000000000..3deef5ac3 --- /dev/null +++ b/src/core/hid/motion_input.h | |||
| @@ -0,0 +1,71 @@ | |||
| 1 | // Copyright 2020 yuzu Emulator Project | ||
| 2 | // Licensed under GPLv2 or any later version | ||
| 3 | // Refer to the license.txt file included | ||
| 4 | |||
| 5 | #pragma once | ||
| 6 | |||
| 7 | #include "common/common_types.h" | ||
| 8 | #include "common/quaternion.h" | ||
| 9 | #include "common/vector_math.h" | ||
| 10 | |||
| 11 | namespace Core::HID { | ||
| 12 | |||
| 13 | class MotionInput { | ||
| 14 | public: | ||
| 15 | explicit MotionInput(); | ||
| 16 | |||
| 17 | MotionInput(const MotionInput&) = default; | ||
| 18 | MotionInput& operator=(const MotionInput&) = default; | ||
| 19 | |||
| 20 | MotionInput(MotionInput&&) = default; | ||
| 21 | MotionInput& operator=(MotionInput&&) = default; | ||
| 22 | |||
| 23 | void SetPID(f32 new_kp, f32 new_ki, f32 new_kd); | ||
| 24 | void SetAcceleration(const Common::Vec3f& acceleration); | ||
| 25 | void SetGyroscope(const Common::Vec3f& gyroscope); | ||
| 26 | void SetQuaternion(const Common::Quaternion<f32>& quaternion); | ||
| 27 | void SetGyroDrift(const Common::Vec3f& drift); | ||
| 28 | void SetGyroThreshold(f32 threshold); | ||
| 29 | |||
| 30 | void EnableReset(bool reset); | ||
| 31 | void ResetRotations(); | ||
| 32 | |||
| 33 | void UpdateRotation(u64 elapsed_time); | ||
| 34 | void UpdateOrientation(u64 elapsed_time); | ||
| 35 | |||
| 36 | [[nodiscard]] std::array<Common::Vec3f, 3> GetOrientation() const; | ||
| 37 | [[nodiscard]] Common::Vec3f GetAcceleration() const; | ||
| 38 | [[nodiscard]] Common::Vec3f GetGyroscope() const; | ||
| 39 | [[nodiscard]] Common::Vec3f GetRotations() const; | ||
| 40 | [[nodiscard]] Common::Quaternion<f32> GetQuaternion() const; | ||
| 41 | |||
| 42 | [[nodiscard]] bool IsMoving(f32 sensitivity) const; | ||
| 43 | [[nodiscard]] bool IsCalibrated(f32 sensitivity) const; | ||
| 44 | |||
| 45 | private: | ||
| 46 | void ResetOrientation(); | ||
| 47 | void SetOrientationFromAccelerometer(); | ||
| 48 | |||
| 49 | // PID constants | ||
| 50 | f32 kp; | ||
| 51 | f32 ki; | ||
| 52 | f32 kd; | ||
| 53 | |||
| 54 | // PID errors | ||
| 55 | Common::Vec3f real_error; | ||
| 56 | Common::Vec3f integral_error; | ||
| 57 | Common::Vec3f derivative_error; | ||
| 58 | |||
| 59 | Common::Quaternion<f32> quat{{0.0f, 0.0f, -1.0f}, 0.0f}; | ||
| 60 | Common::Vec3f rotations; | ||
| 61 | Common::Vec3f accel; | ||
| 62 | Common::Vec3f gyro; | ||
| 63 | Common::Vec3f gyro_drift; | ||
| 64 | |||
| 65 | f32 gyro_threshold = 0.0f; | ||
| 66 | u32 reset_counter = 0; | ||
| 67 | bool reset_enabled = true; | ||
| 68 | bool only_accelerometer = true; | ||
| 69 | }; | ||
| 70 | |||
| 71 | } // namespace Core::HID | ||