WiimoteEmu: MotionPlus is now working.

This commit is contained in:
Jordan Woyak 2019-01-17 09:35:08 -06:00
parent 127b4e77ec
commit 9554ece874
9 changed files with 533 additions and 253 deletions

View File

@ -129,7 +129,7 @@ WiimoteCommon::DataReportBuilder::AccelData ConvertAccelData(const Common::Vec3&
u16(MathUtil::Clamp(std::lround(scaled_accel.z + zero_g), 0l, MAX_VALUE))};
}
Common::Matrix44 EmulateCursorMovement(ControllerEmu::Cursor* ir_group)
void EmulateCursor(MotionState* state, ControllerEmu::Cursor* ir_group, float time_elapsed)
{
using Common::Matrix33;
using Common::Matrix44;
@ -149,10 +149,19 @@ Common::Matrix44 EmulateCursorMovement(ControllerEmu::Cursor* ir_group)
const auto cursor = ir_group->GetState(true);
return Matrix44::Translate({0, MOVE_DISTANCE * float(cursor.z), 0}) *
Matrix44::FromMatrix33(Matrix33::RotateX(pitch_scale * cursor.y) *
Matrix33::RotateZ(yaw_scale * cursor.x)) *
Matrix44::Translate({0, -NEUTRAL_DISTANCE, height});
// TODO: Move state out of ControllerEmu::Cursor
// TODO: Use ApproachPositionWithJerk
// TODO: Move forward/backward after rotation.
const auto new_position =
Common::Vec3{0, NEUTRAL_DISTANCE - MOVE_DISTANCE * float(cursor.z), height};
state->acceleration = new_position - state->position;
state->position = new_position;
// TODO: expose this setting in UI:
constexpr auto MAX_ACCEL = float(MathUtil::TAU * 100);
ApproachAngleWithAccel(state, Common::Vec3(pitch_scale * -cursor.y, 0, yaw_scale * -cursor.x),
MAX_ACCEL, time_elapsed);
}
void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& angle_target,

View File

@ -19,14 +19,19 @@ constexpr double GRAVITY_ACCELERATION = 9.80665;
struct PositionalState
{
// meters
Common::Vec3 position;
// meters/second
Common::Vec3 velocity;
// meters/second^2
Common::Vec3 acceleration;
};
struct RotationalState
{
// radians
Common::Vec3 angle;
// radians/second
Common::Vec3 angular_velocity;
};
@ -47,11 +52,10 @@ void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& target,
void EmulateShake(PositionalState* state, ControllerEmu::Shake* shake_group, float time_elapsed);
void EmulateTilt(RotationalState* state, ControllerEmu::Tilt* tilt_group, float time_elapsed);
void EmulateSwing(MotionState* state, ControllerEmu::Force* swing_group, float time_elapsed);
void EmulateCursor(MotionState* state, ControllerEmu::Cursor* ir_group, float time_elapsed);
// Convert m/s/s acceleration data to the format used by Wiimote/Nunchuk (10-bit unsigned integers).
WiimoteCommon::DataReportBuilder::AccelData ConvertAccelData(const Common::Vec3& accel, u16 zero_g,
u16 one_g);
Common::Matrix44 EmulateCursorMovement(ControllerEmu::Cursor* ir_group);
} // namespace WiimoteEmu

View File

