From c19ad21ae855c9143a871e378e8d8c59abcaebfa Mon Sep 17 00:00:00 2001
From: german77 <juangerman-13@hotmail.com>
Date: Thu, 22 Apr 2021 13:15:59 -0500
Subject: [PATCH 1/2] 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 532e418b0..2ed87d3e9 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 88ebc6497..0c5d2b3b0 100644
--- a/src/core/frontend/input.h
+++ b/src/core/frontend/input.h
@@ -11,6 +11,7 @@
#include <utility>
#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<u8>;
/**
* 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<u8>;
* x vector
* y vector
* z vector
+ *
+ * For quaternion state vector
+ * xyz vector
+ * w float
*/
using MotionStatus = std::tuple<Common::Vec3<float>, Common::Vec3<float>, Common::Vec3<float>,
- std::array<Common::Vec3f, 3>>;
+ std::array<Common::Vec3f, 3>, Common::Quaternion<f32>>;
/**
* 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 cad063e4d..1dd65468d 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> 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 521951424..59328c0fe 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 000000000..801e14b79
--- /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<Input::MotionDevice>);
+}
+
+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 000000000..ac0501683
--- /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 <array>
+#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<f32> quaternion{};
+ };
+ static_assert(sizeof(SevenSixAxisState) == 0x50, "SevenSixAxisState is an invalid size");
+
+ struct SevenSixAxisMemory {
+ CommonHeader header{};
+ std::array<SevenSixAxisState, 0x21> 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<Common::Vec3f, 3> orientation;
+ Common::Quaternion<f32> quaternion;
+ };
+
+ using MotionArray =
+ std::array<std::unique_ptr<Input::MotionDevice>, 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 113a41254..249c300f6 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 c3b07bd41..085f42c48 100644
--- a/src/core/hle/service/hid/controllers/npad.h
+++ b/src/core/hle/service/hid/controllers/npad.h
@@ -8,6 +8,7 @@
#include <atomic>
#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<Common::Vec3f, 3> orientation;
+ Common::Quaternion<f32> quaternion;
};
struct NfcXcdHandle {
diff --git a/src/core/hle/service/hid/hid.cpp b/src/core/hle/service/hid/hid.cpp
index 2aa1942cb..9c4bf6d16 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<Controller_Stubbed>(HidController::UniquePad);
MakeController<Controller_NPad>(HidController::NPad);
MakeController<Controller_Gesture>(HidController::Gesture);
- MakeController<Controller_Stubbed>(HidController::ConsoleSixAxisSensor);
+ MakeController<Controller_ConsoleSixAxis>(HidController::ConsoleSixAxisSensor);
// Homebrew doesn't try to activate some controllers, so we activate them by default
GetController<Controller_NPad>(HidController::NPad).ActivateController();
@@ -78,8 +79,6 @@ IAppletResource::IAppletResource(Core::System& system_)
GetController<Controller_Stubbed>(HidController::CaptureButton).SetCommonHeaderOffset(0x5000);
GetController<Controller_Stubbed>(HidController::InputDetector).SetCommonHeaderOffset(0x5200);
GetController<Controller_Stubbed>(HidController::UniquePad).SetCommonHeaderOffset(0x5A00);
- GetController<Controller_Stubbed>(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<u64>()};
- 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<u64>()};
- 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<Controller_ConsoleSixAxis>(HidController::ConsoleSixAxisSensor)
+ .ActivateController();
+
+ applet_resource->GetController<Controller_ConsoleSixAxis>(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<u64>()};
- LOG_WARNING(Service_HID, "(STUBBED) called, applet_resource_user_id={}",
- applet_resource_user_id);
+ applet_resource->GetController<Controller_ConsoleSixAxis>(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 6a65f175e..1c9d561c0 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<Common::Vec3f, 3> orientation = GetOrientation();
- return {accelerometer, gyroscope, rotation, orientation};
+ const Common::Quaternion<f32> 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<f32> quaternion{
+ {0.0f, 0.0f, 0.0f},
+ 1.0f,
+ };
+ return {accelerometer * accel_magnitude, gyroscope * gyro_magnitude, rotation, orientation,
+ quaternion};
}
void MotionInput::ResetOrientation() {
From cfdec68d5a8001453fbd49e9677f00ea3dfa9bcb Mon Sep 17 00:00:00 2001
From: german77 <juangerman-13@hotmail.com>
Date: Mon, 26 Apr 2021 22:07:16 -0500
Subject: [PATCH 2/2] address comments
---
src/core/hle/service/hid/controllers/console_sixaxis.cpp | 6 +++---
src/core/hle/service/hid/controllers/console_sixaxis.h | 4 ++--
2 files changed, 5 insertions(+), 5 deletions(-)
diff --git a/src/core/hle/service/hid/controllers/console_sixaxis.cpp b/src/core/hle/service/hid/controllers/console_sixaxis.cpp
index 801e14b79..913768fab 100644
--- a/src/core/hle/service/hid/controllers/console_sixaxis.cpp
+++ b/src/core/hle/service/hid/controllers/console_sixaxis.cpp
@@ -77,10 +77,10 @@ void Controller_ConsoleSixAxis::OnLoadInputDevices() {
Input::CreateDevice<Input::MotionDevice>);
}
-void Controller_ConsoleSixAxis::SetTransferMemoryPointer(u8* t_mem_1) {
+void Controller_ConsoleSixAxis::SetTransferMemoryPointer(u8* t_mem) {
is_transfer_memory_set = true;
- transfer_memory = t_mem_1;
-};
+ transfer_memory = t_mem;
+}
void Controller_ConsoleSixAxis::ResetTimestamp() {
auto& cur_entry = seven_six_axis.sevensixaxis_states[seven_six_axis.header.last_entry_index];
diff --git a/src/core/hle/service/hid/controllers/console_sixaxis.h b/src/core/hle/service/hid/controllers/console_sixaxis.h
index ac0501683..1fae98e94 100644
--- a/src/core/hle/service/hid/controllers/console_sixaxis.h
+++ b/src/core/hle/service/hid/controllers/console_sixaxis.h
@@ -30,7 +30,7 @@ public:
void OnLoadInputDevices() override;
// Called on InitializeSevenSixAxisSensor
- void SetTransferMemoryPointer(u8* t_mem_1);
+ void SetTransferMemoryPointer(u8* t_mem);
// Called on ResetSevenSixAxisSensorTimestamp
void ResetTimestamp();
@@ -71,8 +71,8 @@ private:
using MotionArray =
std::array<std::unique_ptr<Input::MotionDevice>, Settings::NativeMotion::NUM_MOTIONS_HID>;
- u8* transfer_memory;
MotionArray motions;
+ u8* transfer_memory = nullptr;
bool is_transfer_memory_set = false;
ConsoleSharedMemory console_six_axis{};
SevenSixAxisMemory seven_six_axis{};