core/hid: Move motion_input, create input converter and hid_types
This commit is contained in:
parent
bf71d18af9
commit
449576df93
|
@ -135,8 +135,13 @@ add_library(core STATIC
|
||||||
frontend/input.h
|
frontend/input.h
|
||||||
hardware_interrupt_manager.cpp
|
hardware_interrupt_manager.cpp
|
||||||
hardware_interrupt_manager.h
|
hardware_interrupt_manager.h
|
||||||
|
hid/hid_types.h
|
||||||
|
hid/input_converter.cpp
|
||||||
|
hid/input_converter.h
|
||||||
hid/input_interpreter.cpp
|
hid/input_interpreter.cpp
|
||||||
hid/input_interpreter.h
|
hid/input_interpreter.h
|
||||||
|
hid/motion_input.cpp
|
||||||
|
hid/motion_input.h
|
||||||
hle/api_version.h
|
hle/api_version.h
|
||||||
hle/ipc.h
|
hle/ipc.h
|
||||||
hle/ipc_helpers.h
|
hle/ipc_helpers.h
|
||||||
|
|
|
@ -0,0 +1,388 @@
|
||||||
|
// Copyright 2021 yuzu Emulator Project
|
||||||
|
// Licensed under GPLv2 or any later version
|
||||||
|
// Refer to the license.txt file included.
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "common/bit_field.h"
|
||||||
|
#include "common/common_funcs.h"
|
||||||
|
#include "common/common_types.h"
|
||||||
|
#include "common/point.h"
|
||||||
|
#include "common/uuid.h"
|
||||||
|
|
||||||
|
namespace Core::HID {
|
||||||
|
|
||||||
|
// This is nn::hid::NpadIdType
|
||||||
|
enum class NpadIdType : u8 {
|
||||||
|
Player1 = 0x0,
|
||||||
|
Player2 = 0x1,
|
||||||
|
Player3 = 0x2,
|
||||||
|
Player4 = 0x3,
|
||||||
|
Player5 = 0x4,
|
||||||
|
Player6 = 0x5,
|
||||||
|
Player7 = 0x6,
|
||||||
|
Player8 = 0x7,
|
||||||
|
Other = 0x10,
|
||||||
|
Handheld = 0x20,
|
||||||
|
|
||||||
|
Invalid = 0xFF,
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Converts a NpadIdType to an array index.
|
||||||
|
constexpr size_t NpadIdTypeToIndex(NpadIdType npad_id_type) {
|
||||||
|
switch (npad_id_type) {
|
||||||
|
case NpadIdType::Player1:
|
||||||
|
return 0;
|
||||||
|
case NpadIdType::Player2:
|
||||||
|
return 1;
|
||||||
|
case NpadIdType::Player3:
|
||||||
|
return 2;
|
||||||
|
case NpadIdType::Player4:
|
||||||
|
return 3;
|
||||||
|
case NpadIdType::Player5:
|
||||||
|
return 4;
|
||||||
|
case NpadIdType::Player6:
|
||||||
|
return 5;
|
||||||
|
case NpadIdType::Player7:
|
||||||
|
return 6;
|
||||||
|
case NpadIdType::Player8:
|
||||||
|
return 7;
|
||||||
|
case NpadIdType::Other:
|
||||||
|
return 8;
|
||||||
|
case NpadIdType::Handheld:
|
||||||
|
return 9;
|
||||||
|
default:
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Converts an array index to a NpadIdType
|
||||||
|
constexpr NpadIdType IndexToNpadIdType(size_t index) {
|
||||||
|
switch (index) {
|
||||||
|
case 0:
|
||||||
|
return NpadIdType::Player1;
|
||||||
|
case 1:
|
||||||
|
return NpadIdType::Player2;
|
||||||
|
case 2:
|
||||||
|
return NpadIdType::Player3;
|
||||||
|
case 3:
|
||||||
|
return NpadIdType::Player4;
|
||||||
|
case 4:
|
||||||
|
return NpadIdType::Player5;
|
||||||
|
case 5:
|
||||||
|
return NpadIdType::Player6;
|
||||||
|
case 6:
|
||||||
|
return NpadIdType::Player7;
|
||||||
|
case 7:
|
||||||
|
return NpadIdType::Player8;
|
||||||
|
case 8:
|
||||||
|
return NpadIdType::Other;
|
||||||
|
case 9:
|
||||||
|
return NpadIdType::Handheld;
|
||||||
|
default:
|
||||||
|
return NpadIdType::Invalid;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// This is nn::hid::NpadType
|
||||||
|
enum class NpadType : u8 {
|
||||||
|
None = 0,
|
||||||
|
ProController = 3,
|
||||||
|
Handheld = 4,
|
||||||
|
JoyconDual = 5,
|
||||||
|
JoyconLeft = 6,
|
||||||
|
JoyconRight = 7,
|
||||||
|
GameCube = 8,
|
||||||
|
Pokeball = 9,
|
||||||
|
MaxNpadType = 10,
|
||||||
|
};
|
||||||
|
|
||||||
|
// This is nn::hid::NpadStyleTag
|
||||||
|
struct NpadStyleTag {
|
||||||
|
union {
|
||||||
|
u32_le raw{};
|
||||||
|
|
||||||
|
BitField<0, 1, u32> fullkey;
|
||||||
|
BitField<1, 1, u32> handheld;
|
||||||
|
BitField<2, 1, u32> joycon_dual;
|
||||||
|
BitField<3, 1, u32> joycon_left;
|
||||||
|
BitField<4, 1, u32> joycon_right;
|
||||||
|
BitField<5, 1, u32> gamecube;
|
||||||
|
BitField<6, 1, u32> palma;
|
||||||
|
BitField<7, 1, u32> lark;
|
||||||
|
BitField<8, 1, u32> handheld_lark;
|
||||||
|
BitField<9, 1, u32> lucia;
|
||||||
|
BitField<29, 1, u32> system_ext;
|
||||||
|
BitField<30, 1, u32> system;
|
||||||
|
};
|
||||||
|
};
|
||||||
|
static_assert(sizeof(NpadStyleTag) == 4, "NpadStyleTag is an invalid size");
|
||||||
|
|
||||||
|
// This is nn::hid::TouchAttribute
|
||||||
|
struct TouchAttribute {
|
||||||
|
union {
|
||||||
|
u32 raw{};
|
||||||
|
BitField<0, 1, u32> start_touch;
|
||||||
|
BitField<1, 1, u32> end_touch;
|
||||||
|
};
|
||||||
|
};
|
||||||
|
static_assert(sizeof(TouchAttribute) == 0x4, "TouchAttribute is an invalid size");
|
||||||
|
|
||||||
|
// This is nn::hid::TouchState
|
||||||
|
struct TouchState {
|
||||||
|
u64_le delta_time;
|
||||||
|
TouchAttribute attribute;
|
||||||
|
u32_le finger;
|
||||||
|
Common::Point<u32_le> position;
|
||||||
|
u32_le diameter_x;
|
||||||
|
u32_le diameter_y;
|
||||||
|
u32_le rotation_angle;
|
||||||
|
};
|
||||||
|
static_assert(sizeof(TouchState) == 0x28, "Touchstate is an invalid size");
|
||||||
|
|
||||||
|
// This is nn::hid::NpadControllerColor
|
||||||
|
struct NpadControllerColor {
|
||||||
|
u32_le body;
|
||||||
|
u32_le button;
|
||||||
|
};
|
||||||
|
static_assert(sizeof(NpadControllerColor) == 8, "NpadControllerColor is an invalid size");
|
||||||
|
|
||||||
|
// This is nn::hid::AnalogStickState
|
||||||
|
struct AnalogStickState {
|
||||||
|
s32_le x;
|
||||||
|
s32_le y;
|
||||||
|
};
|
||||||
|
static_assert(sizeof(AnalogStickState) == 8, "AnalogStickState is an invalid size");
|
||||||
|
|
||||||
|
// This is nn::hid::server::NpadGcTriggerState
|
||||||
|
struct NpadGcTriggerState {
|
||||||
|
s64_le sampling_number{};
|
||||||
|
s32_le left{};
|
||||||
|
s32_le right{};
|
||||||
|
};
|
||||||
|
static_assert(sizeof(NpadGcTriggerState) == 0x10, "NpadGcTriggerState is an invalid size");
|
||||||
|
|
||||||
|
// This is nn::hid::system::NpadBatteryLevel
|
||||||
|
using BatteryLevel = u32;
|
||||||
|
static_assert(sizeof(BatteryLevel) == 0x4, "BatteryLevel is an invalid size");
|
||||||
|
|
||||||
|
// This is nn::hid::system::NpadPowerInfo
|
||||||
|
struct NpadPowerInfo {
|
||||||
|
bool is_powered;
|
||||||
|
bool is_charging;
|
||||||
|
INSERT_PADDING_BYTES(0x6);
|
||||||
|
BatteryLevel battery_level;
|
||||||
|
};
|
||||||
|
static_assert(sizeof(NpadPowerInfo) == 0xC, "NpadPowerInfo is an invalid size");
|
||||||
|
|
||||||
|
// This is nn::hid::NpadButton
|
||||||
|
enum class NpadButton : u64 {
|
||||||
|
None = 0,
|
||||||
|
A = 1U << 0,
|
||||||
|
B = 1U << 1,
|
||||||
|
X = 1U << 2,
|
||||||
|
Y = 1U << 3,
|
||||||
|
StickL = 1U << 4,
|
||||||
|
StickR = 1U << 5,
|
||||||
|
L = 1U << 6,
|
||||||
|
R = 1U << 7,
|
||||||
|
ZL = 1U << 8,
|
||||||
|
ZR = 1U << 9,
|
||||||
|
Plus = 1U << 10,
|
||||||
|
Minus = 1U << 11,
|
||||||
|
|
||||||
|
Left = 1U << 12,
|
||||||
|
Up = 1U << 13,
|
||||||
|
Right = 1U << 14,
|
||||||
|
Down = 1U << 15,
|
||||||
|
|
||||||
|
StickLLeft = 1U << 16,
|
||||||
|
StickLUp = 1U << 17,
|
||||||
|
StickLRight = 1U << 18,
|
||||||
|
StickLDown = 1U << 19,
|
||||||
|
|
||||||
|
StickRLeft = 1U << 20,
|
||||||
|
StickRUp = 1U << 21,
|
||||||
|
StickRRight = 1U << 22,
|
||||||
|
StickRDown = 1U << 23,
|
||||||
|
|
||||||
|
LeftSL = 1U << 24,
|
||||||
|
LeftSR = 1U << 25,
|
||||||
|
|
||||||
|
RightSL = 1U << 26,
|
||||||
|
RightSR = 1U << 27,
|
||||||
|
|
||||||
|
Palma = 1U << 28,
|
||||||
|
HandheldLeftB = 1U << 30,
|
||||||
|
};
|
||||||
|
DECLARE_ENUM_FLAG_OPERATORS(NpadButton);
|
||||||
|
|
||||||
|
struct NpadButtonState {
|
||||||
|
union {
|
||||||
|
NpadButton raw{};
|
||||||
|
|
||||||
|
// Buttons
|
||||||
|
BitField<0, 1, u64> a;
|
||||||
|
BitField<1, 1, u64> b;
|
||||||
|
BitField<2, 1, u64> x;
|
||||||
|
BitField<3, 1, u64> y;
|
||||||
|
BitField<4, 1, u64> stick_l;
|
||||||
|
BitField<5, 1, u64> stick_r;
|
||||||
|
BitField<6, 1, u64> l;
|
||||||
|
BitField<7, 1, u64> r;
|
||||||
|
BitField<8, 1, u64> zl;
|
||||||
|
BitField<9, 1, u64> zr;
|
||||||
|
BitField<10, 1, u64> plus;
|
||||||
|
BitField<11, 1, u64> minus;
|
||||||
|
|
||||||
|
// D-Pad
|
||||||
|
BitField<12, 1, u64> left;
|
||||||
|
BitField<13, 1, u64> up;
|
||||||
|
BitField<14, 1, u64> right;
|
||||||
|
BitField<15, 1, u64> down;
|
||||||
|
|
||||||
|
// Left JoyStick
|
||||||
|
BitField<16, 1, u64> stick_l_left;
|
||||||
|
BitField<17, 1, u64> stick_l_up;
|
||||||
|
BitField<18, 1, u64> stick_l_right;
|
||||||
|
BitField<19, 1, u64> stick_l_down;
|
||||||
|
|
||||||
|
// Right JoyStick
|
||||||
|
BitField<20, 1, u64> stick_r_left;
|
||||||
|
BitField<21, 1, u64> stick_r_up;
|
||||||
|
BitField<22, 1, u64> stick_r_right;
|
||||||
|
BitField<23, 1, u64> stick_r_down;
|
||||||
|
|
||||||
|
BitField<24, 1, u64> left_sl;
|
||||||
|
BitField<25, 1, u64> left_sr;
|
||||||
|
|
||||||
|
BitField<26, 1, u64> right_sl;
|
||||||
|
BitField<27, 1, u64> right_sr;
|
||||||
|
|
||||||
|
BitField<28, 1, u64> palma;
|
||||||
|
BitField<30, 1, u64> handheld_left_b;
|
||||||
|
};
|
||||||
|
};
|
||||||
|
static_assert(sizeof(NpadButtonState) == 0x8, "NpadButtonState has incorrect size.");
|
||||||
|
|
||||||
|
// This is nn::hid::DebugPadButton
|
||||||
|
struct DebugPadButton {
|
||||||
|
union {
|
||||||
|
u32_le raw{};
|
||||||
|
BitField<0, 1, u32> a;
|
||||||
|
BitField<1, 1, u32> b;
|
||||||
|
BitField<2, 1, u32> x;
|
||||||
|
BitField<3, 1, u32> y;
|
||||||
|
BitField<4, 1, u32> l;
|
||||||
|
BitField<5, 1, u32> r;
|
||||||
|
BitField<6, 1, u32> zl;
|
||||||
|
BitField<7, 1, u32> zr;
|
||||||
|
BitField<8, 1, u32> plus;
|
||||||
|
BitField<9, 1, u32> minus;
|
||||||
|
BitField<10, 1, u32> d_left;
|
||||||
|
BitField<11, 1, u32> d_up;
|
||||||
|
BitField<12, 1, u32> d_right;
|
||||||
|
BitField<13, 1, u32> d_down;
|
||||||
|
};
|
||||||
|
};
|
||||||
|
static_assert(sizeof(DebugPadButton) == 0x4, "DebugPadButton is an invalid size");
|
||||||
|
|
||||||
|
// This is nn::hid::VibrationDeviceType
|
||||||
|
enum class VibrationDeviceType : u32 {
|
||||||
|
Unknown = 0,
|
||||||
|
LinearResonantActuator = 1,
|
||||||
|
GcErm = 2,
|
||||||
|
};
|
||||||
|
|
||||||
|
// This is nn::hid::VibrationDevicePosition
|
||||||
|
enum class VibrationDevicePosition : u32 {
|
||||||
|
None = 0,
|
||||||
|
Left = 1,
|
||||||
|
Right = 2,
|
||||||
|
};
|
||||||
|
|
||||||
|
// This is nn::hid::VibrationValue
|
||||||
|
struct VibrationValue {
|
||||||
|
f32 low_amplitude;
|
||||||
|
f32 low_frequency;
|
||||||
|
f32 high_amplitude;
|
||||||
|
f32 high_frequency;
|
||||||
|
};
|
||||||
|
static_assert(sizeof(VibrationValue) == 0x10, "VibrationValue has incorrect size.");
|
||||||
|
|
||||||
|
// This is nn::hid::VibrationGcErmCommand
|
||||||
|
enum class VibrationGcErmCommand : u64 {
|
||||||
|
Stop = 0,
|
||||||
|
Start = 1,
|
||||||
|
StopHard = 2,
|
||||||
|
};
|
||||||
|
|
||||||
|
// This is nn::hid::VibrationDeviceInfo
|
||||||
|
struct VibrationDeviceInfo {
|
||||||
|
VibrationDeviceType type{};
|
||||||
|
VibrationDevicePosition position{};
|
||||||
|
};
|
||||||
|
static_assert(sizeof(VibrationDeviceInfo) == 0x8, "VibrationDeviceInfo has incorrect size.");
|
||||||
|
|
||||||
|
// This is nn::hid::KeyboardModifier
|
||||||
|
struct KeyboardModifier {
|
||||||
|
union {
|
||||||
|
u32_le raw{};
|
||||||
|
BitField<0, 1, u32> control;
|
||||||
|
BitField<1, 1, u32> shift;
|
||||||
|
BitField<2, 1, u32> left_alt;
|
||||||
|
BitField<3, 1, u32> right_alt;
|
||||||
|
BitField<4, 1, u32> gui;
|
||||||
|
BitField<8, 1, u32> caps_lock;
|
||||||
|
BitField<9, 1, u32> scroll_lock;
|
||||||
|
BitField<10, 1, u32> num_lock;
|
||||||
|
BitField<11, 1, u32> katakana;
|
||||||
|
BitField<12, 1, u32> hiragana;
|
||||||
|
};
|
||||||
|
};
|
||||||
|
static_assert(sizeof(KeyboardModifier) == 0x4, "KeyboardModifier is an invalid size");
|
||||||
|
|
||||||
|
// This is nn::hid::KeyboardKey
|
||||||
|
struct KeyboardKey {
|
||||||
|
// This should be a 256 bit flag
|
||||||
|
std::array<u8, 32> key;
|
||||||
|
};
|
||||||
|
static_assert(sizeof(KeyboardKey) == 0x20, "KeyboardKey is an invalid size");
|
||||||
|
|
||||||
|
// This is nn::hid::MouseButton
|
||||||
|
struct MouseButton {
|
||||||
|
union {
|
||||||
|
u32_le raw{};
|
||||||
|
BitField<0, 1, u32> left;
|
||||||
|
BitField<1, 1, u32> right;
|
||||||
|
BitField<2, 1, u32> middle;
|
||||||
|
BitField<3, 1, u32> forward;
|
||||||
|
BitField<4, 1, u32> back;
|
||||||
|
};
|
||||||
|
};
|
||||||
|
static_assert(sizeof(MouseButton) == 0x4, "MouseButton is an invalid size");
|
||||||
|
|
||||||
|
// This is nn::hid::MouseAttribute
|
||||||
|
struct MouseAttribute {
|
||||||
|
union {
|
||||||
|
u32_le raw{};
|
||||||
|
BitField<0, 1, u32> transferable;
|
||||||
|
BitField<1, 1, u32> is_connected;
|
||||||
|
};
|
||||||
|
};
|
||||||
|
static_assert(sizeof(MouseAttribute) == 0x4, "MouseAttribute is an invalid size");
|
||||||
|
|
||||||
|
// This is nn::hid::detail::MouseState
|
||||||
|
struct MouseState {
|
||||||
|
s64_le sampling_number;
|
||||||
|
s32_le x;
|
||||||
|
s32_le y;
|
||||||
|
s32_le delta_x;
|
||||||
|
s32_le delta_y;
|
||||||
|
s32_le delta_wheel_x;
|
||||||
|
s32_le delta_wheel_y;
|
||||||
|
MouseButton button;
|
||||||
|
MouseAttribute attribute;
|
||||||
|
};
|
||||||
|
static_assert(sizeof(MouseState) == 0x28, "MouseState is an invalid size");
|
||||||
|
} // namespace Core::HID
|
|
@ -0,0 +1,345 @@
|
||||||
|
// Copyright 2021 yuzu Emulator Project
|
||||||
|
// Licensed under GPLv2 or any later version
|
||||||
|
// Refer to the license.txt file included
|
||||||
|
|
||||||
|
#include <random>
|
||||||
|
|
||||||
|
#include "common/input.h"
|
||||||
|
#include "core/hid/input_converter.h"
|
||||||
|
|
||||||
|
namespace Core::HID {
|
||||||
|
|
||||||
|
Input::BatteryStatus TransformToBattery(const Input::CallbackStatus& callback) {
|
||||||
|
Input::BatteryStatus battery{};
|
||||||
|
switch (callback.type) {
|
||||||
|
case Input::InputType::Analog:
|
||||||
|
case Input::InputType::Trigger: {
|
||||||
|
const auto value = TransformToTrigger(callback).analog.value;
|
||||||
|
battery = Input::BatteryLevel::Empty;
|
||||||
|
if (value > 0.2f) {
|
||||||
|
battery = Input::BatteryLevel::Critical;
|
||||||
|
}
|
||||||
|
if (value > 0.4f) {
|
||||||
|
battery = Input::BatteryLevel::Low;
|
||||||
|
}
|
||||||
|
if (value > 0.6f) {
|
||||||
|
battery = Input::BatteryLevel::Medium;
|
||||||
|
}
|
||||||
|
if (value > 0.8f) {
|
||||||
|
battery = Input::BatteryLevel::Full;
|
||||||
|
}
|
||||||
|
if (value >= 1.0f) {
|
||||||
|
battery = Input::BatteryLevel::Charging;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case Input::InputType::Battery:
|
||||||
|
battery = callback.battery_status;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
LOG_ERROR(Input, "Conversion from type {} to battery not implemented", callback.type);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
return battery;
|
||||||
|
}
|
||||||
|
|
||||||
|
Input::ButtonStatus TransformToButton(const Input::CallbackStatus& callback) {
|
||||||
|
Input::ButtonStatus status{};
|
||||||
|
switch (callback.type) {
|
||||||
|
case Input::InputType::Analog:
|
||||||
|
case Input::InputType::Trigger:
|
||||||
|
status.value = TransformToTrigger(callback).pressed;
|
||||||
|
break;
|
||||||
|
case Input::InputType::Button:
|
||||||
|
status = callback.button_status;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
LOG_ERROR(Input, "Conversion from type {} to button not implemented", callback.type);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (status.inverted) {
|
||||||
|
status.value = !status.value;
|
||||||
|
}
|
||||||
|
|
||||||
|
return status;
|
||||||
|
}
|
||||||
|
|
||||||
|
Input::MotionStatus TransformToMotion(const Input::CallbackStatus& callback) {
|
||||||
|
Input::MotionStatus status{};
|
||||||
|
switch (callback.type) {
|
||||||
|
case Input::InputType::Button: {
|
||||||
|
if (TransformToButton(callback).value) {
|
||||||
|
std::random_device device;
|
||||||
|
std::mt19937 gen(device());
|
||||||
|
std::uniform_int_distribution<s16> distribution(-1000, 1000);
|
||||||
|
Input::AnalogProperties properties{
|
||||||
|
.deadzone = 0.0,
|
||||||
|
.range = 1.0f,
|
||||||
|
.offset = 0.0,
|
||||||
|
};
|
||||||
|
status.accel.x = {
|
||||||
|
.value = 0,
|
||||||
|
.raw_value = static_cast<f32>(distribution(gen)) * 0.001f,
|
||||||
|
.properties = properties,
|
||||||
|
};
|
||||||
|
status.accel.y = {
|
||||||
|
.value = 0,
|
||||||
|
.raw_value = static_cast<f32>(distribution(gen)) * 0.001f,
|
||||||
|
.properties = properties,
|
||||||
|
};
|
||||||
|
status.accel.z = {
|
||||||
|
.value = 0,
|
||||||
|
.raw_value = static_cast<f32>(distribution(gen)) * 0.001f,
|
||||||
|
.properties = properties,
|
||||||
|
};
|
||||||
|
status.gyro.x = {
|
||||||
|
.value = 0,
|
||||||
|
.raw_value = static_cast<f32>(distribution(gen)) * 0.001f,
|
||||||
|
.properties = properties,
|
||||||
|
};
|
||||||
|
status.gyro.y = {
|
||||||
|
.value = 0,
|
||||||
|
.raw_value = static_cast<f32>(distribution(gen)) * 0.001f,
|
||||||
|
.properties = properties,
|
||||||
|
};
|
||||||
|
status.gyro.z = {
|
||||||
|
.value = 0,
|
||||||
|
.raw_value = static_cast<f32>(distribution(gen)) * 0.001f,
|
||||||
|
.properties = properties,
|
||||||
|
};
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case Input::InputType::Motion:
|
||||||
|
status = callback.motion_status;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
LOG_ERROR(Input, "Conversion from type {} to motion not implemented", callback.type);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
SanitizeAnalog(status.accel.x, false);
|
||||||
|
SanitizeAnalog(status.accel.y, false);
|
||||||
|
SanitizeAnalog(status.accel.z, false);
|
||||||
|
SanitizeAnalog(status.gyro.x, false);
|
||||||
|
SanitizeAnalog(status.gyro.y, false);
|
||||||
|
SanitizeAnalog(status.gyro.z, false);
|
||||||
|
|
||||||
|
return status;
|
||||||
|
}
|
||||||
|
|
||||||
|
Input::StickStatus TransformToStick(const Input::CallbackStatus& callback) {
|
||||||
|
Input::StickStatus status{};
|
||||||
|
|
||||||
|
switch (callback.type) {
|
||||||
|
case Input::InputType::Stick:
|
||||||
|
status = callback.stick_status;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
LOG_ERROR(Input, "Conversion from type {} to stick not implemented", callback.type);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
SanitizeStick(status.x, status.y, true);
|
||||||
|
const Input::AnalogProperties& properties_x = status.x.properties;
|
||||||
|
const Input::AnalogProperties& properties_y = status.y.properties;
|
||||||
|
const float x = status.x.value;
|
||||||
|
const float y = status.y.value;
|
||||||
|
|
||||||
|
// Set directional buttons
|
||||||
|
status.right = x > properties_x.threshold;
|
||||||
|
status.left = x < -properties_x.threshold;
|
||||||
|
status.up = y > properties_y.threshold;
|
||||||
|
status.down = y < -properties_y.threshold;
|
||||||
|
|
||||||
|
return status;
|
||||||
|
}
|
||||||
|
|
||||||
|
Input::TouchStatus TransformToTouch(const Input::CallbackStatus& callback) {
|
||||||
|
Input::TouchStatus status{};
|
||||||
|
|
||||||
|
switch (callback.type) {
|
||||||
|
case Input::InputType::Touch:
|
||||||
|
status = callback.touch_status;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
LOG_ERROR(Input, "Conversion from type {} to touch not implemented", callback.type);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
SanitizeAnalog(status.x, true);
|
||||||
|
SanitizeAnalog(status.y, true);
|
||||||
|
float& x = status.x.value;
|
||||||
|
float& y = status.y.value;
|
||||||
|
|
||||||
|
// Adjust if value is inverted
|
||||||
|
x = status.x.properties.inverted ? 1.0f + x : x;
|
||||||
|
y = status.y.properties.inverted ? 1.0f + y : y;
|
||||||
|
|
||||||
|
// clamp value
|
||||||
|
x = std::clamp(x, 0.0f, 1.0f);
|
||||||
|
y = std::clamp(y, 0.0f, 1.0f);
|
||||||
|
|
||||||
|
if (status.pressed.inverted) {
|
||||||
|
status.pressed.value = !status.pressed.value;
|
||||||
|
}
|
||||||
|
|
||||||
|
return status;
|
||||||
|
}
|
||||||
|
|
||||||
|
Input::TriggerStatus TransformToTrigger(const Input::CallbackStatus& callback) {
|
||||||
|
Input::TriggerStatus status{};
|
||||||
|
float& raw_value = status.analog.raw_value;
|
||||||
|
bool calculate_button_value = true;
|
||||||
|
|
||||||
|
switch (callback.type) {
|
||||||
|
case Input::InputType::Analog:
|
||||||
|
status.analog.properties = callback.analog_status.properties;
|
||||||
|
raw_value = callback.analog_status.raw_value;
|
||||||
|
break;
|
||||||
|
case Input::InputType::Button:
|
||||||
|
status.analog.properties.range = 1.0f;
|
||||||
|
status.analog.properties.inverted = callback.button_status.inverted;
|
||||||
|
raw_value = callback.button_status.value ? 1.0f : 0.0f;
|
||||||
|
break;
|
||||||
|
case Input::InputType::Trigger:
|
||||||
|
status = callback.trigger_status;
|
||||||
|
calculate_button_value = false;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
LOG_ERROR(Input, "Conversion from type {} to trigger not implemented", callback.type);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
SanitizeAnalog(status.analog, true);
|
||||||
|
const Input::AnalogProperties& properties = status.analog.properties;
|
||||||
|
float& value = status.analog.value;
|
||||||
|
|
||||||
|
// Set button status
|
||||||
|
if (calculate_button_value) {
|
||||||
|
status.pressed = value > properties.threshold;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Adjust if value is inverted
|
||||||
|
value = properties.inverted ? 1.0f + value : value;
|
||||||
|
|
||||||
|
// clamp value
|
||||||
|
value = std::clamp(value, 0.0f, 1.0f);
|
||||||
|
|
||||||
|
return status;
|
||||||
|
}
|
||||||
|
|
||||||
|
void SanitizeAnalog(Input::AnalogStatus& analog, bool clamp_value) {
|
||||||
|
const Input::AnalogProperties& properties = analog.properties;
|
||||||
|
float& raw_value = analog.raw_value;
|
||||||
|
float& value = analog.value;
|
||||||
|
|
||||||
|
if (!std::isnormal(raw_value)) {
|
||||||
|
raw_value = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Apply center offset
|
||||||
|
raw_value -= properties.offset;
|
||||||
|
|
||||||
|
// Set initial values to be formated
|
||||||
|
value = raw_value;
|
||||||
|
|
||||||
|
// Calculate vector size
|
||||||
|
const float r = std::abs(value);
|
||||||
|
|
||||||
|
// Return zero if value is smaller than the deadzone
|
||||||
|
if (r <= properties.deadzone || properties.deadzone == 1.0f) {
|
||||||
|
analog.value = 0;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Adjust range of value
|
||||||
|
const float deadzone_factor =
|
||||||
|
1.0f / r * (r - properties.deadzone) / (1.0f - properties.deadzone);
|
||||||
|
value = value * deadzone_factor / properties.range;
|
||||||
|
|
||||||
|
// Invert direction if needed
|
||||||
|
if (properties.inverted) {
|
||||||
|
value = -value;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Clamp value
|
||||||
|
if (clamp_value) {
|
||||||
|
value = std::clamp(value, -1.0f, 1.0f);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void SanitizeStick(Input::AnalogStatus& analog_x, Input::AnalogStatus& analog_y, bool clamp_value) {
|
||||||
|
const Input::AnalogProperties& properties_x = analog_x.properties;
|
||||||
|
const Input::AnalogProperties& properties_y = analog_y.properties;
|
||||||
|
float& raw_x = analog_x.raw_value;
|
||||||
|
float& raw_y = analog_y.raw_value;
|
||||||
|
float& x = analog_x.value;
|
||||||
|
float& y = analog_y.value;
|
||||||
|
|
||||||
|
if (!std::isnormal(raw_x)) {
|
||||||
|
raw_x = 0;
|
||||||
|
}
|
||||||
|
if (!std::isnormal(raw_y)) {
|
||||||
|
raw_y = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Apply center offset
|
||||||
|
raw_x += properties_x.offset;
|
||||||
|
raw_y += properties_y.offset;
|
||||||
|
|
||||||
|
// Apply X scale correction from offset
|
||||||
|
if (std::abs(properties_x.offset) < 0.5f) {
|
||||||
|
if (raw_x > 0) {
|
||||||
|
raw_x /= 1 + properties_x.offset;
|
||||||
|
} else {
|
||||||
|
raw_x /= 1 - properties_x.offset;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Apply Y scale correction from offset
|
||||||
|
if (std::abs(properties_y.offset) < 0.5f) {
|
||||||
|
if (raw_y > 0) {
|
||||||
|
raw_y /= 1 + properties_y.offset;
|
||||||
|
} else {
|
||||||
|
raw_y /= 1 - properties_y.offset;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Invert direction if needed
|
||||||
|
raw_x = properties_x.inverted ? -raw_x : raw_x;
|
||||||
|
raw_y = properties_y.inverted ? -raw_y : raw_y;
|
||||||
|
|
||||||
|
// Set initial values to be formated
|
||||||
|
x = raw_x;
|
||||||
|
y = raw_y;
|
||||||
|
|
||||||
|
// Calculate vector size
|
||||||
|
float r = x * x + y * y;
|
||||||
|
r = std::sqrt(r);
|
||||||
|
|
||||||
|
// TODO(German77): Use deadzone and range of both axis
|
||||||
|
|
||||||
|
// Return zero if values are smaller than the deadzone
|
||||||
|
if (r <= properties_x.deadzone || properties_x.deadzone >= 1.0f) {
|
||||||
|
x = 0;
|
||||||
|
y = 0;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Adjust range of joystick
|
||||||
|
const float deadzone_factor =
|
||||||
|
1.0f / r * (r - properties_x.deadzone) / (1.0f - properties_x.deadzone);
|
||||||
|
x = x * deadzone_factor / properties_x.range;
|
||||||
|
y = y * deadzone_factor / properties_x.range;
|
||||||
|
r = r * deadzone_factor / properties_x.range;
|
||||||
|
|
||||||
|
// Normalize joystick
|
||||||
|
if (clamp_value && r > 1.0f) {
|
||||||
|
x /= r;
|
||||||
|
y /= r;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace Core::HID
|
|
@ -0,0 +1,77 @@
|
||||||
|
// Copyright 2021 yuzu Emulator Project
|
||||||
|
// Licensed under GPLv2 or any later version
|
||||||
|
// Refer to the license.txt file included
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
namespace Input {
|
||||||
|
struct CallbackStatus;
|
||||||
|
};
|
||||||
|
|
||||||
|
namespace Core::HID {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Converts raw input data into a valid battery status.
|
||||||
|
*
|
||||||
|
* @param Supported callbacks: Analog, Battery, Trigger.
|
||||||
|
* @return A valid BatteryStatus object.
|
||||||
|
*/
|
||||||
|
Input::BatteryStatus TransformToBattery(const Input::CallbackStatus& callback);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Converts raw input data into a valid button status. Applies invert properties to the output.
|
||||||
|
*
|
||||||
|
* @param Supported callbacks: Analog, Button, Trigger.
|
||||||
|
* @return A valid TouchStatus object.
|
||||||
|
*/
|
||||||
|
Input::ButtonStatus TransformToButton(const Input::CallbackStatus& callback);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Converts raw input data into a valid motion status.
|
||||||
|
*
|
||||||
|
* @param Supported callbacks: Motion.
|
||||||
|
* @return A valid TouchStatus object.
|
||||||
|
*/
|
||||||
|
Input::MotionStatus TransformToMotion(const Input::CallbackStatus& callback);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Converts raw input data into a valid stick status. Applies offset, deadzone, range and invert
|
||||||
|
* properties to the output.
|
||||||
|
*
|
||||||
|
* @param Supported callbacks: Stick.
|
||||||
|
* @return A valid StickStatus object.
|
||||||
|
*/
|
||||||
|
Input::StickStatus TransformToStick(const Input::CallbackStatus& callback);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Converts raw input data into a valid touch status.
|
||||||
|
*
|
||||||
|
* @param Supported callbacks: Touch.
|
||||||
|
* @return A valid TouchStatus object.
|
||||||
|
*/
|
||||||
|
Input::TouchStatus TransformToTouch(const Input::CallbackStatus& callback);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Converts raw input data into a valid trigger status. Applies offset, deadzone, range and
|
||||||
|
* invert properties to the output. Button status uses the threshold property if necessary.
|
||||||
|
*
|
||||||
|
* @param Supported callbacks: Analog, Button, Trigger.
|
||||||
|
* @return A valid TriggerStatus object.
|
||||||
|
*/
|
||||||
|
Input::TriggerStatus TransformToTrigger(const Input::CallbackStatus& callback);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Converts raw analog data into a valid analog value
|
||||||
|
* @param An analog object containing raw data and properties, bool that determines if the value
|
||||||
|
* needs to be clamped between -1.0f and 1.0f.
|
||||||
|
*/
|
||||||
|
void SanitizeAnalog(Input::AnalogStatus& analog, bool clamp_value);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Converts raw stick data into a valid stick value
|
||||||
|
* @param Two analog objects containing raw data and properties, bool that determines if the value
|
||||||
|
* needs to be clamped into the unit circle.
|
||||||
|
*/
|
||||||
|
void SanitizeStick(Input::AnalogStatus& analog_x, Input::AnalogStatus& analog_y, bool clamp_value);
|
||||||
|
|
||||||
|
} // namespace Core::HID
|
|
@ -0,0 +1,278 @@
|
||||||
|
// Copyright 2020 yuzu Emulator Project
|
||||||
|
// Licensed under GPLv2 or any later version
|
||||||
|
// Refer to the license.txt file included
|
||||||
|
|
||||||
|
#include "common/math_util.h"
|
||||||
|
#include "core/hid/motion_input.h"
|
||||||
|
|
||||||
|
namespace Core::HID {
|
||||||
|
|
||||||
|
MotionInput::MotionInput() {
|
||||||
|
// Initialize PID constants with default values
|
||||||
|
SetPID(0.3f, 0.005f, 0.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotionInput::SetPID(f32 new_kp, f32 new_ki, f32 new_kd) {
|
||||||
|
kp = new_kp;
|
||||||
|
ki = new_ki;
|
||||||
|
kd = new_kd;
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotionInput::SetAcceleration(const Common::Vec3f& acceleration) {
|
||||||
|
accel = acceleration;
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) {
|
||||||
|
gyro = gyroscope - gyro_drift;
|
||||||
|
|
||||||
|
// Auto adjust drift to minimize drift
|
||||||
|
if (!IsMoving(0.1f)) {
|
||||||
|
gyro_drift = (gyro_drift * 0.9999f) + (gyroscope * 0.0001f);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (gyro.Length2() < gyro_threshold) {
|
||||||
|
gyro = {};
|
||||||
|
} else {
|
||||||
|
only_accelerometer = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotionInput::SetQuaternion(const Common::Quaternion<f32>& quaternion) {
|
||||||
|
quat = quaternion;
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotionInput::SetGyroDrift(const Common::Vec3f& drift) {
|
||||||
|
gyro_drift = drift;
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotionInput::SetGyroThreshold(f32 threshold) {
|
||||||
|
gyro_threshold = threshold;
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotionInput::EnableReset(bool reset) {
|
||||||
|
reset_enabled = reset;
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotionInput::ResetRotations() {
|
||||||
|
rotations = {};
|
||||||
|
}
|
||||||
|
|
||||||
|
bool MotionInput::IsMoving(f32 sensitivity) const {
|
||||||
|
return gyro.Length() >= sensitivity || accel.Length() <= 0.9f || accel.Length() >= 1.1f;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool MotionInput::IsCalibrated(f32 sensitivity) const {
|
||||||
|
return real_error.Length() < sensitivity;
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotionInput::UpdateRotation(u64 elapsed_time) {
|
||||||
|
const auto sample_period = static_cast<f32>(elapsed_time) / 1000000.0f;
|
||||||
|
if (sample_period > 0.1f) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
rotations += gyro * sample_period;
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotionInput::UpdateOrientation(u64 elapsed_time) {
|
||||||
|
if (!IsCalibrated(0.1f)) {
|
||||||
|
ResetOrientation();
|
||||||
|
}
|
||||||
|
// Short name local variable for readability
|
||||||
|
f32 q1 = quat.w;
|
||||||
|
f32 q2 = quat.xyz[0];
|
||||||
|
f32 q3 = quat.xyz[1];
|
||||||
|
f32 q4 = quat.xyz[2];
|
||||||
|
const auto sample_period = static_cast<f32>(elapsed_time) / 1000000.0f;
|
||||||
|
|
||||||
|
// Ignore invalid elapsed time
|
||||||
|
if (sample_period > 0.1f) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto normal_accel = accel.Normalized();
|
||||||
|
auto rad_gyro = gyro * Common::PI * 2;
|
||||||
|
const f32 swap = rad_gyro.x;
|
||||||
|
rad_gyro.x = rad_gyro.y;
|
||||||
|
rad_gyro.y = -swap;
|
||||||
|
rad_gyro.z = -rad_gyro.z;
|
||||||
|
|
||||||
|
// Clear gyro values if there is no gyro present
|
||||||
|
if (only_accelerometer) {
|
||||||
|
rad_gyro.x = 0;
|
||||||
|
rad_gyro.y = 0;
|
||||||
|
rad_gyro.z = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Ignore drift correction if acceleration is not reliable
|
||||||
|
if (accel.Length() >= 0.75f && accel.Length() <= 1.25f) {
|
||||||
|
const f32 ax = -normal_accel.x;
|
||||||
|
const f32 ay = normal_accel.y;
|
||||||
|
const f32 az = -normal_accel.z;
|
||||||
|
|
||||||
|
// Estimated direction of gravity
|
||||||
|
const f32 vx = 2.0f * (q2 * q4 - q1 * q3);
|
||||||
|
const f32 vy = 2.0f * (q1 * q2 + q3 * q4);
|
||||||
|
const f32 vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4;
|
||||||
|
|
||||||
|
// Error is cross product between estimated direction and measured direction of gravity
|
||||||
|
const Common::Vec3f new_real_error = {
|
||||||
|
az * vx - ax * vz,
|
||||||
|
ay * vz - az * vy,
|
||||||
|
ax * vy - ay * vx,
|
||||||
|
};
|
||||||
|
|
||||||
|
derivative_error = new_real_error - real_error;
|
||||||
|
real_error = new_real_error;
|
||||||
|
|
||||||
|
// Prevent integral windup
|
||||||
|
if (ki != 0.0f && !IsCalibrated(0.05f)) {
|
||||||
|
integral_error += real_error;
|
||||||
|
} else {
|
||||||
|
integral_error = {};
|
||||||
|
}
|
||||||
|
|
||||||
|
// Apply feedback terms
|
||||||
|
if (!only_accelerometer) {
|
||||||
|
rad_gyro += kp * real_error;
|
||||||
|
rad_gyro += ki * integral_error;
|
||||||
|
rad_gyro += kd * derivative_error;
|
||||||
|
} else {
|
||||||
|
// Give more weight to accelerometer values to compensate for the lack of gyro
|
||||||
|
rad_gyro += 35.0f * kp * real_error;
|
||||||
|
rad_gyro += 10.0f * ki * integral_error;
|
||||||
|
rad_gyro += 10.0f * kd * derivative_error;
|
||||||
|
|
||||||
|
// Emulate gyro values for games that need them
|
||||||
|
gyro.x = -rad_gyro.y;
|
||||||
|
gyro.y = rad_gyro.x;
|
||||||
|
gyro.z = -rad_gyro.z;
|
||||||
|
UpdateRotation(elapsed_time);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const f32 gx = rad_gyro.y;
|
||||||
|
const f32 gy = rad_gyro.x;
|
||||||
|
const f32 gz = rad_gyro.z;
|
||||||
|
|
||||||
|
// Integrate rate of change of quaternion
|
||||||
|
const f32 pa = q2;
|
||||||
|
const f32 pb = q3;
|
||||||
|
const f32 pc = q4;
|
||||||
|
q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * sample_period);
|
||||||
|
q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * sample_period);
|
||||||
|
q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * sample_period);
|
||||||
|
q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * sample_period);
|
||||||
|
|
||||||
|
quat.w = q1;
|
||||||
|
quat.xyz[0] = q2;
|
||||||
|
quat.xyz[1] = q3;
|
||||||
|
quat.xyz[2] = q4;
|
||||||
|
quat = quat.Normalized();
|
||||||
|
}
|
||||||
|
|
||||||
|
std::array<Common::Vec3f, 3> MotionInput::GetOrientation() const {
|
||||||
|
const Common::Quaternion<float> quad{
|
||||||
|
.xyz = {-quat.xyz[1], -quat.xyz[0], -quat.w},
|
||||||
|
.w = -quat.xyz[2],
|
||||||
|
};
|
||||||
|
const std::array<float, 16> matrix4x4 = quad.ToMatrix();
|
||||||
|
|
||||||
|
return {Common::Vec3f(matrix4x4[0], matrix4x4[1], -matrix4x4[2]),
|
||||||
|
Common::Vec3f(matrix4x4[4], matrix4x4[5], -matrix4x4[6]),
|
||||||
|
Common::Vec3f(-matrix4x4[8], -matrix4x4[9], matrix4x4[10])};
|
||||||
|
}
|
||||||
|
|
||||||
|
Common::Vec3f MotionInput::GetAcceleration() const {
|
||||||
|
return accel;
|
||||||
|
}
|
||||||
|
|
||||||
|
Common::Vec3f MotionInput::GetGyroscope() const {
|
||||||
|
return gyro;
|
||||||
|
}
|
||||||
|
|
||||||
|
Common::Quaternion<f32> MotionInput::GetQuaternion() const {
|
||||||
|
return quat;
|
||||||
|
}
|
||||||
|
|
||||||
|
Common::Vec3f MotionInput::GetRotations() const {
|
||||||
|
return rotations;
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotionInput::ResetOrientation() {
|
||||||
|
if (!reset_enabled || only_accelerometer) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (!IsMoving(0.5f) && accel.z <= -0.9f) {
|
||||||
|
++reset_counter;
|
||||||
|
if (reset_counter > 900) {
|
||||||
|
quat.w = 0;
|
||||||
|
quat.xyz[0] = 0;
|
||||||
|
quat.xyz[1] = 0;
|
||||||
|
quat.xyz[2] = -1;
|
||||||
|
SetOrientationFromAccelerometer();
|
||||||
|
integral_error = {};
|
||||||
|
reset_counter = 0;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
reset_counter = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void MotionInput::SetOrientationFromAccelerometer() {
|
||||||
|
int iterations = 0;
|
||||||
|
const f32 sample_period = 0.015f;
|
||||||
|
|
||||||
|
const auto normal_accel = accel.Normalized();
|
||||||
|
|
||||||
|
while (!IsCalibrated(0.01f) && ++iterations < 100) {
|
||||||
|
// Short name local variable for readability
|
||||||
|
f32 q1 = quat.w;
|
||||||
|
f32 q2 = quat.xyz[0];
|
||||||
|
f32 q3 = quat.xyz[1];
|
||||||
|
f32 q4 = quat.xyz[2];
|
||||||
|
|
||||||
|
Common::Vec3f rad_gyro;
|
||||||
|
const f32 ax = -normal_accel.x;
|
||||||
|
const f32 ay = normal_accel.y;
|
||||||
|
const f32 az = -normal_accel.z;
|
||||||
|
|
||||||
|
// Estimated direction of gravity
|
||||||
|
const f32 vx = 2.0f * (q2 * q4 - q1 * q3);
|
||||||
|
const f32 vy = 2.0f * (q1 * q2 + q3 * q4);
|
||||||
|
const f32 vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4;
|
||||||
|
|
||||||
|
// Error is cross product between estimated direction and measured direction of gravity
|
||||||
|
const Common::Vec3f new_real_error = {
|
||||||
|
az * vx - ax * vz,
|
||||||
|
ay * vz - az * vy,
|
||||||
|
ax * vy - ay * vx,
|
||||||
|
};
|
||||||
|
|
||||||
|
derivative_error = new_real_error - real_error;
|
||||||
|
real_error = new_real_error;
|
||||||
|
|
||||||
|
rad_gyro += 10.0f * kp * real_error;
|
||||||
|
rad_gyro += 5.0f * ki * integral_error;
|
||||||
|
rad_gyro += 10.0f * kd * derivative_error;
|
||||||
|
|
||||||
|
const f32 gx = rad_gyro.y;
|
||||||
|
const f32 gy = rad_gyro.x;
|
||||||
|
const f32 gz = rad_gyro.z;
|
||||||
|
|
||||||
|
// Integrate rate of change of quaternion
|
||||||
|
const f32 pa = q2;
|
||||||
|
const f32 pb = q3;
|
||||||
|
const f32 pc = q4;
|
||||||
|
q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * sample_period);
|
||||||
|
q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * sample_period);
|
||||||
|
q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * sample_period);
|
||||||
|
q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * sample_period);
|
||||||
|
|
||||||
|
quat.w = q1;
|
||||||
|
quat.xyz[0] = q2;
|
||||||
|
quat.xyz[1] = q3;
|
||||||
|
quat.xyz[2] = q4;
|
||||||
|
quat = quat.Normalized();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} // namespace Core::HID
|
|
@ -0,0 +1,71 @@
|
||||||
|
// Copyright 2020 yuzu Emulator Project
|
||||||
|
// Licensed under GPLv2 or any later version
|
||||||
|
// Refer to the license.txt file included
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "common/common_types.h"
|
||||||
|
#include "common/quaternion.h"
|
||||||
|
#include "common/vector_math.h"
|
||||||
|
|
||||||
|
namespace Core::HID {
|
||||||
|
|
||||||
|
class MotionInput {
|
||||||
|
public:
|
||||||
|
explicit MotionInput();
|
||||||
|
|
||||||
|
MotionInput(const MotionInput&) = default;
|
||||||
|
MotionInput& operator=(const MotionInput&) = default;
|
||||||
|
|
||||||
|
MotionInput(MotionInput&&) = default;
|
||||||
|
MotionInput& operator=(MotionInput&&) = default;
|
||||||
|
|
||||||
|
void SetPID(f32 new_kp, f32 new_ki, f32 new_kd);
|
||||||
|
void SetAcceleration(const Common::Vec3f& acceleration);
|
||||||
|
void SetGyroscope(const Common::Vec3f& gyroscope);
|
||||||
|
void SetQuaternion(const Common::Quaternion<f32>& quaternion);
|
||||||
|
void SetGyroDrift(const Common::Vec3f& drift);
|
||||||
|
void SetGyroThreshold(f32 threshold);
|
||||||
|
|
||||||
|
void EnableReset(bool reset);
|
||||||
|
void ResetRotations();
|
||||||
|
|
||||||
|
void UpdateRotation(u64 elapsed_time);
|
||||||
|
void UpdateOrientation(u64 elapsed_time);
|
||||||
|
|
||||||
|
[[nodiscard]] std::array<Common::Vec3f, 3> GetOrientation() const;
|
||||||
|
[[nodiscard]] Common::Vec3f GetAcceleration() const;
|
||||||
|
[[nodiscard]] Common::Vec3f GetGyroscope() const;
|
||||||
|
[[nodiscard]] Common::Vec3f GetRotations() const;
|
||||||
|
[[nodiscard]] Common::Quaternion<f32> GetQuaternion() const;
|
||||||
|
|
||||||
|
[[nodiscard]] bool IsMoving(f32 sensitivity) const;
|
||||||
|
[[nodiscard]] bool IsCalibrated(f32 sensitivity) const;
|
||||||
|
|
||||||
|
private:
|
||||||
|
void ResetOrientation();
|
||||||
|
void SetOrientationFromAccelerometer();
|
||||||
|
|
||||||
|
// PID constants
|
||||||
|
f32 kp;
|
||||||
|
f32 ki;
|
||||||
|
f32 kd;
|
||||||
|
|
||||||
|
// PID errors
|
||||||
|
Common::Vec3f real_error;
|
||||||
|
Common::Vec3f integral_error;
|
||||||
|
Common::Vec3f derivative_error;
|
||||||
|
|
||||||
|
Common::Quaternion<f32> quat{{0.0f, 0.0f, -1.0f}, 0.0f};
|
||||||
|
Common::Vec3f rotations;
|
||||||
|
Common::Vec3f accel;
|
||||||
|
Common::Vec3f gyro;
|
||||||
|
Common::Vec3f gyro_drift;
|
||||||
|
|
||||||
|
f32 gyro_threshold = 0.0f;
|
||||||
|
u32 reset_counter = 0;
|
||||||
|
bool reset_enabled = true;
|
||||||
|
bool only_accelerometer = true;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace Core::HID
|
Reference in New Issue