From c19ad21ae855c9143a871e378e8d8c59abcaebfa Mon Sep 17 00:00:00 2001 From: german77 Date: Thu, 22 Apr 2021 13:15:59 -0500 Subject: [PATCH] hid: Implement SevenSixAxis and ConsoleSixAxisSensor --- src/core/CMakeLists.txt | 2 + src/core/frontend/input.h | 12 ++- src/core/hle/kernel/transfer_memory.cpp | 4 + src/core/hle/kernel/transfer_memory.h | 3 + .../hid/controllers/console_sixaxis.cpp | 90 +++++++++++++++++++ .../service/hid/controllers/console_sixaxis.h | 80 +++++++++++++++++ src/core/hle/service/hid/controllers/npad.cpp | 4 +- src/core/hle/service/hid/controllers/npad.h | 2 + src/core/hle/service/hid/hid.cpp | 30 ++++--- src/input_common/motion_input.cpp | 10 ++- 10 files changed, 220 insertions(+), 17 deletions(-) create mode 100644 src/core/hle/service/hid/controllers/console_sixaxis.cpp create mode 100644 src/core/hle/service/hid/controllers/console_sixaxis.h diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index 532e418b0e..2ed87d3e9c 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -391,6 +391,8 @@ add_library(core STATIC hle/service/hid/xcd.cpp hle/service/hid/xcd.h hle/service/hid/errors.h + hle/service/hid/controllers/console_sixaxis.cpp + hle/service/hid/controllers/console_sixaxis.h hle/service/hid/controllers/controller_base.cpp hle/service/hid/controllers/controller_base.h hle/service/hid/controllers/debug_pad.cpp diff --git a/src/core/frontend/input.h b/src/core/frontend/input.h index 88ebc64979..0c5d2b3b0a 100644 --- a/src/core/frontend/input.h +++ b/src/core/frontend/input.h @@ -11,6 +11,7 @@ #include #include "common/logging/log.h" #include "common/param_package.h" +#include "common/quaternion.h" #include "common/vector_math.h" namespace Input { @@ -143,9 +144,10 @@ using VibrationDevice = InputDevice; /** * A motion status is an object that returns a tuple of accelerometer state vector, - * gyroscope state vector, rotation state vector and orientation state matrix. + * gyroscope state vector, rotation state vector, orientation state matrix and quaterion state + * vector. * - * For both vectors: + * For both 3D vectors: * x+ is the same direction as RIGHT on D-pad. * y+ is normal to the touch screen, pointing outward. * z+ is the same direction as UP on D-pad. @@ -164,9 +166,13 @@ using VibrationDevice = InputDevice; * x vector * y vector * z vector + * + * For quaternion state vector + * xyz vector + * w float */ using MotionStatus = std::tuple, Common::Vec3, Common::Vec3, - std::array>; + std::array, Common::Quaternion>; /** * A motion device is an input device that returns a motion status object diff --git a/src/core/hle/kernel/transfer_memory.cpp b/src/core/hle/kernel/transfer_memory.cpp index cad063e4d5..1dd65468d8 100644 --- a/src/core/hle/kernel/transfer_memory.cpp +++ b/src/core/hle/kernel/transfer_memory.cpp @@ -36,6 +36,10 @@ std::shared_ptr TransferMemory::Create(KernelCore& kernel, return transfer_memory; } +u8* TransferMemory::GetPointer() { + return memory.GetPointer(base_address); +} + const u8* TransferMemory::GetPointer() const { return memory.GetPointer(base_address); } diff --git a/src/core/hle/kernel/transfer_memory.h b/src/core/hle/kernel/transfer_memory.h index 5219514242..59328c0fef 100644 --- a/src/core/hle/kernel/transfer_memory.h +++ b/src/core/hle/kernel/transfer_memory.h @@ -56,6 +56,9 @@ public: return HANDLE_TYPE; } + /// Gets a pointer to the backing block of this instance. + u8* GetPointer(); + /// Gets a pointer to the backing block of this instance. const u8* GetPointer() const; diff --git a/src/core/hle/service/hid/controllers/console_sixaxis.cpp b/src/core/hle/service/hid/controllers/console_sixaxis.cpp new file mode 100644 index 0000000000..801e14b791 --- /dev/null +++ b/src/core/hle/service/hid/controllers/console_sixaxis.cpp @@ -0,0 +1,90 @@ +// Copyright 2021 yuzu Emulator Project +// Licensed under GPLv2 or any later version +// Refer to the license.txt file included. + +#include "common/settings.h" +#include "core/core_timing.h" +#include "core/hle/service/hid/controllers/console_sixaxis.h" + +namespace Service::HID { +constexpr std::size_t SHARED_MEMORY_OFFSET = 0x3C200; + +Controller_ConsoleSixAxis::Controller_ConsoleSixAxis(Core::System& system) + : ControllerBase(system) {} +Controller_ConsoleSixAxis::~Controller_ConsoleSixAxis() = default; + +void Controller_ConsoleSixAxis::OnInit() {} + +void Controller_ConsoleSixAxis::OnRelease() {} + +void Controller_ConsoleSixAxis::OnUpdate(const Core::Timing::CoreTiming& core_timing, u8* data, + std::size_t size) { + seven_six_axis.header.timestamp = core_timing.GetCPUTicks(); + seven_six_axis.header.total_entry_count = 17; + + if (!IsControllerActivated() || !is_transfer_memory_set) { + seven_six_axis.header.entry_count = 0; + seven_six_axis.header.last_entry_index = 0; + return; + } + seven_six_axis.header.entry_count = 16; + + const auto& last_entry = + seven_six_axis.sevensixaxis_states[seven_six_axis.header.last_entry_index]; + seven_six_axis.header.last_entry_index = (seven_six_axis.header.last_entry_index + 1) % 17; + auto& cur_entry = seven_six_axis.sevensixaxis_states[seven_six_axis.header.last_entry_index]; + + cur_entry.sampling_number = last_entry.sampling_number + 1; + cur_entry.sampling_number2 = cur_entry.sampling_number; + + // Try to read sixaxis sensor states + MotionDevice motion_device{}; + const auto& device = motions[0]; + if (device) { + std::tie(motion_device.accel, motion_device.gyro, motion_device.rotation, + motion_device.orientation, motion_device.quaternion) = device->GetStatus(); + console_six_axis.is_seven_six_axis_sensor_at_rest = motion_device.gyro.Length2() < 0.0001f; + } + + cur_entry.accel = motion_device.accel; + // Zero gyro values as they just mess up with the camera + // Note: Probably a correct sensivity setting must be set + cur_entry.gyro = {}; + cur_entry.quaternion = { + { + motion_device.quaternion.xyz.y, + motion_device.quaternion.xyz.x, + -motion_device.quaternion.w, + }, + -motion_device.quaternion.xyz.z, + }; + + console_six_axis.sampling_number++; + // TODO(German77): Find the purpose of those values + console_six_axis.verticalization_error = 0.0f; + console_six_axis.gyro_bias = {0.0f, 0.0f, 0.0f}; + + // Update console six axis shared memory + std::memcpy(data + SHARED_MEMORY_OFFSET, &console_six_axis, sizeof(console_six_axis)); + // Update seven six axis transfer memory + std::memcpy(transfer_memory, &seven_six_axis, sizeof(seven_six_axis)); +} + +void Controller_ConsoleSixAxis::OnLoadInputDevices() { + const auto player = Settings::values.players.GetValue()[0]; + std::transform(player.motions.begin() + Settings::NativeMotion::MOTION_HID_BEGIN, + player.motions.begin() + Settings::NativeMotion::MOTION_HID_END, motions.begin(), + Input::CreateDevice); +} + +void Controller_ConsoleSixAxis::SetTransferMemoryPointer(u8* t_mem_1) { + is_transfer_memory_set = true; + transfer_memory = t_mem_1; +}; + +void Controller_ConsoleSixAxis::ResetTimestamp() { + auto& cur_entry = seven_six_axis.sevensixaxis_states[seven_six_axis.header.last_entry_index]; + cur_entry.sampling_number = 0; + cur_entry.sampling_number2 = 0; +} +} // namespace Service::HID diff --git a/src/core/hle/service/hid/controllers/console_sixaxis.h b/src/core/hle/service/hid/controllers/console_sixaxis.h new file mode 100644 index 0000000000..ac0501683b --- /dev/null +++ b/src/core/hle/service/hid/controllers/console_sixaxis.h @@ -0,0 +1,80 @@ +// Copyright 2021 yuzu Emulator Project +// Licensed under GPLv2 or any later version +// Refer to the license.txt file included. + +#pragma once + +#include +#include "common/bit_field.h" +#include "common/common_types.h" +#include "common/quaternion.h" +#include "core/frontend/input.h" +#include "core/hle/service/hid/controllers/controller_base.h" + +namespace Service::HID { +class Controller_ConsoleSixAxis final : public ControllerBase { +public: + explicit Controller_ConsoleSixAxis(Core::System& system); + ~Controller_ConsoleSixAxis() override; + + // Called when the controller is initialized + void OnInit() override; + + // When the controller is released + void OnRelease() override; + + // When the controller is requesting an update for the shared memory + void OnUpdate(const Core::Timing::CoreTiming& core_timing, u8* data, size_t size) override; + + // Called when input devices should be loaded + void OnLoadInputDevices() override; + + // Called on InitializeSevenSixAxisSensor + void SetTransferMemoryPointer(u8* t_mem_1); + + // Called on ResetSevenSixAxisSensorTimestamp + void ResetTimestamp(); + +private: + struct SevenSixAxisState { + INSERT_PADDING_WORDS(4); // unused + s64_le sampling_number{}; + s64_le sampling_number2{}; + u64 unknown{}; + Common::Vec3f accel{}; + Common::Vec3f gyro{}; + Common::Quaternion quaternion{}; + }; + static_assert(sizeof(SevenSixAxisState) == 0x50, "SevenSixAxisState is an invalid size"); + + struct SevenSixAxisMemory { + CommonHeader header{}; + std::array sevensixaxis_states{}; + }; + static_assert(sizeof(SevenSixAxisMemory) == 0xA70, "SevenSixAxisMemory is an invalid size"); + + struct ConsoleSharedMemory { + u64_le sampling_number{}; + bool is_seven_six_axis_sensor_at_rest{}; + f32 verticalization_error{}; + Common::Vec3f gyro_bias{}; + }; + static_assert(sizeof(ConsoleSharedMemory) == 0x20, "ConsoleSharedMemory is an invalid size"); + + struct MotionDevice { + Common::Vec3f accel; + Common::Vec3f gyro; + Common::Vec3f rotation; + std::array orientation; + Common::Quaternion quaternion; + }; + + using MotionArray = + std::array, Settings::NativeMotion::NUM_MOTIONS_HID>; + u8* transfer_memory; + MotionArray motions; + bool is_transfer_memory_set = false; + ConsoleSharedMemory console_six_axis{}; + SevenSixAxisMemory seven_six_axis{}; +}; +} // namespace Service::HID diff --git a/src/core/hle/service/hid/controllers/npad.cpp b/src/core/hle/service/hid/controllers/npad.cpp index 113a412541..249c300f66 100644 --- a/src/core/hle/service/hid/controllers/npad.cpp +++ b/src/core/hle/service/hid/controllers/npad.cpp @@ -654,8 +654,8 @@ void Controller_NPad::OnMotionUpdate(const Core::Timing::CoreTiming& core_timing const auto& device = motions[i][e]; if (device) { std::tie(motion_devices[e].accel, motion_devices[e].gyro, - motion_devices[e].rotation, motion_devices[e].orientation) = - device->GetStatus(); + motion_devices[e].rotation, motion_devices[e].orientation, + motion_devices[e].quaternion) = device->GetStatus(); sixaxis_at_rest = sixaxis_at_rest && motion_devices[e].gyro.Length2() < 0.0001f; } } diff --git a/src/core/hle/service/hid/controllers/npad.h b/src/core/hle/service/hid/controllers/npad.h index c3b07bd414..085f42c48e 100644 --- a/src/core/hle/service/hid/controllers/npad.h +++ b/src/core/hle/service/hid/controllers/npad.h @@ -8,6 +8,7 @@ #include #include "common/bit_field.h" #include "common/common_types.h" +#include "common/quaternion.h" #include "common/settings.h" #include "core/frontend/input.h" #include "core/hle/kernel/object.h" @@ -467,6 +468,7 @@ private: Common::Vec3f gyro; Common::Vec3f rotation; std::array orientation; + Common::Quaternion quaternion; }; struct NfcXcdHandle { diff --git a/src/core/hle/service/hid/hid.cpp b/src/core/hle/service/hid/hid.cpp index 2aa1942cb1..9c4bf6d167 100644 --- a/src/core/hle/service/hid/hid.cpp +++ b/src/core/hle/service/hid/hid.cpp @@ -26,6 +26,7 @@ #include "core/hle/service/hid/xcd.h" #include "core/hle/service/service.h" +#include "core/hle/service/hid/controllers/console_sixaxis.h" #include "core/hle/service/hid/controllers/controller_base.h" #include "core/hle/service/hid/controllers/debug_pad.h" #include "core/hle/service/hid/controllers/gesture.h" @@ -67,7 +68,7 @@ IAppletResource::IAppletResource(Core::System& system_) MakeController(HidController::UniquePad); MakeController(HidController::NPad); MakeController(HidController::Gesture); - MakeController(HidController::ConsoleSixAxisSensor); + MakeController(HidController::ConsoleSixAxisSensor); // Homebrew doesn't try to activate some controllers, so we activate them by default GetController(HidController::NPad).ActivateController(); @@ -78,8 +79,6 @@ IAppletResource::IAppletResource(Core::System& system_) GetController(HidController::CaptureButton).SetCommonHeaderOffset(0x5000); GetController(HidController::InputDetector).SetCommonHeaderOffset(0x5200); GetController(HidController::UniquePad).SetCommonHeaderOffset(0x5A00); - GetController(HidController::ConsoleSixAxisSensor) - .SetCommonHeaderOffset(0x3C200); // Register update callbacks pad_update_event = Core::Timing::CreateEvent( @@ -1404,8 +1403,9 @@ void Hid::ActivateConsoleSixAxisSensor(Kernel::HLERequestContext& ctx) { IPC::RequestParser rp{ctx}; const auto applet_resource_user_id{rp.Pop()}; - LOG_WARNING(Service_HID, "(STUBBED) called, applet_resource_user_id={}", - applet_resource_user_id); + applet_resource->ActivateController(HidController::ConsoleSixAxisSensor); + + LOG_WARNING(Service_HID, "called, applet_resource_user_id={}", applet_resource_user_id); IPC::ResponseBuilder rb{ctx, 2}; rb.Push(RESULT_SUCCESS); @@ -1455,8 +1455,9 @@ void Hid::ActivateSevenSixAxisSensor(Kernel::HLERequestContext& ctx) { IPC::RequestParser rp{ctx}; const auto applet_resource_user_id{rp.Pop()}; - LOG_WARNING(Service_HID, "(STUBBED) called, applet_resource_user_id={}", - applet_resource_user_id); + applet_resource->ActivateController(HidController::ConsoleSixAxisSensor); + + LOG_WARNING(Service_HID, "called, applet_resource_user_id={}", applet_resource_user_id); IPC::ResponseBuilder rb{ctx, 2}; rb.Push(RESULT_SUCCESS); @@ -1518,8 +1519,15 @@ void Hid::InitializeSevenSixAxisSensor(Kernel::HLERequestContext& ctx) { ASSERT_MSG(t_mem_1->GetSize() == 0x1000, "t_mem_1 has incorrect size"); ASSERT_MSG(t_mem_2->GetSize() == 0x7F000, "t_mem_2 has incorrect size"); + // Activate console six axis controller + applet_resource->GetController(HidController::ConsoleSixAxisSensor) + .ActivateController(); + + applet_resource->GetController(HidController::ConsoleSixAxisSensor) + .SetTransferMemoryPointer(t_mem_1->GetPointer()); + LOG_WARNING(Service_HID, - "(STUBBED) called, t_mem_1_handle=0x{:08X}, t_mem_2_handle=0x{:08X}, " + "called, t_mem_1_handle=0x{:08X}, t_mem_2_handle=0x{:08X}, " "applet_resource_user_id={}", t_mem_1_handle, t_mem_2_handle, applet_resource_user_id); @@ -1542,8 +1550,10 @@ void Hid::ResetSevenSixAxisSensorTimestamp(Kernel::HLERequestContext& ctx) { IPC::RequestParser rp{ctx}; const auto applet_resource_user_id{rp.Pop()}; - LOG_WARNING(Service_HID, "(STUBBED) called, applet_resource_user_id={}", - applet_resource_user_id); + applet_resource->GetController(HidController::ConsoleSixAxisSensor) + .ResetTimestamp(); + + LOG_WARNING(Service_HID, "called, applet_resource_user_id={}", applet_resource_user_id); IPC::ResponseBuilder rb{ctx, 2}; rb.Push(RESULT_SUCCESS); diff --git a/src/input_common/motion_input.cpp b/src/input_common/motion_input.cpp index 6a65f175e6..1c9d561c0b 100644 --- a/src/input_common/motion_input.cpp +++ b/src/input_common/motion_input.cpp @@ -195,7 +195,8 @@ Input::MotionStatus MotionInput::GetMotion() const { const Common::Vec3f accelerometer = GetAcceleration(); const Common::Vec3f rotation = GetRotations(); const std::array orientation = GetOrientation(); - return {accelerometer, gyroscope, rotation, orientation}; + const Common::Quaternion quaternion = GetQuaternion(); + return {accelerometer, gyroscope, rotation, orientation, quaternion}; } Input::MotionStatus MotionInput::GetRandomMotion(int accel_magnitude, int gyro_magnitude) const { @@ -218,7 +219,12 @@ Input::MotionStatus MotionInput::GetRandomMotion(int accel_magnitude, int gyro_m Common::Vec3f{0.0f, 1.0f, 0.0f}, Common::Vec3f{0.0f, 0.0f, 1.0f}, }; - return {accelerometer * accel_magnitude, gyroscope * gyro_magnitude, rotation, orientation}; + constexpr Common::Quaternion quaternion{ + {0.0f, 0.0f, 0.0f}, + 1.0f, + }; + return {accelerometer * accel_magnitude, gyroscope * gyro_magnitude, rotation, orientation, + quaternion}; } void MotionInput::ResetOrientation() {