diff options
| author | german77 <juangerman-13@hotmail.com> | 2021-09-20 16:29:43 -0500 | 
|---|---|---|
| committer | Narr the Reg <juangerman-13@hotmail.com> | 2021-11-24 20:30:22 -0600 | 
| commit | 449576df93f6beb70cff0e009ccb2dd8bce1e085 (patch) | |
| tree | fae04e0d292da6128a074cbe046de6169019774c /src/core/hid | |
| parent | bf71d18af99368d7658c9519086c40e73c6abfdd (diff) | |
core/hid: Move motion_input, create input converter and hid_types
Diffstat (limited to 'src/core/hid')
| -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 | 
5 files changed, 1159 insertions, 0 deletions
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 @@ +// 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 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 @@ +// 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 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 @@ +// 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 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 @@ +// 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 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 @@ +// 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  | 
