ShuriZma
/
dolphin
mirror of https://github.com/dolphin-emu/dolphin.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

WiimoteEmu: Update MotionPlus status from DesiredWiimoteState.

This commit is contained in:
Admiral H. Curtiss 2022-09-17 15:26:14 +02:00
parent 26fd4ea361
commit e7543a9e05
No known key found for this signature in database
GPG Key ID: F051B4C4044F33FB
6 changed files with 82 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Source/Core/Core/HW/WiimoteEmu/DesiredWiimoteState.h
View File

@ -4,9 +4,11 @@
#pragma once
#include <array>
#include <optional>
#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<CameraPoint, 2> camera_points = DEFAULT_CAMERA;
std::optional<MotionPlus::DataFormat::Data> motion_plus = std::nullopt;
};
} // namespace WiimoteEmu

8
Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp
View File

@ -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<ExtensionNumber>(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)

92
Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp
View File

@ -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);

9
Source/Core/Core/HW/WiimoteEmu/MotionPlus.h
View File

@ -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();

14
Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
View File

@ -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<ExtensionNumber>(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<ExtensionNumber>(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();

2
Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
View File

@ -190,7 +190,7 @@ private:
template <typename T, typename H>
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();