From e7543a9e050abde59e7ac98a507b1b17f4b37bba Mon Sep 17 00:00:00 2001 From: "Admiral H. Curtiss" Date: Sat, 17 Sep 2022 15:26:14 +0200 Subject: [PATCH] WiimoteEmu: Update MotionPlus status from DesiredWiimoteState. --- .../Core/HW/WiimoteEmu/DesiredWiimoteState.h | 3 + .../Core/HW/WiimoteEmu/EmuSubroutines.cpp | 8 +- Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp | 92 ++++++++++++------- Source/Core/Core/HW/WiimoteEmu/MotionPlus.h | 9 +- Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp | 14 ++- Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h | 2 +- 6 files changed, 82 insertions(+), 46 deletions(-) diff --git a/Source/Core/Core/HW/WiimoteEmu/DesiredWiimoteState.h b/Source/Core/Core/HW/WiimoteEmu/DesiredWiimoteState.h index ac23c77567..5060ac4388 100644 --- a/Source/Core/Core/HW/WiimoteEmu/DesiredWiimoteState.h +++ b/Source/Core/Core/HW/WiimoteEmu/DesiredWiimoteState.h @@ -4,9 +4,11 @@ #pragma once #include +#include #include "Core/HW/WiimoteCommon/WiimoteReport.h" #include "Core/HW/WiimoteEmu/Camera.h" +#include "Core/HW/WiimoteEmu/MotionPlus.h" #include "Core/HW/WiimoteEmu/WiimoteEmu.h" namespace WiimoteEmu @@ -23,5 +25,6 @@ struct DesiredWiimoteState WiimoteCommon::ButtonData buttons{}; // non-button state in this is ignored WiimoteCommon::AccelData acceleration = DEFAULT_ACCELERATION; std::array camera_points = DEFAULT_CAMERA; + std::optional motion_plus = std::nullopt; }; } // namespace WiimoteEmu diff --git a/Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp b/Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp index 4d1d84da9d..6fa5bbf3f0 100644 --- a/Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp @@ -142,7 +142,8 @@ void Wiimote::SendAck(OutputReportID rpt_id, ErrorCode error_code) InterruptDataInputCallback(rpt.GetData(), rpt.GetSize()); } -void Wiimote::HandleExtensionSwap() +void Wiimote::HandleExtensionSwap(ExtensionNumber desired_extension_number, + bool desired_motion_plus) { if (WIIMOTE_BALANCE_BOARD == m_index) { @@ -151,11 +152,6 @@ void Wiimote::HandleExtensionSwap() return; } - ExtensionNumber desired_extension_number = - static_cast(m_attachments->GetSelectedAttachment()); - - const bool desired_motion_plus = m_motion_plus_setting.GetValue(); - // FYI: AttachExtension also connects devices to the i2c bus if (m_is_motion_plus_attached && !desired_motion_plus) diff --git a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp index 65fdb475d7..8d985abfea 100644 --- a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp @@ -522,9 +522,56 @@ void MotionPlus::Update() } } +MotionPlus::DataFormat::Data MotionPlus::GetGyroscopeData(const Common::Vec3& angular_velocity) +{ + // Conversion from radians to the calibrated values in degrees. + constexpr float VALUE_SCALE = + (CALIBRATION_SCALE_OFFSET >> (CALIBRATION_BITS - BITS_OF_PRECISION)) / float(MathUtil::TAU) * + 360; + + constexpr float SLOW_SCALE = VALUE_SCALE / CALIBRATION_SLOW_SCALE_DEGREES; + constexpr float FAST_SCALE = VALUE_SCALE / CALIBRATION_FAST_SCALE_DEGREES; + + static_assert(ZERO_VALUE == 1 << (BITS_OF_PRECISION - 1), + "SLOW_MAX_RAD_PER_SEC assumes calibrated zero is at center of sensor values."); + + constexpr u16 SENSOR_RANGE = 1 << (BITS_OF_PRECISION - 1); + constexpr float SLOW_MAX_RAD_PER_SEC = SENSOR_RANGE / SLOW_SCALE; + + // Slow (high precision) scaling can be used if it fits in the sensor range. + const float yaw = angular_velocity.z; + const bool yaw_slow = (std::abs(yaw) < SLOW_MAX_RAD_PER_SEC); + const s32 yaw_value = yaw * (yaw_slow ? SLOW_SCALE : FAST_SCALE); + + const float roll = angular_velocity.y; + const bool roll_slow = (std::abs(roll) < SLOW_MAX_RAD_PER_SEC); + const s32 roll_value = roll * (roll_slow ? SLOW_SCALE : FAST_SCALE); + + const float pitch = angular_velocity.x; + const bool pitch_slow = (std::abs(pitch) < SLOW_MAX_RAD_PER_SEC); + const s32 pitch_value = pitch * (pitch_slow ? SLOW_SCALE : FAST_SCALE); + + const u16 clamped_yaw_value = u16(std::clamp(yaw_value + ZERO_VALUE, 0, MAX_VALUE)); + const u16 clamped_roll_value = u16(std::clamp(roll_value + ZERO_VALUE, 0, MAX_VALUE)); + const u16 clamped_pitch_value = u16(std::clamp(pitch_value + ZERO_VALUE, 0, MAX_VALUE)); + + return MotionPlus::DataFormat::Data{ + MotionPlus::DataFormat::GyroRawValue{MotionPlus::DataFormat::GyroType( + clamped_pitch_value, clamped_roll_value, clamped_yaw_value)}, + MotionPlus::DataFormat::SlowType(pitch_slow, roll_slow, yaw_slow)}; +} + +MotionPlus::DataFormat::Data MotionPlus::GetDefaultGyroscopeData() +{ + return MotionPlus::DataFormat::Data{ + MotionPlus::DataFormat::GyroRawValue{ + MotionPlus::DataFormat::GyroType(u16(ZERO_VALUE), u16(ZERO_VALUE), u16(ZERO_VALUE))}, + MotionPlus::DataFormat::SlowType(true, true, true)}; +} + // This is something that is triggered by a read of 0x00 on real hardware. // But we do it here for determinism reasons. -void MotionPlus::PrepareInput(const Common::Vec3& angular_velocity) +void MotionPlus::PrepareInput(const MotionPlus::DataFormat::Data& gyroscope_data) { if (GetActivationStatus() != ActivationStatus::Active) return; @@ -592,41 +639,16 @@ void MotionPlus::PrepareInput(const Common::Vec3& angular_velocity) // If the above logic determined this should be M+ data, update it here. if (mplus_data.is_mp_data) { - constexpr int BITS_OF_PRECISION = 14; + const bool pitch_slow = gyroscope_data.is_slow.x; + const bool roll_slow = gyroscope_data.is_slow.y; + const bool yaw_slow = gyroscope_data.is_slow.z; + const u16 pitch_value = gyroscope_data.gyro.value.x; + const u16 roll_value = gyroscope_data.gyro.value.y; + const u16 yaw_value = gyroscope_data.gyro.value.z; - // Conversion from radians to the calibrated values in degrees. - constexpr float VALUE_SCALE = - (CALIBRATION_SCALE_OFFSET >> (CALIBRATION_BITS - BITS_OF_PRECISION)) / - float(MathUtil::TAU) * 360; - - constexpr float SLOW_SCALE = VALUE_SCALE / CALIBRATION_SLOW_SCALE_DEGREES; - constexpr float FAST_SCALE = VALUE_SCALE / CALIBRATION_FAST_SCALE_DEGREES; - - constexpr s32 ZERO_VALUE = CALIBRATION_ZERO >> (CALIBRATION_BITS - BITS_OF_PRECISION); - constexpr s32 MAX_VALUE = (1 << BITS_OF_PRECISION) - 1; - - static_assert(ZERO_VALUE == 1 << (BITS_OF_PRECISION - 1), - "SLOW_MAX_RAD_PER_SEC assumes calibrated zero is at center of sensor values."); - - constexpr u16 SENSOR_RANGE = 1 << (BITS_OF_PRECISION - 1); - constexpr float SLOW_MAX_RAD_PER_SEC = SENSOR_RANGE / SLOW_SCALE; - - // Slow (high precision) scaling can be used if it fits in the sensor range. - const float yaw = angular_velocity.z; - mplus_data.yaw_slow = (std::abs(yaw) < SLOW_MAX_RAD_PER_SEC); - s32 yaw_value = yaw * (mplus_data.yaw_slow ? SLOW_SCALE : FAST_SCALE); - - const float roll = angular_velocity.y; - mplus_data.roll_slow = (std::abs(roll) < SLOW_MAX_RAD_PER_SEC); - s32 roll_value = roll * (mplus_data.roll_slow ? SLOW_SCALE : FAST_SCALE); - - const float pitch = angular_velocity.x; - mplus_data.pitch_slow = (std::abs(pitch) < SLOW_MAX_RAD_PER_SEC); - s32 pitch_value = pitch * (mplus_data.pitch_slow ? SLOW_SCALE : FAST_SCALE); - - yaw_value = std::clamp(yaw_value + ZERO_VALUE, 0, MAX_VALUE); - roll_value = std::clamp(roll_value + ZERO_VALUE, 0, MAX_VALUE); - pitch_value = std::clamp(pitch_value + ZERO_VALUE, 0, MAX_VALUE); + mplus_data.yaw_slow = u8(yaw_slow); + mplus_data.roll_slow = u8(roll_slow); + mplus_data.pitch_slow = u8(pitch_slow); // Bits 0-7 mplus_data.yaw1 = u8(yaw_value); diff --git a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.h b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.h index db7cf2b044..ced21d8599 100644 --- a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.h +++ b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.h @@ -125,7 +125,10 @@ public: ExtensionPort& GetExtPort(); // Vec3 is interpreted as radians/s about the x,y,z axes following the "right-hand rule". - void PrepareInput(const Common::Vec3& angular_velocity); + static MotionPlus::DataFormat::Data GetGyroscopeData(const Common::Vec3& angular_velocity); + static MotionPlus::DataFormat::Data GetDefaultGyroscopeData(); + + void PrepareInput(const MotionPlus::DataFormat::Data& gyroscope_data); // Pointer to 6 bytes is expected. static void ApplyPassthroughModifications(PassthroughMode, u8* data); @@ -218,6 +221,10 @@ private: static constexpr u16 CALIBRATION_FAST_SCALE_DEGREES = 0x4b0; static constexpr u16 CALIBRATION_SLOW_SCALE_DEGREES = 0x10e; + static constexpr int BITS_OF_PRECISION = 14; + static constexpr s32 ZERO_VALUE = CALIBRATION_ZERO >> (CALIBRATION_BITS - BITS_OF_PRECISION); + static constexpr s32 MAX_VALUE = (1 << BITS_OF_PRECISION) - 1; + void Activate(); void Deactivate(); void OnPassthroughModeWrite(); diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp index d3c365bb42..33eaa9bef3 100644 --- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp +++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp @@ -174,7 +174,8 @@ void Wiimote::Reset() // Switch to desired M+ status and extension (if any). // M+ and EXT are reset on attachment. - HandleExtensionSwap(); + HandleExtensionSwap(static_cast(m_attachments->GetSelectedAttachment()), + m_motion_plus_setting.GetValue()); // Reset sub-devices. m_speaker_logic.Reset(); @@ -460,6 +461,10 @@ DesiredWiimoteState Wiimote::BuildDesiredWiimoteState() Common::Vec2(m_fov_x_setting.GetValue(), m_fov_y_setting.GetValue()) / 360 * float(MathUtil::TAU)); + // Calculate MotionPlus state. + if (m_motion_plus_setting.GetValue()) + wiimote_state.motion_plus = MotionPlus::GetGyroscopeData(GetTotalAngularVelocity()); + return wiimote_state; } @@ -475,7 +480,8 @@ void Wiimote::Update() UpdateButtonsStatus(target_state); // If a new extension is requested in the GUI the change will happen here. - HandleExtensionSwap(); + HandleExtensionSwap(static_cast(m_attachments->GetSelectedAttachment()), + target_state.motion_plus.has_value()); // Allow extension to perform any regular duties it may need. // (e.g. Nunchuk motion simulation step) @@ -579,7 +585,9 @@ void Wiimote::SendDataReport(const DesiredWiimoteState& target_state) if (m_is_motion_plus_attached) { // TODO: Make input preparation triggered by bus read. - m_motion_plus.PrepareInput(GetTotalAngularVelocity()); + m_motion_plus.PrepareInput(target_state.motion_plus.has_value() ? + target_state.motion_plus.value() : + MotionPlus::GetDefaultGyroscopeData()); } u8* ext_data = rpt_builder.GetExtDataPtr(); diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h index cf78d3c0ca..9d6c3a0de1 100644 --- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h +++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h @@ -190,7 +190,7 @@ private: template void InvokeHandler(H&& handler, const WiimoteCommon::OutputReportGeneric& rpt, u32 size); - void HandleExtensionSwap(); + void HandleExtensionSwap(ExtensionNumber desired_extension_number, bool desired_motion_plus); bool ProcessExtensionPortEvent(); void SendDataReport(const DesiredWiimoteState& target_state); bool ProcessReadDataRequest();