@ -149,11 +149,17 @@ void Wiimote::SendAck(OutputReportID rpt_id, ErrorCode error_code)
void Wiimote::HandleExtensionSwap()
{
if (WIIMOTE_BALANCE_BOARD == m_index)
{
// Prevent M+ or anything else silly from being attached to a balance board.
// In the future if we support an emulated balance board we can force the BB "extension" here.
return;
}
ExtensionNumber desired_extension_number =
static_cast<ExtensionNumber>(m_attachments->GetSelectedAttachment());
// const bool desired_motion_plus = m_motion_plus_setting->GetValue();
const bool desired_motion_plus = false;
const bool desired_motion_plus = m_motion_plus_setting.GetValue();
// FYI: AttachExtension also connects devices to the i2c bus
@ -283,7 +289,7 @@ void Wiimote::HandleWriteData(const OutputReportWriteData& wd)
if (address >= 0x0FCA && address < 0x12C0)
{
// TODO: Only write parts of the Mii block.
// TODO: Use fifferent files for different wiimote numbers.
// TODO: Use different files for different wiimote numbers.
std::ofstream file;
File::OpenFStream(file, File::GetUserPath(D_SESSION_WIIROOT_IDX) + "/mii.bin",
std::ios::binary | std::ios::out);
@ -578,12 +584,16 @@ void Wiimote::DoState(PointerWrap& p)
(m_is_motion_plus_attached ? m_motion_plus.GetExtPort() : m_extension_port)
.AttachExtension(GetActiveExtension());
m_motion_plus.DoState(p);
GetActiveExtension()->DoState(p);
if (m_is_motion_plus_attached)
m_motion_plus.DoState(p);
if (m_active_extension != ExtensionNumber::NONE)
GetActiveExtension()->DoState(p);
// Dynamics
p.Do(m_swing_state);
p.Do(m_tilt_state);
p.Do(m_cursor_state);
p.Do(m_shake_state);
p.DoMarker("Wiimote");

View File

@ -91,8 +91,7 @@ void Nunchuk::Update()
EmulateTilt(&m_tilt_state, m_tilt, 1.f / ::Wiimote::UPDATE_FREQ);
EmulateShake(&m_shake_state, m_shake, 1.f / ::Wiimote::UPDATE_FREQ);
const auto transformation =
GetRotationalMatrix(-m_tilt_state.angle) * GetRotationalMatrix(-m_swing_state.angle);
const auto transformation = GetRotationalMatrix(-m_tilt_state.angle - m_swing_state.angle);
Common::Vec3 accel = transformation * (m_swing_state.acceleration +
Common::Vec3(0, 0, float(GRAVITY_ACCELERATION)));

View File

@ -7,8 +7,12 @@
#include "Common/BitUtils.h"
#include "Common/ChunkFile.h"
#include "Common/Logging/Log.h"
#include "Common/MathUtil.h"
#include "Common/MsgHandler.h"
#include "Core/HW/Wiimote.h"
#include "Core/HW/WiimoteEmu/Dynamics.h"
namespace WiimoteEmu
{
MotionPlus::MotionPlus() : Extension("MotionPlus")
@ -17,47 +21,63 @@ MotionPlus::MotionPlus() : Extension("MotionPlus")
void MotionPlus::Reset()
{
reg_data = {};
m_reg_data = {};
m_activation_progress = {};
// FYI: This ID changes on activation/deactivation
constexpr std::array<u8, 6> initial_id = {0x00, 0x00, 0xA6, 0x20, 0x00, 0x05};
m_reg_data.ext_identifier = initial_id;
// FYI: This ID changes on activation
std::copy(std::begin(initial_id), std::end(initial_id), reg_data.ext_identifier);
// TODO: determine meaning of calibration data:
constexpr std::array<u8, 32> cdata = {
// Calibration data.
// Copied from real hardware as it has yet to be fully reverse engineered.
// It's possible a checksum is present as the other extensions have one.
constexpr std::array<u8, 32> cal_data = {
0x78, 0xd9, 0x78, 0x38, 0x77, 0x9d, 0x2f, 0x0c, 0xcf, 0xf0, 0x31,
0xad, 0xc8, 0x0b, 0x5e, 0x39, 0x6f, 0x81, 0x7b, 0x89, 0x78, 0x51,
0x33, 0x60, 0xc9, 0xf5, 0x37, 0xc1, 0x2d, 0xe9, 0x15, 0x8d,
// 0x79, 0xbc, 0x77, 0xa3, 0x76, 0xd9, 0x30, 0x6c, 0xce, 0x8a, 0x2b,
// 0x83, 0xc8, 0x02, 0x0e, 0x70, 0x74, 0xb5, 0x79, 0x8e, 0x76, 0x45,
// 0x38, 0x22, 0xc7, 0xd6, 0x32, 0x3b, 0x2d, 0x35, 0xde, 0x37,
};
std::copy(std::begin(cdata), std::end(cdata), reg_data.calibration_data);
// TODO: determine the meaning behind this:
constexpr std::array<u8, 64> cert = {
0x99, 0x1a, 0x07, 0x1b, 0x97, 0xf1, 0x11, 0x78, 0x0c, 0x42, 0x2b, 0x68, 0xdf,
0x44, 0x38, 0x0d, 0x2b, 0x7e, 0xd6, 0x84, 0x84, 0x58, 0x65, 0xc9, 0xf2, 0x95,
0xd9, 0xaf, 0xb6, 0xc4, 0x87, 0xd5, 0x18, 0xdb, 0x67, 0x3a, 0xc0, 0x71, 0xec,
0x3e, 0xf4, 0xe6, 0x7e, 0x35, 0xa3, 0x29, 0xf8, 0x1f, 0xc5, 0x7c, 0x3d, 0xb9,
0x56, 0x22, 0x95, 0x98, 0x8f, 0xfb, 0x66, 0x3e, 0x9a, 0xdd, 0xeb, 0x7e,
};
std::copy(std::begin(cert), std::end(cert), reg_data.cert_data);
// constexpr std::array<u8, 32> cal_data = {
// 0x7d, 0xe2, 0x80, 0x5f, 0x78, 0x56, 0x31, 0x04, 0xce, 0xce, 0x33,
// 0xf9, 0xc8, 0x04, 0x63, 0x22, 0x77, 0x26, 0x7c, 0xb7, 0x79, 0x62,
// 0x34, 0x56, 0xc9, 0xa3, 0x3a, 0x35, 0x2d, 0xa8, 0xa9, 0xbc,
// };
m_reg_data.calibration_data = cal_data;
}
void MotionPlus::DoState(PointerWrap& p)
{
p.Do(reg_data);
p.Do(m_reg_data);
p.Do(m_activation_progress);
}
bool MotionPlus::IsActive() const
MotionPlus::ActivationStatus MotionPlus::GetActivationStatus() const
{
return (ACTIVE_DEVICE_ADDR << 1) == reg_data.ext_identifier[2];
// M+ takes a bit of time to activate. During which it is completely unresponsive.
constexpr u8 ACTIVATION_STEPS = ::Wiimote::UPDATE_FREQ * 20 / 1000;
if ((ACTIVE_DEVICE_ADDR << 1) == m_reg_data.ext_identifier[2])
{
if (m_activation_progress < ACTIVATION_STEPS)
return ActivationStatus::Activating;
else
return ActivationStatus::Active;
}
else
{
if (m_activation_progress != 0)
return ActivationStatus::Deactivating;
else
return ActivationStatus::Inactive;
}
}
MotionPlus::PassthroughMode MotionPlus::GetPassthroughMode() const
{
return static_cast<PassthroughMode>(reg_data.ext_identifier[4]);
return static_cast<PassthroughMode>(m_reg_data.ext_identifier[4]);
}
ExtensionPort& MotionPlus::GetExtPort()
@ -67,118 +87,170 @@ ExtensionPort& MotionPlus::GetExtPort()
int MotionPlus::BusRead(u8 slave_addr, u8 addr, int count, u8* data_out)
{
if (IsActive())
switch (GetActivationStatus())
{
// FYI: Motion plus does not respond to 0x53 when activated
case ActivationStatus::Inactive:
if (INACTIVE_DEVICE_ADDR != slave_addr)
{
// Passthrough to the connected extension. (if any)
return m_i2c_bus.BusRead(slave_addr, addr, count, data_out);
}
if (ACTIVE_DEVICE_ADDR == slave_addr)
return RawRead(&reg_data, addr, count, data_out);
else
// Perform a normal read of the M+ register.
return RawRead(&m_reg_data, addr, count, data_out);
case ActivationStatus::Active:
// FYI: Motion plus does not respond to 0x53 when activated.
if (ACTIVE_DEVICE_ADDR != slave_addr)
{
// No i2c passthrough when activated.
return 0;
}
else
{
if (INACTIVE_DEVICE_ADDR == slave_addr)
{
return RawRead(&reg_data, addr, count, data_out);
}
else
{
// Passthrough to the connected extension (if any)
return i2c_bus.BusRead(slave_addr, addr, count, data_out);
}
// Perform a normal read of the M+ register.
return RawRead(&m_reg_data, addr, count, data_out);
default:
case ActivationStatus::Activating:
case ActivationStatus::Deactivating:
// The extension port is completely unresponsive here.
return 0;
}
}
int MotionPlus::BusWrite(u8 slave_addr, u8 addr, int count, const u8* data_in)
{
if (IsActive())
switch (GetActivationStatus())
{
// Motion plus does not respond to 0x53 when activated
if (ACTIVE_DEVICE_ADDR == slave_addr)
case ActivationStatus::Inactive:
{
if (INACTIVE_DEVICE_ADDR != slave_addr)
{
auto const result = RawWrite(&reg_data, addr, count, data_in);
// It seems a write of any value triggers deactivation.
// TODO: kill magic number
if (0xf0 == addr)
{
// Deactivate motion plus:
reg_data.ext_identifier[2] = INACTIVE_DEVICE_ADDR << 1;
reg_data.cert_ready = 0x0;
// Pass through the activation write to the attached extension:
// The M+ deactivation signal is cleverly the same as EXT activation:
i2c_bus.BusWrite(slave_addr, addr, count, data_in);
}
// TODO: kill magic number
else if (0xf1 == addr)
{
INFO_LOG(WIIMOTE, "M+ cert activation: 0x%x", reg_data.cert_enable);
// 0x14,0x18 is also a valid value
// 0x1a is final value
reg_data.cert_ready = 0x18;
}
// TODO: kill magic number
else if (0xf2 == addr)
{
INFO_LOG(WIIMOTE, "M+ calibration ?? : 0x%x", reg_data.unknown_0xf2[0]);
}
return result;
// Passthrough to the connected extension. (if any)
return m_i2c_bus.BusWrite(slave_addr, addr, count, data_in);
}
else
auto const result = RawWrite(&m_reg_data, addr, count, data_in);
if (PASSTHROUGH_MODE_OFFSET == addr)
{
OnPassthroughModeWrite();
}
return result;
}
case ActivationStatus::Active:
{
// FYI: Motion plus does not respond to 0x53 when activated.
if (ACTIVE_DEVICE_ADDR != slave_addr)
{
// No i2c passthrough when activated.
return 0;
}
}
else
{
if (INACTIVE_DEVICE_ADDR == slave_addr)
{
auto const result = RawWrite(&reg_data, addr, count, data_in);
// It seems a write of any value triggers activation.
if (0xfe == addr)
auto const result = RawWrite(&m_reg_data, addr, count, data_in);
if (offsetof(Register, initialized) == addr)
{
// It seems a write of any value here triggers deactivation on a real M+.
Deactivate();
// Passthrough the write to the attached extension.
// The M+ deactivation signal is cleverly the same as EXT activation.
m_i2c_bus.BusWrite(slave_addr, addr, count, data_in);
}
else if (offsetof(Register, init_stage) == addr)
{
if (m_reg_data.init_stage == 0x01)
{
INFO_LOG(WIIMOTE, "M+ has been activated: %d", data_in[0]);
// Activate motion plus:
reg_data.ext_identifier[2] = ACTIVE_DEVICE_ADDR << 1;
// TODO: kill magic number
// reg_data.cert_ready = 0x2;
// A real M+ is unresponsive on the bus for some time during activation
// Reads fail to ack and ext data gets filled with 0xff for a frame or two
// I don't think we need to emulate that.
// TODO: activate extension and disable encrption
// also do this if an extension is attached after activation.
std::array<u8, 1> data = {0x55};
i2c_bus.BusWrite(ACTIVE_DEVICE_ADDR, 0xf0, (int)data.size(), data.data());
m_reg_data.init_progress = 0x18;
}
else
{
// Games are sometimes unhappy with the 64 bytes of data that we have provided.
// We have no choice here but to deactivate and try again.
WARN_LOG(WIIMOTE, "M+ reset due to bad initialization sequence.");
return result;
Deactivate();
}
}
else
else if (offsetof(Register, calibration_trigger) == addr)
{
// Passthrough to the connected extension (if any)
return i2c_bus.BusWrite(slave_addr, addr, count, data_in);
// Games seem to invoke this twice to start and stop. Exact consequences unknown.
DEBUG_LOG(WIIMOTE, "M+ calibration trigger: 0x%x", m_reg_data.calibration_trigger);
}
else if (PASSTHROUGH_MODE_OFFSET == addr)
{
// Games sometimes (not often) write zero here to deactivate the M+.
OnPassthroughModeWrite();
}
return result;
}
default:
case ActivationStatus::Activating:
case ActivationStatus::Deactivating:
// The extension port is completely unresponsive here.
return 0;
}
}
void MotionPlus::OnPassthroughModeWrite()
{
const auto status = GetActivationStatus();
switch (GetPassthroughMode())
{
case PassthroughMode::Disabled:
case PassthroughMode::Nunchuk:
case PassthroughMode::Classic:
if (ActivationStatus::Active != status)
Activate();
break;
default:
if (ActivationStatus::Inactive != status)
Deactivate();
break;
}
}
void MotionPlus::Activate()
{
DEBUG_LOG(WIIMOTE, "M+ has been activated.");
m_reg_data.ext_identifier[2] = ACTIVE_DEVICE_ADDR << 1;
m_reg_data.init_progress = 0x2;
// We must do this to reset our extension_connected flag:
m_reg_data.controller_data = {};
}
void MotionPlus::Deactivate()
{
DEBUG_LOG(WIIMOTE, "M+ has been deactivated.");
m_reg_data.ext_identifier[2] = INACTIVE_DEVICE_ADDR << 1;
m_reg_data.init_progress = 0x0;
}
bool MotionPlus::ReadDeviceDetectPin() const
{
if (IsActive())
{
return true;
}
else
switch (GetActivationStatus())
{
case ActivationStatus::Inactive:
// When inactive the device detect pin reads from the ext port:
return m_extension_port.IsDeviceConnected();
case ActivationStatus::Active:
return true;
default:
case ActivationStatus::Activating:
case ActivationStatus::Deactivating:
return false;
}
}
@ -189,97 +261,168 @@ bool MotionPlus::IsButtonPressed() const
void MotionPlus::Update()
{
if (!IsActive())
switch (GetActivationStatus())
{
return;
case ActivationStatus::Activating:
++m_activation_progress;
break;
case ActivationStatus::Deactivating:
--m_activation_progress;
break;
case ActivationStatus::Active:
{
u8* const data = m_reg_data.controller_data.data();
DataFormat mplus_data = Common::BitCastPtr<DataFormat>(data);
const bool is_ext_connected = m_extension_port.IsDeviceConnected();
// Check for extension change:
if (is_ext_connected != mplus_data.extension_connected)
{
if (is_ext_connected)
{
DEBUG_LOG(WIIMOTE, "M+ initializing new extension.");
// The M+ automatically initializes an extension when attached.
// What we do here does not exactly match a real M+,
// but it's close enough for our emulated extensions which are not very picky.
// Disable encryption
{
constexpr u8 INIT_OFFSET = offsetof(Register, initialized);
std::array<u8, 1> enc_data = {0x55};
m_i2c_bus.BusWrite(ACTIVE_DEVICE_ADDR, INIT_OFFSET, (int)enc_data.size(),
enc_data.data());
}
// Read identifier
{
constexpr u8 ID_OFFSET = offsetof(Register, ext_identifier);
std::array<u8, 6> id_data = {};
m_i2c_bus.BusRead(ACTIVE_DEVICE_ADDR, ID_OFFSET, (int)id_data.size(), id_data.data());
m_reg_data.passthrough_ext_id_0 = id_data[0];
m_reg_data.passthrough_ext_id_4 = id_data[4];
m_reg_data.passthrough_ext_id_5 = id_data[5];
}
// Read calibration data
{
constexpr u8 CAL_OFFSET = offsetof(Register, calibration_data);
m_i2c_bus.BusRead(ACTIVE_DEVICE_ADDR, CAL_OFFSET,
(int)m_reg_data.passthrough_ext_calib.size(),
m_reg_data.passthrough_ext_calib.data());
}
}
// Update flag in register:
mplus_data.extension_connected = is_ext_connected;
Common::BitCastPtr<DataFormat>(data) = mplus_data;
}
break;
}
auto& data = reg_data.controller_data;
default:
break;
}
}
if (0x0 == reg_data.cert_ready)
// 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)
{
if (GetActivationStatus() != ActivationStatus::Active)
return;
u8* const data = m_reg_data.controller_data.data();
// Try to alternate between M+ and EXT data:
// This flag is checked down below where the controller data is prepared.
DataFormat mplus_data = Common::BitCastPtr<DataFormat>(data);
mplus_data.is_mp_data ^= true;
// Maintain the current state of this bit rather than reading from the port.
// We update this bit elsewhere and performs some tasks on change.
const bool is_ext_connected = mplus_data.extension_connected;
if (0x2 == m_reg_data.init_progress)
{
// Without sending this nonsense, inputs are unresponsive.. even regular buttons
// Device still operates when changing the data slightly so its not any sort of encrpytion
// It even works when removing the is_mp_data bit in the last byte
// My M+ non-inside gives: 61,46,45,aa,0,2 or b6,46,45,9a,0,2
// static const u8 init_data[6] = {0x8e, 0xb0, 0x4f, 0x5a, 0xfc | 0x01, 0x02};
constexpr std::array<u8, 6> init_data = {0x81, 0x46, 0x46, 0xb6, 0x01, 0x02};
// Activation sets init_progress to 0x2.
// Harness this to send some special first-time data.
// The first data report of the M+ contains some unknown data.
// Without sending this, inputs are unresponsive.. even regular buttons.
// The data varies but it is typically something like the following:
const std::array<u8, 6> init_data = {0x81, 0x46, 0x46, 0xb6, is_ext_connected, 0x02};
// const std::array<u8, 6> init_data = {0xdd, 0x46, 0x47, 0xb6, is_ext_connected, 0x02};
// const std::array<u8, 6> init_data = {0xc3, 0xb0, 0x4f, 0x52, u8(0xfc | is_ext_connected),
// 0x02};
// const std::array<u8, 6> init_data = {0xf0, 0x46, 0x47, 0xb6, is_ext_connected, 0x02};
std::copy(std::begin(init_data), std::end(init_data), data);
reg_data.cert_ready = 0x2;
m_reg_data.init_progress = 0x4;
return;
}
if (0x2 == reg_data.cert_ready)
else if (0x4 == m_reg_data.init_progress)
{
constexpr std::array<u8, 6> init_data = {0x7f, 0xcf, 0xdf, 0x8b, 0x4f, 0x82};
std::copy(std::begin(init_data), std::end(init_data), data);
reg_data.cert_ready = 0x8;
return;
// Force another report of M+ data.
// The second data report is regular M+ data, even if a passthrough mode is set.
mplus_data.is_mp_data = true;
// This is some sort of calibration data and checksum.
// Copied from real hardware as it has yet to be fully reverse engineered.
constexpr std::array<u8, 64> init_data = {
0x99, 0x1a, 0x07, 0x1b, 0x97, 0xf1, 0x11, 0x78, 0x0c, 0x42, 0x2b, 0x68, 0xdf,
0x44, 0x38, 0x0d, 0x2b, 0x7e, 0xd6, 0x84, 0x84, 0x58, 0x65, 0xc9, 0xf2, 0x95,
0xd9, 0xaf, 0xb6, 0xc4, 0x87, 0xd5, 0x18, 0xdb, 0x67, 0x3a, 0xc0, 0x71, 0xec,
0x3e, 0xf4, 0xe6, 0x7e, 0x35, 0xa3, 0x29, 0xf8, 0x1f, 0xc5, 0x7c, 0x3d, 0xb9,
0x56, 0x22, 0x95, 0x98, 0x8f, 0xfb, 0x66, 0x3e, 0x9a, 0xdd, 0xeb, 0x7e,
};
m_reg_data.init_data = init_data;
DEBUG_LOG(WIIMOTE, "M+ initialization data step 1 is ready.");
// Note. A real M+ can take about 2 seconds to reach this state.
// Games seem to not care that we complete almost instantly.
m_reg_data.init_progress = 0xe;
}
if (0x8 == reg_data.cert_ready)
else if (0x18 == m_reg_data.init_progress)
{
// A real wiimote takes about 2 seconds to reach this state:
reg_data.cert_ready = 0xe;
}
if (0x18 == reg_data.cert_ready)
{
// TODO: determine the meaning of this
constexpr std::array<u8, 64> mp_cert2 = {
// This is some sort of calibration data and checksum.
// Copied from real hardware as it has yet to be fully reverse engineered.
constexpr std::array<u8, 64> init_data = {
0xa5, 0x84, 0x1f, 0xd6, 0xbd, 0xdc, 0x7a, 0x4c, 0xf3, 0xc0, 0x24, 0xe0, 0x92,
0xef, 0x19, 0x28, 0x65, 0xe0, 0x62, 0x7c, 0x9b, 0x41, 0x6f, 0x12, 0xc3, 0xac,
0x78, 0xe4, 0xfc, 0x6b, 0x7b, 0x0a, 0xb4, 0x50, 0xd6, 0xf2, 0x45, 0xf7, 0x93,
0x04, 0xaf, 0xf2, 0xb7, 0x26, 0x94, 0xee, 0xad, 0x92, 0x05, 0x6d, 0xe5, 0xc6,
0xd6, 0x36, 0xdc, 0xa5, 0x69, 0x0f, 0xc8, 0x99, 0xf2, 0x1c, 0x4e, 0x0d,
};
m_reg_data.init_data = init_data;
std::copy(std::begin(mp_cert2), std::end(mp_cert2), reg_data.cert_data);
DEBUG_LOG(WIIMOTE, "M+ initialization data step 2 is ready.");
if (0x01 != reg_data.cert_enable)
{
PanicAlert("M+ Failure! Game requested cert2 with value other than 0x01. M+ will disconnect "
"shortly unfortunately. Reconnect wiimote and hope for the best.");
}
// A real wiimote takes about 2 seconds to reach this state:
reg_data.cert_ready = 0x1a;
INFO_LOG(WIIMOTE, "M+ cert 2 ready!");
// Note. A real M+ can take about 2 seconds to reach this state.
// Games seem to not care that we complete almost instantly.
m_reg_data.init_progress = 0x1a;
}
// TODO: make sure a motion plus report is sent first after init
// After the first two data reports it alternates between EXT and M+ data.
// Failure to read from the extension results in a fallback to M+ data.
// On real mplus:
// For some reason the first read seems to have garbage data
// is_mp_data and extension_connected are set, but the data is junk
// it does seem to have some sort of pattern though, byte 5 is always 2
// something like: d5, b0, 4e, 6e, fc, 2
// When a passthrough mode is set:
// the second read is valid mplus data, which then triggers a read from the extension
// the third read is finally extension data
// If an extension is not attached the data is always mplus data
// even when passthrough is enabled
// Real M+ seems to only ever read 6 bytes from the extension.
// Real M+ only ever reads 6 bytes from the extension which is triggered by a read at 0x00.
// Data after 6 bytes seems to be zero-filled.
// After reading, the real M+ uses that data for the next frame.
// But we are going to use it for the current frame instead.
// After reading from the EXT, the real M+ uses that data for the next frame.
// But we are going to use it for the current frame, because we can.
constexpr int EXT_AMT = 6;
// Always read from 0x52 @ 0x00:
constexpr u8 EXT_SLAVE = ExtensionPort::REPORT_I2C_SLAVE;
constexpr u8 EXT_ADDR = ExtensionPort::REPORT_I2C_ADDR;
// Try to alternate between M+ and EXT data:
DataFormat mplus_data = Common::BitCastPtr<DataFormat>(data);
mplus_data.is_mp_data ^= true;
// hax!!!
// static const u8 hacky_mp_data[6] = {0x1d, 0x91, 0x49, 0x87, 0x73, 0x7a};
// static const u8 hacky_nc_data[6] = {0x79, 0x7f, 0x4b, 0x83, 0x8b, 0xec};
// auto& hacky_ptr = mplus_data.is_mp_data ? hacky_mp_data : hacky_nc_data;
// std::copy(std::begin(hacky_ptr), std::end(hacky_ptr), data);
// return;
// If the last frame had M+ data try to send some non-M+ data:
if (!mplus_data.is_mp_data)
{
@ -293,10 +436,11 @@ void MotionPlus::Update()
}
case PassthroughMode::Nunchuk:
{
if (EXT_AMT == i2c_bus.BusRead(EXT_SLAVE, EXT_ADDR, EXT_AMT, data))
if (EXT_AMT == m_i2c_bus.BusRead(EXT_SLAVE, EXT_ADDR, EXT_AMT, data))
{
// Passthrough data modifications via wiibrew.org
// Data passing through drops the least significant bit of the three accelerometer values
// Verified on real hardware via a test of every bit.
// Data passing through drops the least significant bit of the three accelerometer values.
// Bit 7 of byte 5 is moved to bit 6 of byte 5, overwriting it
Common::SetBit(data[5], 6, Common::ExtractBit(data[5], 7));
// Bit 0 of byte 4 is moved to bit 7 of byte 5
@ -308,6 +452,8 @@ void MotionPlus::Update()
// Bit 0 of byte 5 is moved to bit 2 of byte 5, overwriting it
Common::SetBit(data[5], 2, Common::ExtractBit(data[5], 0));
mplus_data = Common::BitCastPtr<DataFormat>(data);
// Bit 0 and 1 of byte 5 contain a M+ flag and a zero bit which is set below.
mplus_data.is_mp_data = false;
}
@ -320,15 +466,18 @@ void MotionPlus::Update()
}
case PassthroughMode::Classic:
{
if (EXT_AMT == i2c_bus.BusRead(EXT_SLAVE, EXT_ADDR, EXT_AMT, data))
if (EXT_AMT == m_i2c_bus.BusRead(EXT_SLAVE, EXT_ADDR, EXT_AMT, data))
{
// Passthrough data modifications via wiibrew.org
// Verified on real hardware via a test of every bit.
// Data passing through drops the least significant bit of the axes of the left (or only)
// joystick Bit 0 of Byte 4 is overwritten [by the 'extension_connected' flag] Bits 0 and 1
// of Byte 5 are moved to bit 0 of Bytes 0 and 1, overwriting what was there before
// of Byte 5 are moved to bit 0 of Bytes 0 and 1, overwriting what was there before.
Common::SetBit(data[0], 0, Common::ExtractBit(data[5], 0));
Common::SetBit(data[1], 0, Common::ExtractBit(data[5], 1));
mplus_data = Common::BitCastPtr<DataFormat>(data);
// Bit 0 and 1 of byte 5 contain a M+ flag and a zero bit which is set below.
mplus_data.is_mp_data = false;
}
@ -340,7 +489,9 @@ void MotionPlus::Update()
break;
}
default:
PanicAlert("MotionPlus unknown passthrough-mode %d", (int)GetPassthroughMode());
// This really shouldn't happen as the M+ deactivates on an invalid mode write.
WARN_LOG(WIIMOTE, "M+ unknown passthrough-mode %d", (int)GetPassthroughMode());
mplus_data.is_mp_data = true;
break;
}
}
@ -348,18 +499,51 @@ void MotionPlus::Update()
// If the above logic determined this should be M+ data, update it here
if (mplus_data.is_mp_data)
{
// Wiibrew: "While the Wiimote is still, the values will be about 0x1F7F (8,063)"
// high-velocity range should be about +/- 1500 or 1600 dps
// low-velocity range should be about +/- 400 dps
// Wiibrew implies it shoould be +/- 595 and 2700
// These are the max referene velocities used by the sensor of the M+.
// TODO: Reverse engineer the calibration data to send perfect values.
constexpr float SLOW_MAX_RAD_PER_SEC = 440 * float(MathUtil::TAU) / 360;
constexpr float FAST_MAX_RAD_PER_SEC = 2000 * float(MathUtil::TAU) / 360;
u16 yaw_value = 0x2000;
u16 roll_value = 0x2000;
u16 pitch_value = 0x2000;
constexpr int BITS_OF_PRECISION = 14;
constexpr s32 MAX_VALUE = (1 << BITS_OF_PRECISION) - 1;
mplus_data.yaw_slow = 1;
mplus_data.roll_slow = 1;
mplus_data.pitch_slow = 1;
// constexpr u16 NEUTRAL_YAW = 0x1f66;
// constexpr u16 NEUTRAL_ROLL = 0x2058;
// constexpr u16 NEUTRAL_PITCH = 0x1fa8;
constexpr u16 NEUTRAL_YAW = 0x1f2e;
constexpr u16 NEUTRAL_ROLL = 0x1f72;
constexpr u16 NEUTRAL_PITCH = 0x1f9d;
// constexpr u16 SENSOR_NEUTRAL = (1 << (BITS_OF_PRECISION - 1));
// constexpr u16 SENSOR_NEUTRAL = 0x783a >> 2;
constexpr u16 SENSOR_RANGE = (1 << (BITS_OF_PRECISION - 1));
constexpr float SLOW_SCALE = SENSOR_RANGE / SLOW_MAX_RAD_PER_SEC;
constexpr float FAST_SCALE = SENSOR_RANGE / FAST_MAX_RAD_PER_SEC;
const float yaw = angular_velocity.z;
// TODO: verify roll signedness with our calibration data.
const float roll = angular_velocity.y;
const float pitch = angular_velocity.x;
// Slow scaling can be used if it fits in the sensor range.
mplus_data.yaw_slow = (std::abs(yaw) < SLOW_MAX_RAD_PER_SEC);
s32 yaw_value = yaw * (mplus_data.yaw_slow ? SLOW_SCALE : FAST_SCALE);
mplus_data.roll_slow = (std::abs(roll) < SLOW_MAX_RAD_PER_SEC);
s32 roll_value = roll * (mplus_data.roll_slow ? SLOW_SCALE : FAST_SCALE);
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 = MathUtil::Clamp(yaw_value + NEUTRAL_YAW, 0, MAX_VALUE);
roll_value = MathUtil::Clamp(roll_value + NEUTRAL_ROLL, 0, MAX_VALUE);
pitch_value = MathUtil::Clamp(pitch_value + NEUTRAL_PITCH, 0, MAX_VALUE);
// INFO_LOG(WIIMOTE, "M+ YAW: 0x%x slow:%d", yaw_value, mplus_data.yaw_slow);
// INFO_LOG(WIIMOTE, "M+ ROL: 0x%x slow:%d", roll_value, mplus_data.roll_slow);
// INFO_LOG(WIIMOTE, "M+ PIT: 0x%x slow:%d", pitch_value, mplus_data.pitch_slow);
// Bits 0-7
mplus_data.yaw1 = yaw_value & 0xff;
@ -372,7 +556,7 @@ void MotionPlus::Update()
mplus_data.pitch2 = pitch_value >> 8;
}
mplus_data.extension_connected = m_extension_port.IsDeviceConnected();
mplus_data.extension_connected = is_ext_connected;
mplus_data.zero = 0;
Common::BitCastPtr<DataFormat>(data) = mplus_data;

View File

@ -7,11 +7,14 @@
#include <array>
#include "Common/CommonTypes.h"
#include "Core/HW/WiimoteEmu/Dynamics.h"
#include "Core/HW/WiimoteEmu/ExtensionPort.h"
#include "Core/HW/WiimoteEmu/I2CBus.h"
namespace WiimoteEmu
{
struct AngularVelocity;
struct MotionPlus : public Extension
{
public:
@ -23,6 +26,9 @@ 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);
private:
#pragma pack(push, 1)
struct DataFormat
@ -49,16 +55,18 @@ private:
struct Register
{
u8 controller_data[21];
std::array<u8, 21> controller_data;
u8 unknown_0x15[11];
// address 0x20
u8 calibration_data[0x20];
std::array<u8, 0x20> calibration_data;
u8 unknown_0x40[0x10];
// address 0x40
// Data is read from the extension on the passthrough port.
std::array<u8, 0x10> passthrough_ext_calib;
// address 0x50
u8 cert_data[0x40];
std::array<u8, 0x40> init_data;
u8 unknown_0x90[0x60];
@ -66,33 +74,49 @@ private:
u8 initialized;
// address 0xF1
u8 cert_enable;
u8 init_stage;
// Conduit 2 writes 1 byte to 0xf2 on calibration screen
u8 unknown_0xf2[5];
// address 0xF2
// Games write 0x00 here twice to start and stop calibration.
u8 calibration_trigger;
// address 0xf7
// Wii Sports Resort reads regularly
// Value starts at 0x00 and goes up after activation (not initialization)
// Immediately returns 0x02, even still after 15 and 30 seconds
// After the first data read the value seems to progress to 0x4,0x8,0xc,0xe
// More typical seems to be 2,8,c,e
// A value of 0xe triggers the game to read 64 bytes from 0x50
// The game claims M+ is disconnected after this read of unsatisfactory data
u8 cert_ready;
// address 0xF3
u8 unknown_0xf3[3];
u8 unknown_0xf8[2];
// address 0xF6
// Value is taken from the extension on the passthrough port.
u8 passthrough_ext_id_4;
// address 0xF7
// Games read this value to know when the data at 0x50 is ready.
// Value is 0x02 upon activation.
// Real M+ changes this value from 0x4, 0x8, 0xc, and finally 0xe.
// Games then trigger a 2nd stage via a write to 0xf1.
// Real M+ changes this value to 0x14, 0x18, and finally 0x1a.
// Note: The speed of this value progression seems to be
// greatly increased by the reading of regular controller data.
// Note: We don't progress like this. We jump to the final value as soon as possible.
u8 init_progress;
// address 0xF8
// Values are taken from the extension on the passthrough port.
u8 passthrough_ext_id_0;
u8 passthrough_ext_id_5;
// address 0xFA
u8 ext_identifier[6];
std::array<u8, 6> ext_identifier;
};
#pragma pack(pop)
static_assert(sizeof(DataFormat) == 6, "Wrong size");
static_assert(0x100 == sizeof(Register));
static_assert(0x100 == sizeof(Register), "Wrong size");
static const u8 INACTIVE_DEVICE_ADDR = 0x53;
static const u8 ACTIVE_DEVICE_ADDR = 0x52;
static constexpr u8 INACTIVE_DEVICE_ADDR = 0x53;
static constexpr u8 ACTIVE_DEVICE_ADDR = 0x52;
static constexpr u8 PASSTHROUGH_MODE_OFFSET = 0xfe;
enum class PassthroughMode : u8
{
@ -101,31 +125,33 @@ private:
Classic = 0x07,
};
bool IsActive() const;
enum class ActivationStatus
{
Inactive,
Activating,
Deactivating,
Active,
};
void Activate();
void Deactivate();
void OnPassthroughModeWrite();
ActivationStatus GetActivationStatus() const;
PassthroughMode GetPassthroughMode() const;
// TODO: when activated it seems the motion plus reactivates the extension
// It sends 0x55 to 0xf0
// It also writes 0x00 to slave:0x52 addr:0xfa for some reason
// And starts a write to 0xfa but never writes bytes..
// It tries to read data at 0x00 for 3 times (failing)
// then it reads the 16 bytes of calibration at 0x20 and stops
// TODO: if an extension is attached after activation, it also does this.
int BusRead(u8 slave_addr, u8 addr, int count, u8* data_out) override;
int BusWrite(u8 slave_addr, u8 addr, int count, const u8* data_in) override;
bool ReadDeviceDetectPin() const override;
bool IsButtonPressed() const override;
// TODO: rename m_
Register m_reg_data = {};
Register reg_data = {};
u8 m_activation_progress = {};
// The port on the end of the motion plus:
I2CBus i2c_bus;
ExtensionPort m_extension_port{&i2c_bus};
I2CBus m_i2c_bus;
ExtensionPort m_extension_port{&m_i2c_bus};
};
} // namespace WiimoteEmu

View File

@ -93,6 +93,7 @@ void Wiimote::Reset()
m_eeprom.accel_calibration_1 = accel_calibration;
m_eeprom.accel_calibration_2 = accel_calibration;
// TODO: Is this needed?
// Data of unknown purpose:
constexpr std::array<u8, 24> EEPROM_DATA_16D0 = {0x00, 0x00, 0x00, 0xFF, 0x11, 0xEE, 0x00, 0x00,
0x33, 0xCC, 0x44, 0xBB, 0x00, 0x00, 0x66, 0x99,
@ -106,29 +107,29 @@ void Wiimote::Reset()
m_i2c_bus.AddSlave(&m_speaker_logic);
m_i2c_bus.AddSlave(&m_camera_logic);
// Reset extension connections:
// Reset extension connections to NONE:
m_is_motion_plus_attached = false;
m_active_extension = ExtensionNumber::NONE;
m_extension_port.AttachExtension(GetNoneExtension());
m_motion_plus.GetExtPort().AttachExtension(GetNoneExtension());
// Switch to desired M+ status and extension (if any).
// M+ and EXT are reset on attachment.
HandleExtensionSwap();
// Reset sub-devices:
// Reset sub-devices.
m_speaker_logic.Reset();
m_camera_logic.Reset();
m_motion_plus.Reset();
GetActiveExtension()->Reset();
m_status = {};
// TODO: This will suppress a status report on connect when an extension is already attached.
// I am not 100% sure if this is proper.
// This will suppress a status report on connect when an extension is already attached.
// TODO: I am not 100% sure if this is proper.
m_status.extension = m_extension_port.IsDeviceConnected();
// Dynamics:
m_swing_state = {};
m_tilt_state = {};
m_cursor_state = {};
m_shake_state = {};
}
@ -165,6 +166,8 @@ Wiimote::Wiimote(const unsigned int index) : m_index(index)
m_attachments->AddAttachment(std::make_unique<WiimoteEmu::Drums>());
m_attachments->AddAttachment(std::make_unique<WiimoteEmu::Turntable>());
m_attachments->AddSetting(&m_motion_plus_setting, {_trans("Attach MotionPlus")}, true);
// rumble
groups.emplace_back(m_rumble = new ControllerEmu::ControlGroup(_trans("Rumble")));
m_rumble->controls.emplace_back(
@ -193,8 +196,6 @@ Wiimote::Wiimote(const unsigned int index) : m_index(index)
_trans("%")},
95, 0, 100);
// m_options->AddSetting(&m_motion_plus_setting, {_trans("Attach MotionPlus")}, true);
// Note: "Upright" and "Sideways" options can be enabled at the same time which produces an
// orientation where the wiimote points towards the left with the buttons towards you.
m_options->AddSetting(&m_upright_setting,
@ -310,6 +311,7 @@ void Wiimote::UpdateButtonsStatus()
m_dpad->GetState(&m_status.buttons.hex, IsSideways() ? dpad_sideways_bitmasks : dpad_bitmasks);
}
// This is called every ::Wiimote::UPDATE_FREQ (200hz)
void Wiimote::Update()
{
// Check if connected.
@ -322,6 +324,7 @@ void Wiimote::Update()
// Data is later accessed in IsSideways and IsUpright
m_hotkeys->GetState();
// Update our motion simulations.
StepDynamics();
// Update buttons in the status struct which is sent in 99% of input reports.
@ -334,10 +337,22 @@ void Wiimote::Update()
// If a new extension is requested in the GUI the change will happen here.
HandleExtensionSwap();
// Allow extension to perform any regular duties it may need.
// (e.g. Nunchuk motion simulation step)
// Input is prepared here too.
// TODO: Separate input preparation from Update.
GetActiveExtension()->Update();
if (m_is_motion_plus_attached)
{
// M+ has some internal state that must processed.
m_motion_plus.Update();
}
// Returns true if a report was sent.
if (ProcessExtensionPortEvent())
{
// Extension port event occured.
// Extension port event occurred.
// Don't send any other reports.
return;
}
@ -403,6 +418,8 @@ void Wiimote::SendDataReport()
// IR Camera:
if (rpt_builder.HasIR())
{
// Note: Camera logic currently contains no changing state so we can just update it here.
// If that changes this should be moved to Wiimote::Update();
m_camera_logic.Update(GetTransformation());
// The real wiimote reads camera data from the i2c bus starting at offset 0x37:
@ -416,9 +433,16 @@ void Wiimote::SendDataReport()
// Extension port:
if (rpt_builder.HasExt())
{
// Update extension first as motion-plus may read from it.
GetActiveExtension()->Update();
m_motion_plus.Update();
// Prepare extension input first as motion-plus may read from it.
// This currently happens in Wiimote::Update();
// TODO: Separate extension input data preparation from Update.
// GetActiveExtension()->PrepareInput();
if (m_is_motion_plus_attached)
{
// TODO: Make input preparation triggered by bus read.
m_motion_plus.PrepareInput(GetAngularVelocity());
}
u8* ext_data = rpt_builder.GetExtDataPtr();
const u8 ext_size = rpt_builder.GetExtDataSize();
@ -658,10 +682,8 @@ void Wiimote::StepDynamics()
{
EmulateSwing(&m_swing_state, m_swing, 1.f / ::Wiimote::UPDATE_FREQ);
EmulateTilt(&m_tilt_state, m_tilt, 1.f / ::Wiimote::UPDATE_FREQ);
EmulateCursor(&m_cursor_state, m_ir, 1.f / ::Wiimote::UPDATE_FREQ);
EmulateShake(&m_shake_state, m_shake, 1.f / ::Wiimote::UPDATE_FREQ);
// TODO: Move cursor state out of ControllerEmu::Cursor
// const auto cursor_mtx = EmulateCursorMovement(m_ir);
}
Common::Vec3 Wiimote::GetAcceleration()
@ -677,6 +699,8 @@ Common::Vec3 Wiimote::GetAcceleration()
if (IsUpright())
orientation *= Common::Matrix33::RotateX(float(MathUtil::TAU / 4));
// TODO: cursor accel:
Common::Vec3 accel =
orientation *
GetTransformation().Transform(
@ -687,15 +711,31 @@ Common::Vec3 Wiimote::GetAcceleration()
return accel;
}
Common::Vec3 Wiimote::GetAngularVelocity()
{
// TODO: make cursor movement produce angular velocity.
auto orientation = Common::Matrix33::Identity();
// TODO: make a function out of this:
if (IsSideways())
orientation *= Common::Matrix33::RotateZ(float(MathUtil::TAU / -4));
if (IsUpright())
orientation *= Common::Matrix33::RotateX(float(MathUtil::TAU / 4));
return orientation * (m_tilt_state.angular_velocity + m_swing_state.angular_velocity);
}
Common::Matrix44 Wiimote::GetTransformation() const
{
// Includes positional and rotational effects of:
// IR, Swing, Tilt, Shake
// TODO: think about and clean up matrix order, make nunchuk match.
return Common::Matrix44::Translate(-m_shake_state.position) *
Common::Matrix44::FromMatrix33(GetRotationalMatrix(-m_tilt_state.angle) *
GetRotationalMatrix(-m_swing_state.angle)) *
EmulateCursorMovement(m_ir) * Common::Matrix44::Translate(-m_swing_state.position);
Common::Matrix44::FromMatrix33(GetRotationalMatrix(
-m_tilt_state.angle - m_swing_state.angle - m_cursor_state.angle)) *
Common::Matrix44::Translate(-m_swing_state.position - m_cursor_state.position);
}
} // namespace WiimoteEmu

View File

@ -137,8 +137,15 @@ private:
void UpdateButtonsStatus();
// Returns simulated accelerometer data in m/s^2.
Common::Vec3 GetAcceleration();
// Used for simulating camera data. Does not include orientation transformations.
// Returns simulated gyroscope data in radians/s.
Common::Vec3 GetAngularVelocity();
// Returns the transformation of the world around the wiimote.
// Used for simulating camera data and for rotating acceleration data.
// Does not include orientation transformations.
Common::Matrix44 GetTransformation() const;
void HIDOutputReport(const void* data, u32 size);
@ -236,7 +243,7 @@ private:
ControllerEmu::SettingValue<bool> m_upright_setting;
ControllerEmu::SettingValue<double> m_battery_setting;
ControllerEmu::SettingValue<double> m_speaker_pan_setting;
// ControllerEmu::SettingValue<bool> m_motion_plus_setting;
ControllerEmu::SettingValue<bool> m_motion_plus_setting;
SpeakerLogic m_speaker_logic;
MotionPlus m_motion_plus;
@ -267,6 +274,7 @@ private:
// Dynamics:
MotionState m_swing_state;
RotationalState m_tilt_state;
MotionState m_cursor_state;
PositionalState m_shake_state;
};
} // namespace WiimoteEmu

View File

@ -74,7 +74,7 @@ static Common::Event g_compressAndDumpStateSyncEvent;
static std::thread g_save_thread;
// Don't forget to increase this after doing changes on the savestate system
static const u32 STATE_VERSION = 108; // Last changed in PR 7870
static const u32 STATE_VERSION = 109; // Last changed in PR 7861
// Maps savestate versions to Dolphin versions.
// Versions after 42 don't need to be added to this list,