From 9554ece874094a50182d3b8e0e036b2be08c403c Mon Sep 17 00:00:00 2001
From: Jordan Woyak <jordan.woyak@gmail.com>
Date: Thu, 17 Jan 2019 09:35:08 -0600
Subject: [PATCH 1/4] WiimoteEmu: MotionPlus is now working.

---
 Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp   |  19 +-
 Source/Core/Core/HW/WiimoteEmu/Dynamics.h     |   8 +-
 .../Core/HW/WiimoteEmu/EmuSubroutines.cpp     |  20 +-
 .../Core/HW/WiimoteEmu/Extension/Nunchuk.cpp  |   3 +-
 Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp | 550 ++++++++++++------
 Source/Core/Core/HW/WiimoteEmu/MotionPlus.h   |  96 +--
 Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp |  76 ++-
 Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h   |  12 +-
 Source/Core/Core/State.cpp                    |   2 +-
 9 files changed, 533 insertions(+), 253 deletions(-)

diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
index 7fac29e9b9..49e8cb2693 100644
--- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
@@ -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,
diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h b/Source/Core/Core/HW/WiimoteEmu/Dynamics.h
index 0316c9ec34..c7483b6e91 100644
--- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.h
+++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.h
@@ -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
diff --git a/Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp b/Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp
index d1fa9bd968..ec5a595597 100644
--- a/Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp
@@ -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");
diff --git a/Source/Core/Core/HW/WiimoteEmu/Extension/Nunchuk.cpp b/Source/Core/Core/HW/WiimoteEmu/Extension/Nunchuk.cpp
index 9389ee046d..bc5feef79a 100644
--- a/Source/Core/Core/HW/WiimoteEmu/Extension/Nunchuk.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/Extension/Nunchuk.cpp
@@ -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)));
diff --git a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp
index faa4a90ff2..517643cc72 100644
--- a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp
@@ -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;
diff --git a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.h b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.h
index 06bc21b441..2814301a67 100644
--- a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.h
+++ b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.h
@@ -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
diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
index 0bbb208fde..81b16ca192 100644
--- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
@@ -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
diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
index fd505abf3d..4ba86a7217 100644
--- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
+++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
@@ -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
diff --git a/Source/Core/Core/State.cpp b/Source/Core/Core/State.cpp
index f1bd5aaed5..c0a452f63a 100644
--- a/Source/Core/Core/State.cpp
+++ b/Source/Core/Core/State.cpp
@@ -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,

From 59e1c83445bc869b3cf79e9461194faac7a64efe Mon Sep 17 00:00:00 2001
From: Jordan Woyak <jordan.woyak@gmail.com>
Date: Sat, 16 Mar 2019 17:26:14 -0500
Subject: [PATCH 2/4] WiimoteEmu/MotionPlus: Build non-hardcoded calibration
 data and other cleanups.

---
 Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp | 279 ++++++++++--------
 Source/Core/Core/HW/WiimoteEmu/MotionPlus.h   |  29 +-
 Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp |   2 +-
 3 files changed, 177 insertions(+), 133 deletions(-)

diff --git a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp
index 517643cc72..0833f68c28 100644
--- a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp
@@ -4,11 +4,14 @@
 
 #include "Core/HW/WiimoteEmu/MotionPlus.h"
 
+#include <zlib.h>
+
 #include "Common/BitUtils.h"
 #include "Common/ChunkFile.h"
 #include "Common/Logging/Log.h"
 #include "Common/MathUtil.h"
 #include "Common/MsgHandler.h"
+#include "Common/Swap.h"
 
 #include "Core/HW/Wiimote.h"
 #include "Core/HW/WiimoteEmu/Dynamics.h"
@@ -25,27 +28,67 @@ void MotionPlus::Reset()
 
   m_activation_progress = {};
 
+  // Seeing as we allow disconnection of the M+, we'll say we're not integrated.
+  // (0x00 or 0x01)
+  constexpr u8 IS_INTEGRATED = 0x00;
+
   // FYI: This ID changes on activation/deactivation
-  constexpr std::array<u8, 6> initial_id = {0x00, 0x00, 0xA6, 0x20, 0x00, 0x05};
+  constexpr std::array<u8, 6> initial_id = {IS_INTEGRATED, 0x00, 0xA6, 0x20, 0x00, 0x05};
   m_reg_data.ext_identifier = initial_id;
 
-  // 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,
+  // Build calibration data.
+
+  // Matching signedness of my real Wiimote+.
+  // This also results in all values following the "right-hand rule".
+  constexpr u16 YAW_SCALE = CALIBRATION_ZERO - CALIBRATION_SCALE_OFFSET;
+  constexpr u16 ROLL_SCALE = CALIBRATION_ZERO + CALIBRATION_SCALE_OFFSET;
+  constexpr u16 PITCH_SCALE = CALIBRATION_ZERO - CALIBRATION_SCALE_OFFSET;
+
+#pragma pack(push, 1)
+  struct CalibrationBlock
+  {
+    u16 yaw_zero = Common::swap16(CALIBRATION_ZERO);
+    u16 roll_zero = Common::swap16(CALIBRATION_ZERO);
+    u16 pitch_zero = Common::swap16(CALIBRATION_ZERO);
+    u16 yaw_scale = Common::swap16(YAW_SCALE);
+    u16 roll_scale = Common::swap16(ROLL_SCALE);
+    u16 pitch_scale = Common::swap16(PITCH_SCALE);
+    u8 degrees_div_6;
   };
-  // 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;
+
+  struct CalibrationData
+  {
+    CalibrationBlock fast;
+    u8 uid_1;
+    Common::BigEndianValue<u16> crc32_msb;
+    CalibrationBlock slow;
+    u8 uid_2;
+    Common::BigEndianValue<u16> crc32_lsb;
+  };
+#pragma pack(pop)
+
+  static_assert(sizeof(CalibrationData) == 0x20, "Bad size.");
+
+  static_assert(CALIBRATION_FAST_SCALE_DEGREES % 6 == 0, "Value aught to be divisible by 6.");
+  static_assert(CALIBRATION_SLOW_SCALE_DEGREES % 6 == 0, "Value aught to be divisible by 6.");
+
+  CalibrationData calibration;
+  calibration.fast.degrees_div_6 = CALIBRATION_FAST_SCALE_DEGREES / 6;
+  calibration.slow.degrees_div_6 = CALIBRATION_SLOW_SCALE_DEGREES / 6;
+
+  // From what I can tell, this value is only used to compare against a previously made copy.
+  // I've copied the values from my Wiimote+ just in case it's something relevant.
+  calibration.uid_1 = 0x0b;
+  calibration.uid_2 = 0xe9;
+
+  // Update checksum (crc32 of all data other than the checksum itself):
+  const auto crc_result = crc32(crc32(0, reinterpret_cast<const u8*>(&calibration), 0xe),
+                                reinterpret_cast<const u8*>(&calibration) + 0x10, 0xe);
+
+  calibration.crc32_lsb = u16(crc_result);
+  calibration.crc32_msb = u16(crc_result >> 16);
+
+  Common::BitCastPtr<CalibrationData>(m_reg_data.calibration_data.data()) = calibration;
 }
 
 void MotionPlus::DoState(PointerWrap& p)
@@ -57,7 +100,8 @@ void MotionPlus::DoState(PointerWrap& p)
 MotionPlus::ActivationStatus MotionPlus::GetActivationStatus() const
 {
   // M+ takes a bit of time to activate. During which it is completely unresponsive.
-  constexpr u8 ACTIVATION_STEPS = ::Wiimote::UPDATE_FREQ * 20 / 1000;
+  constexpr int ACTIVATION_MS = 20;
+  constexpr u8 ACTIVATION_STEPS = ::Wiimote::UPDATE_FREQ * ACTIVATION_MS / 1000;
 
   if ((ACTIVE_DEVICE_ADDR << 1) == m_reg_data.ext_identifier[2])
   {
@@ -151,39 +195,40 @@ int MotionPlus::BusWrite(u8 slave_addr, u8 addr, int count, const u8* data_in)
 
     auto const result = RawWrite(&m_reg_data, addr, count, data_in);
 
-    if (offsetof(Register, initialized) == addr)
+    switch (addr)
     {
+    case offsetof(Register, initialized):
       // 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)
-      {
-        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.");
+      break;
 
-        Deactivate();
+    case offsetof(Register, challenge_type):
+      if (CHALLENGE_X_READY == m_reg_data.challenge_progress)
+      {
+        if (0 == m_reg_data.challenge_type)
+        {
+          ERROR_LOG(WIIMOTE, "M+ parameter y0 is not yet supported! Deactivating.");
+
+          Deactivate();
+        }
+
+        m_reg_data.challenge_progress = CHALLENGE_PREPARE_Y;
       }
-    }
-    else if (offsetof(Register, calibration_trigger) == addr)
-    {
-      // Games seem to invoke this twice to start and stop. Exact consequences unknown.
+      break;
+
+    case offsetof(Register, calibration_trigger):
+      // Games seem to invoke this to start and stop calibration. Exact consequences unknown.
       DEBUG_LOG(WIIMOTE, "M+ calibration trigger: 0x%x", m_reg_data.calibration_trigger);
-    }
-    else if (PASSTHROUGH_MODE_OFFSET == addr)
-    {
+      break;
+
+    case PASSTHROUGH_MODE_OFFSET:
       // Games sometimes (not often) write zero here to deactivate the M+.
       OnPassthroughModeWrite();
+      break;
     }
 
     return result;
@@ -222,7 +267,7 @@ 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;
+  m_reg_data.challenge_progress = CHALLENGE_START;
 
   // We must do this to reset our extension_connected flag:
   m_reg_data.controller_data = {};
@@ -233,7 +278,7 @@ 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;
+  m_reg_data.challenge_progress = 0x0;
 }
 
 bool MotionPlus::ReadDeviceDetectPin() const
@@ -294,7 +339,7 @@ void MotionPlus::Update()
         {
           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(),
+          m_i2c_bus.BusWrite(ACTIVE_DEVICE_ADDR, INIT_OFFSET, int(enc_data.size()),
                              enc_data.data());
         }
 
@@ -302,7 +347,7 @@ void MotionPlus::Update()
         {
           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_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];
@@ -312,7 +357,7 @@ void MotionPlus::Update()
         {
           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(),
+                            int(m_reg_data.passthrough_ext_calib.size()),
                             m_reg_data.passthrough_ext_calib.data());
         }
       }
@@ -348,67 +393,60 @@ void MotionPlus::PrepareInput(const Common::Vec3& angular_velocity)
   // 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)
+  switch (m_reg_data.challenge_progress)
   {
-    // Activation sets init_progress to 0x2.
-    // Harness this to send some special first-time data.
+  case CHALLENGE_START:
+    // Activation starts the challenge_progress.
+    // Harness this to force non-passthrough data for the first report.
+    mplus_data.is_mp_data = true;
 
-    // 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};
+    // Note: A real M+ seems to always send some garbage/mystery data for the first report.
+    // Things seem to work without doing that so we'll just send normal data.
 
-    std::copy(std::begin(init_data), std::end(init_data), data);
+    m_reg_data.challenge_progress = CHALLENGE_PREPARE_X;
+    break;
 
-    m_reg_data.init_progress = 0x4;
-
-    return;
-  }
-  else if (0x4 == m_reg_data.init_progress)
-  {
+  case CHALLENGE_PREPARE_X:
     // 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 = {
+    // Big-int little endian parameter x.
+    m_reg_data.challenge_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.");
+    m_reg_data.challenge_progress = CHALLENGE_X_READY;
+    break;
 
-    // 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;
-  }
-  else if (0x18 == m_reg_data.init_progress)
-  {
-    // 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;
+  case CHALLENGE_PREPARE_Y:
+    if (0 == m_reg_data.challenge_type)
+    {
+      // TODO: Prepare y0.
+    }
+    else
+    {
+      // Big-int little endian parameter y1.
+      m_reg_data.challenge_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,
+      };
+    }
 
-    DEBUG_LOG(WIIMOTE, "M+ initialization data step 2 is ready.");
+    // Note. A real M+ takes about 1200ms to reach this state (for y1)
+    // (y0 is almost instant)
+    m_reg_data.challenge_progress = CHALLENGE_Y_READY;
+    break;
 
-    // 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;
+  default:
+    break;
   }
 
   // After the first two data reports it alternates between EXT and M+ data.
@@ -471,8 +509,8 @@ void MotionPlus::PrepareInput(const Common::Vec3& angular_velocity)
         // 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.
+        // 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.
         Common::SetBit(data[0], 0, Common::ExtractBit(data[5], 0));
         Common::SetBit(data[1], 0, Common::ExtractBit(data[5], 1));
 
@@ -490,70 +528,65 @@ void MotionPlus::PrepareInput(const Common::Vec3& angular_velocity)
     }
     default:
       // This really shouldn't happen as the M+ deactivates on an invalid mode write.
-      WARN_LOG(WIIMOTE, "M+ unknown passthrough-mode %d", (int)GetPassthroughMode());
+      ERROR_LOG(WIIMOTE, "M+ unknown passthrough-mode %d", int(GetPassthroughMode()));
       mplus_data.is_mp_data = true;
       break;
     }
   }
 
-  // If the above logic determined this should be M+ data, update it here
+  // If the above logic determined this should be M+ data, update it here.
   if (mplus_data.is_mp_data)
   {
-    // 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;
-
     constexpr int BITS_OF_PRECISION = 14;
+
+    // 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;
 
-    // constexpr u16 NEUTRAL_YAW = 0x1f66;
-    // constexpr u16 NEUTRAL_ROLL = 0x2058;
-    // constexpr u16 NEUTRAL_PITCH = 0x1fa8;
+    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 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;
+    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;
-    // 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);
 
+    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 = 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);
+    yaw_value = MathUtil::Clamp(yaw_value + ZERO_VALUE, 0, MAX_VALUE);
+    roll_value = MathUtil::Clamp(roll_value + ZERO_VALUE, 0, MAX_VALUE);
+    pitch_value = MathUtil::Clamp(pitch_value + ZERO_VALUE, 0, MAX_VALUE);
 
+    // TODO: Remove before merge.
     // 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;
-    mplus_data.roll1 = roll_value & 0xff;
-    mplus_data.pitch1 = pitch_value & 0xff;
+    mplus_data.yaw1 = u8(yaw_value);
+    mplus_data.roll1 = u8(roll_value);
+    mplus_data.pitch1 = u8(pitch_value);
 
     // Bits 8-13
-    mplus_data.yaw2 = yaw_value >> 8;
-    mplus_data.roll2 = roll_value >> 8;
-    mplus_data.pitch2 = pitch_value >> 8;
+    mplus_data.yaw2 = u8(yaw_value >> 8);
+    mplus_data.roll2 = u8(roll_value >> 8);
+    mplus_data.pitch2 = u8(pitch_value >> 8);
   }
 
   mplus_data.extension_connected = is_ext_connected;
diff --git a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.h b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.h
index 2814301a67..df24f2a234 100644
--- a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.h
+++ b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.h
@@ -66,7 +66,7 @@ private:
     std::array<u8, 0x10> passthrough_ext_calib;
 
     // address 0x50
-    std::array<u8, 0x40> init_data;
+    std::array<u8, 0x40> challenge_data;
 
     u8 unknown_0x90[0x60];
 
@@ -74,10 +74,11 @@ private:
     u8 initialized;
 
     // address 0xF1
-    u8 init_stage;
+    // Value is either 0 or 1.
+    u8 challenge_type;
 
     // address 0xF2
-    // Games write 0x00 here twice to start and stop calibration.
+    // Games write 0x00 here to start and stop calibration.
     u8 calibration_trigger;
 
     // address 0xF3
@@ -93,12 +94,8 @@ private:
     // 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;
+    u8 challenge_progress;
 
     // address 0xF8
     // Values are taken from the extension on the passthrough port.
@@ -109,7 +106,6 @@ private:
     std::array<u8, 6> ext_identifier;
   };
 #pragma pack(pop)
-
   static_assert(sizeof(DataFormat) == 6, "Wrong size");
   static_assert(0x100 == sizeof(Register), "Wrong size");
 
@@ -118,6 +114,21 @@ private:
 
   static constexpr u8 PASSTHROUGH_MODE_OFFSET = 0xfe;
 
+  static constexpr int CALIBRATION_BITS = 16;
+
+  // static constexpr u16 CALIBRATION_ZERO = 1 << (CALIBRATION_BITS - 1);
+  static constexpr u16 CALIBRATION_ZERO = 1 << (CALIBRATION_BITS - 1);
+  // Values are similar to that of a typical real M+.
+  static constexpr u16 CALIBRATION_SCALE_OFFSET = 0x4400;
+  static constexpr u16 CALIBRATION_FAST_SCALE_DEGREES = 0x4b0;
+  static constexpr u16 CALIBRATION_SLOW_SCALE_DEGREES = 0x10e;
+
+  static constexpr u8 CHALLENGE_START = 0x02;
+  static constexpr u8 CHALLENGE_PREPARE_X = 0x04;
+  static constexpr u8 CHALLENGE_PREPARE_Y = 0x18;
+  static constexpr u8 CHALLENGE_X_READY = 0x0e;
+  static constexpr u8 CHALLENGE_Y_READY = 0x1a;
+
   enum class PassthroughMode : u8
   {
     Disabled = 0x04,
diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
index 81b16ca192..3fdf239dda 100644
--- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
@@ -699,7 +699,7 @@ Common::Vec3 Wiimote::GetAcceleration()
   if (IsUpright())
     orientation *= Common::Matrix33::RotateX(float(MathUtil::TAU / 4));
 
-  // TODO: cursor accel:
+  // TODO: Cursor movement should produce acceleration.
 
   Common::Vec3 accel =
       orientation *

From 43746003678df348acdbee9aa866511ad9dd9bfc Mon Sep 17 00:00:00 2001
From: Jordan Woyak <jordan.woyak@gmail.com>
Date: Tue, 19 Mar 2019 19:15:17 -0500
Subject: [PATCH 3/4] WiimoteEmu: Implement MotionPlus parameter y0 and other
 cleanups.

---
 Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp   |  30 +-
 Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp | 359 +++++++++++-------
 Source/Core/Core/HW/WiimoteEmu/MotionPlus.h   |  62 +--
 Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp |  32 +-
 Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h   |   3 +
 .../Config/Mapping/MappingIndicator.cpp       |   4 +-
 .../Config/Mapping/WiimoteEmuGeneral.cpp      |   2 +-
 .../ControllerEmu/ControlGroup/Cursor.cpp     |   9 +-
 .../ControllerEmu/ControlGroup/Cursor.h       |   2 +
 9 files changed, 298 insertions(+), 205 deletions(-)

diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
index 49e8cb2693..c109e2dcf6 100644
--- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
@@ -131,8 +131,15 @@ WiimoteCommon::DataReportBuilder::AccelData ConvertAccelData(const Common::Vec3&
 
 void EmulateCursor(MotionState* state, ControllerEmu::Cursor* ir_group, float time_elapsed)
 {
-  using Common::Matrix33;
-  using Common::Matrix44;
+  const auto cursor = ir_group->GetState(true);
+
+  if (!cursor.IsVisible())
+  {
+    // Move the wiimote a kilometer forward so the sensor bar is always behind it.
+    *state = {};
+    state->position = {0, -1000, 0};
+    return;
+  }
 
   // Nintendo recommends a distance of 1-3 meters.
   constexpr float NEUTRAL_DISTANCE = 2.f;
@@ -147,21 +154,30 @@ void EmulateCursor(MotionState* state, ControllerEmu::Cursor* ir_group, float ti
   const float yaw_scale = ir_group->GetTotalYaw() / 2;
   const float pitch_scale = ir_group->GetTotalPitch() / 2;
 
-  const auto cursor = ir_group->GetState(true);
-
   // 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};
+      Common::Vec3(0, NEUTRAL_DISTANCE - MOVE_DISTANCE * float(cursor.z), -height);
+
+  const auto target_angle = Common::Vec3(pitch_scale * -cursor.y, 0, yaw_scale * -cursor.x);
+
+  // If cursor was hidden, jump to the target position/angle immediately.
+  if (state->position.y < 0)
+  {
+    state->position = new_position;
+    state->angle = target_angle;
+
+    return;
+  }
+
   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);
+  ApproachAngleWithAccel(state, target_angle, MAX_ACCEL, time_elapsed);
 }
 
 void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& angle_target,
diff --git a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp
index 0833f68c28..1afe31b2ad 100644
--- a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp
@@ -4,6 +4,10 @@
 
 #include "Core/HW/WiimoteEmu/MotionPlus.h"
 
+#include <algorithm>
+#include <cmath>
+
+#include <mbedtls/bignum.h>
 #include <zlib.h>
 
 #include "Common/BitUtils.h"
@@ -16,6 +20,39 @@
 #include "Core/HW/Wiimote.h"
 #include "Core/HW/WiimoteEmu/Dynamics.h"
 
+namespace
+{
+// Minimal wrapper mainly to handle init/free
+struct MPI : mbedtls_mpi
+{
+  explicit MPI(const char* base_10_str) : MPI() { mbedtls_mpi_read_string(this, 10, base_10_str); }
+
+  MPI() { mbedtls_mpi_init(this); }
+  ~MPI() { mbedtls_mpi_free(this); }
+
+  mbedtls_mpi* Data() { return this; };
+
+  template <std::size_t N>
+  bool ReadBinary(const u8 (&in_data)[N])
+  {
+    return 0 == mbedtls_mpi_read_binary(this, std::begin(in_data), ArraySize(in_data));
+  }
+
+  template <std::size_t N>
+  bool WriteLittleEndianBinary(std::array<u8, N>* out_data)
+  {
+    if (mbedtls_mpi_write_binary(this, out_data->data(), out_data->size()))
+      return false;
+
+    std::reverse(out_data->begin(), out_data->end());
+    return true;
+  }
+
+  MPI(const MPI&) = delete;
+  MPI& operator=(const MPI&) = delete;
+};
+}  // namespace
+
 namespace WiimoteEmu
 {
 MotionPlus::MotionPlus() : Extension("MotionPlus")
@@ -26,7 +63,7 @@ void MotionPlus::Reset()
 {
   m_reg_data = {};
 
-  m_activation_progress = {};
+  m_progress_timer = {};
 
   // Seeing as we allow disconnection of the M+, we'll say we're not integrated.
   // (0x00 or 0x01)
@@ -69,21 +106,23 @@ void MotionPlus::Reset()
 
   static_assert(sizeof(CalibrationData) == 0x20, "Bad size.");
 
-  static_assert(CALIBRATION_FAST_SCALE_DEGREES % 6 == 0, "Value aught to be divisible by 6.");
-  static_assert(CALIBRATION_SLOW_SCALE_DEGREES % 6 == 0, "Value aught to be divisible by 6.");
+  static_assert(CALIBRATION_FAST_SCALE_DEGREES % 6 == 0, "Value should be divisible by 6.");
+  static_assert(CALIBRATION_SLOW_SCALE_DEGREES % 6 == 0, "Value should be divisible by 6.");
 
   CalibrationData calibration;
   calibration.fast.degrees_div_6 = CALIBRATION_FAST_SCALE_DEGREES / 6;
   calibration.slow.degrees_div_6 = CALIBRATION_SLOW_SCALE_DEGREES / 6;
 
   // From what I can tell, this value is only used to compare against a previously made copy.
-  // I've copied the values from my Wiimote+ just in case it's something relevant.
+  // If the value matches that of the last connected wiimote which passed the "challenge",
+  // then it seems the "challenge" is not performed a second time.
   calibration.uid_1 = 0x0b;
   calibration.uid_2 = 0xe9;
 
   // Update checksum (crc32 of all data other than the checksum itself):
-  const auto crc_result = crc32(crc32(0, reinterpret_cast<const u8*>(&calibration), 0xe),
-                                reinterpret_cast<const u8*>(&calibration) + 0x10, 0xe);
+  auto crc_result = crc32(0, Z_NULL, 0);
+  crc_result = crc32(crc_result, reinterpret_cast<const Bytef*>(&calibration), 0xe);
+  crc_result = crc32(crc_result, reinterpret_cast<const Bytef*>(&calibration) + 0x10, 0xe);
 
   calibration.crc32_lsb = u16(crc_result);
   calibration.crc32_msb = u16(crc_result >> 16);
@@ -94,25 +133,21 @@ void MotionPlus::Reset()
 void MotionPlus::DoState(PointerWrap& p)
 {
   p.Do(m_reg_data);
-  p.Do(m_activation_progress);
+  p.Do(m_progress_timer);
 }
 
 MotionPlus::ActivationStatus MotionPlus::GetActivationStatus() const
 {
-  // M+ takes a bit of time to activate. During which it is completely unresponsive.
-  constexpr int ACTIVATION_MS = 20;
-  constexpr u8 ACTIVATION_STEPS = ::Wiimote::UPDATE_FREQ * ACTIVATION_MS / 1000;
-
   if ((ACTIVE_DEVICE_ADDR << 1) == m_reg_data.ext_identifier[2])
   {
-    if (m_activation_progress < ACTIVATION_STEPS)
+    if (ChallengeState::Activating == m_reg_data.challenge_state)
       return ActivationStatus::Activating;
     else
       return ActivationStatus::Active;
   }
   else
   {
-    if (m_activation_progress != 0)
+    if (m_progress_timer != 0)
       return ActivationStatus::Deactivating;
     else
       return ActivationStatus::Inactive;
@@ -174,6 +209,8 @@ int MotionPlus::BusWrite(u8 slave_addr, u8 addr, int count, const u8* data_in)
       return m_i2c_bus.BusWrite(slave_addr, addr, count, data_in);
     }
 
+    DEBUG_LOG(WIIMOTE, "Inactive M+ write 0x%x : %s", addr, ArrayToString(data_in, count).c_str());
+
     auto const result = RawWrite(&m_reg_data, addr, count, data_in);
 
     if (PASSTHROUGH_MODE_OFFSET == addr)
@@ -193,30 +230,47 @@ int MotionPlus::BusWrite(u8 slave_addr, u8 addr, int count, const u8* data_in)
       return 0;
     }
 
+    DEBUG_LOG(WIIMOTE, "Active M+ write 0x%x : %s", addr, ArrayToString(data_in, count).c_str());
+
     auto const result = RawWrite(&m_reg_data, addr, count, data_in);
 
     switch (addr)
     {
-    case offsetof(Register, initialized):
+    case offsetof(Register, init_trigger):
       // 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.
+      // The M+ deactivation signal is cleverly the same as EXT initialization.
       m_i2c_bus.BusWrite(slave_addr, addr, count, data_in);
       break;
 
     case offsetof(Register, challenge_type):
-      if (CHALLENGE_X_READY == m_reg_data.challenge_progress)
+      if (ChallengeState::ParameterXReady == m_reg_data.challenge_state)
       {
+        DEBUG_LOG(WIIMOTE, "M+ challenge: 0x%x", m_reg_data.challenge_type);
+
+        // After games read parameter x they write here to request y0 or y1.
         if (0 == m_reg_data.challenge_type)
         {
-          ERROR_LOG(WIIMOTE, "M+ parameter y0 is not yet supported! Deactivating.");
-
-          Deactivate();
+          // Preparing y0 on the real M+ is almost instant (30ms maybe).
+          constexpr int PREPARE_Y0_MS = 30;
+          m_progress_timer = ::Wiimote::UPDATE_FREQ * PREPARE_Y0_MS / 1000;
+        }
+        else
+        {
+          // A real M+ takes about 1200ms to prepare y1.
+          // Games seem to not care that we don't take that long.
+          constexpr int PREPARE_Y1_MS = 500;
+          m_progress_timer = ::Wiimote::UPDATE_FREQ * PREPARE_Y1_MS / 1000;
         }
 
-        m_reg_data.challenge_progress = CHALLENGE_PREPARE_Y;
+        // Games give the M+ a bit of time to compute the value.
+        // y0 gets about half a second.
+        // y1 gets at about 9.5 seconds.
+        // After this the M+ will fail the "challenge".
+
+        m_reg_data.challenge_state = ChallengeState::PreparingY;
       }
       break;
 
@@ -267,10 +321,16 @@ void MotionPlus::Activate()
   DEBUG_LOG(WIIMOTE, "M+ has been activated.");
 
   m_reg_data.ext_identifier[2] = ACTIVE_DEVICE_ADDR << 1;
-  m_reg_data.challenge_progress = CHALLENGE_START;
 
-  // We must do this to reset our extension_connected flag:
+  // We must do this to reset our extension_connected and is_mp_data flags:
   m_reg_data.controller_data = {};
+
+  m_reg_data.challenge_state = ChallengeState::Activating;
+
+  // M+ takes a bit of time to activate. During which it is completely unresponsive.
+  // This also affects the device detect pin which results in wiimote status reports.
+  constexpr int ACTIVATION_MS = 20;
+  m_progress_timer = ::Wiimote::UPDATE_FREQ * ACTIVATION_MS / 1000;
 }
 
 void MotionPlus::Deactivate()
@@ -278,7 +338,11 @@ void MotionPlus::Deactivate()
   DEBUG_LOG(WIIMOTE, "M+ has been deactivated.");
 
   m_reg_data.ext_identifier[2] = INACTIVE_DEVICE_ADDR << 1;
-  m_reg_data.challenge_progress = 0x0;
+
+  // M+ takes a bit of time to deactivate. During which it is completely unresponsive.
+  // This also affects the device detect pin which results in wiimote status reports.
+  constexpr int DEACTIVATION_MS = 20;
+  m_progress_timer = ::Wiimote::UPDATE_FREQ * DEACTIVATION_MS / 1000;
 }
 
 bool MotionPlus::ReadDeviceDetectPin() const
@@ -306,70 +370,134 @@ bool MotionPlus::IsButtonPressed() const
 
 void MotionPlus::Update()
 {
-  switch (GetActivationStatus())
+  if (m_progress_timer)
+    --m_progress_timer;
+
+  if (!m_progress_timer && ActivationStatus::Activating == GetActivationStatus())
   {
-  case ActivationStatus::Activating:
-    ++m_activation_progress;
-    break;
+    // M+ is active now that the timer is up.
+    m_reg_data.challenge_state = ChallengeState::PreparingX;
 
-  case ActivationStatus::Deactivating:
-    --m_activation_progress;
-    break;
+    // Games give the M+ about a minute to prepare x before failure.
+    // A real M+ can take about 1500ms.
+    // The SDK seems to have a race condition that fails if a non-ready value is not read.
+    // A necessary delay preventing challenge failure is not inserted if x is immediately ready.
+    // So we must use at least a small delay.
+    // Note: This does not delay game start. The challenge takes place in the background.
+    constexpr int PREPARE_X_MS = 500;
+    m_progress_timer = ::Wiimote::UPDATE_FREQ * PREPARE_X_MS / 1000;
+  }
 
-  case ActivationStatus::Active:
+  if (ActivationStatus::Active != GetActivationStatus())
+    return;
+
+  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)
   {
-    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)
     {
-      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
       {
-        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());
-        }
+        constexpr u8 INIT_OFFSET = offsetof(Register, init_trigger);
+        std::array<u8, 1> enc_data = {0x55};
+        m_i2c_bus.BusWrite(ACTIVE_DEVICE_ADDR, INIT_OFFSET, int(enc_data.size()), enc_data.data());
       }
 
-      // Update flag in register:
-      mplus_data.extension_connected = is_ext_connected;
-      Common::BitCastPtr<DataFormat>(data) = mplus_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;
+  }
+
+  // Only perform any of the following challenge logic if our timer is up.
+  if (m_progress_timer)
+    return;
+
+  // This is potentially any value that is less than cert_n and >= 2.
+  // A real M+ uses random values each run.
+  constexpr u8 magic[] = "DOLPHIN DOES WHAT NINTENDON'T.";
+
+  constexpr char cert_n[] =
+      "67614561104116375676885818084175632651294951727285593632649596941616763967271774525888270484"
+      "88546653264235848263182009106217734439508352645687684489830161";
+
+  constexpr char sqrt_v[] =
+      "22331959796794118515742337844101477131884013381589363004659408068948154670914705521646304758"
+      "02483462872732436570235909421331424649287229820640697259759264";
+
+  switch (m_reg_data.challenge_state)
+  {
+  case ChallengeState::PreparingX:
+  {
+    MPI param_x;
+    param_x.ReadBinary(magic);
+
+    mbedtls_mpi_mul_mpi(&param_x, &param_x, &param_x);
+    mbedtls_mpi_mod_mpi(&param_x, &param_x, MPI(cert_n).Data());
+
+    // Big-int little endian parameter x.
+    param_x.WriteLittleEndianBinary(&m_reg_data.challenge_data);
+
+    DEBUG_LOG(WIIMOTE, "M+ parameter x ready.");
+    m_reg_data.challenge_state = ChallengeState::ParameterXReady;
     break;
   }
 
+  case ChallengeState::PreparingY:
+    if (0 == m_reg_data.challenge_type)
+    {
+      MPI param_y0;
+      param_y0.ReadBinary(magic);
+
+      // Big-int little endian parameter y0.
+      param_y0.WriteLittleEndianBinary(&m_reg_data.challenge_data);
+    }
+    else
+    {
+      MPI param_y1;
+      param_y1.ReadBinary(magic);
+
+      mbedtls_mpi_mul_mpi(&param_y1, &param_y1, MPI(sqrt_v).Data());
+      mbedtls_mpi_mod_mpi(&param_y1, &param_y1, MPI(cert_n).Data());
+
+      // Big-int little endian parameter y1.
+      param_y1.WriteLittleEndianBinary(&m_reg_data.challenge_data);
+    }
+
+    DEBUG_LOG(WIIMOTE, "M+ parameter y ready.");
+    m_reg_data.challenge_state = ChallengeState::ParameterYReady;
+    break;
+
   default:
     break;
   }
@@ -384,86 +512,31 @@ void MotionPlus::PrepareInput(const Common::Vec3& angular_velocity)
 
   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.
+  // FYI: A real M+ seems to always send some garbage/mystery data for the first report,
+  // followed by a normal M+ data report, and then finally passhrough data (if enabled).
+  // Things seem to work without doing that so we'll just send normal M+ data right away.
   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;
 
-  switch (m_reg_data.challenge_progress)
-  {
-  case CHALLENGE_START:
-    // Activation starts the challenge_progress.
-    // Harness this to force non-passthrough data for the first report.
-    mplus_data.is_mp_data = true;
-
-    // Note: A real M+ seems to always send some garbage/mystery data for the first report.
-    // Things seem to work without doing that so we'll just send normal data.
-
-    m_reg_data.challenge_progress = CHALLENGE_PREPARE_X;
-    break;
-
-  case CHALLENGE_PREPARE_X:
-    // 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;
-
-    // Big-int little endian parameter x.
-    m_reg_data.challenge_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.challenge_progress = CHALLENGE_X_READY;
-    break;
-
-  case CHALLENGE_PREPARE_Y:
-    if (0 == m_reg_data.challenge_type)
-    {
-      // TODO: Prepare y0.
-    }
-    else
-    {
-      // Big-int little endian parameter y1.
-      m_reg_data.challenge_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,
-      };
-    }
-
-    // Note. A real M+ takes about 1200ms to reach this state (for y1)
-    // (y0 is almost instant)
-    m_reg_data.challenge_progress = CHALLENGE_Y_READY;
-    break;
-
-  default:
-    break;
-  }
-
-  // After the first two data reports it alternates between EXT and M+ data.
+  // After the first "garbage" report a real M+ alternates between M+ and EXT data.
   // Failure to read from the extension results in a fallback to M+ data.
-
-  // 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 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;
+  mplus_data.is_mp_data ^= true;
 
   // If the last frame had M+ data try to send some non-M+ data:
   if (!mplus_data.is_mp_data)
   {
+    // 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 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;
+
     switch (GetPassthroughMode())
     {
     case PassthroughMode::Disabled:
diff --git a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.h b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.h
index df24f2a234..07869957f7 100644
--- a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.h
+++ b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.h
@@ -30,6 +30,33 @@ public:
   void PrepareInput(const Common::Vec3& angular_velocity);
 
 private:
+  enum class ChallengeState : u8
+  {
+    // Note: This is not a value seen on a real M+.
+    // Used to emulate activation state during which the M+ is not responsive.
+    Activating = 0x00,
+
+    PreparingX = 0x02,
+    ParameterXReady = 0x0e,
+    PreparingY = 0x14,
+    ParameterYReady = 0x1a,
+  };
+
+  enum class PassthroughMode : u8
+  {
+    Disabled = 0x04,
+    Nunchuk = 0x05,
+    Classic = 0x07,
+  };
+
+  enum class ActivationStatus
+  {
+    Inactive,
+    Activating,
+    Deactivating,
+    Active,
+  };
+
 #pragma pack(push, 1)
   struct DataFormat
   {
@@ -71,7 +98,9 @@ private:
     u8 unknown_0x90[0x60];
 
     // address 0xF0
-    u8 initialized;
+    // Writes initialize the M+ to it's default (non-activated) state.
+    // Used to deactivate the M+ and activate an attached extension.
+    u8 init_trigger;
 
     // address 0xF1
     // Value is either 0 or 1.
@@ -90,12 +119,12 @@ private:
 
     // 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.
+    // Value is 0x02 upon activation. (via a write to 0xfe)
+    // Real M+ changes this value to 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: We don't progress like this. We jump to the final value as soon as possible.
-    u8 challenge_progress;
+    ChallengeState challenge_state;
 
     // address 0xF8
     // Values are taken from the extension on the passthrough port.
@@ -116,34 +145,12 @@ private:
 
   static constexpr int CALIBRATION_BITS = 16;
 
-  // static constexpr u16 CALIBRATION_ZERO = 1 << (CALIBRATION_BITS - 1);
   static constexpr u16 CALIBRATION_ZERO = 1 << (CALIBRATION_BITS - 1);
   // Values are similar to that of a typical real M+.
   static constexpr u16 CALIBRATION_SCALE_OFFSET = 0x4400;
   static constexpr u16 CALIBRATION_FAST_SCALE_DEGREES = 0x4b0;
   static constexpr u16 CALIBRATION_SLOW_SCALE_DEGREES = 0x10e;
 
-  static constexpr u8 CHALLENGE_START = 0x02;
-  static constexpr u8 CHALLENGE_PREPARE_X = 0x04;
-  static constexpr u8 CHALLENGE_PREPARE_Y = 0x18;
-  static constexpr u8 CHALLENGE_X_READY = 0x0e;
-  static constexpr u8 CHALLENGE_Y_READY = 0x1a;
-
-  enum class PassthroughMode : u8
-  {
-    Disabled = 0x04,
-    Nunchuk = 0x05,
-    Classic = 0x07,
-  };
-
-  enum class ActivationStatus
-  {
-    Inactive,
-    Activating,
-    Deactivating,
-    Active,
-  };
-
   void Activate();
   void Deactivate();
   void OnPassthroughModeWrite();
@@ -159,7 +166,8 @@ private:
 
   Register m_reg_data = {};
 
-  u8 m_activation_progress = {};
+  // Used for timing of activation, deactivation, and preparation of challenge values.
+  u8 m_progress_timer = {};
 
   // The port on the end of the motion plus:
   I2CBus m_i2c_bus;
diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
index 3fdf239dda..096247e8e6 100644
--- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
@@ -688,21 +688,10 @@ void Wiimote::StepDynamics()
 
 Common::Vec3 Wiimote::GetAcceleration()
 {
-  // Includes effects of:
-  // IR, Tilt, Swing, Orientation, Shake
-
-  auto orientation = Common::Matrix33::Identity();
-
-  if (IsSideways())
-    orientation *= Common::Matrix33::RotateZ(float(MathUtil::TAU / -4));
-
-  if (IsUpright())
-    orientation *= Common::Matrix33::RotateX(float(MathUtil::TAU / 4));
-
   // TODO: Cursor movement should produce acceleration.
 
   Common::Vec3 accel =
-      orientation *
+      GetOrientation() *
       GetTransformation().Transform(
           m_swing_state.acceleration + Common::Vec3(0, 0, float(GRAVITY_ACCELERATION)), 0);
 
@@ -713,17 +702,8 @@ Common::Vec3 Wiimote::GetAcceleration()
 
 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);
+  return GetOrientation() * (m_tilt_state.angular_velocity + m_swing_state.angular_velocity +
+                             m_cursor_state.angular_velocity);
 }
 
 Common::Matrix44 Wiimote::GetTransformation() const
@@ -738,4 +718,10 @@ Common::Matrix44 Wiimote::GetTransformation() const
          Common::Matrix44::Translate(-m_swing_state.position - m_cursor_state.position);
 }
 
+Common::Matrix33 Wiimote::GetOrientation() const
+{
+  return Common::Matrix33::RotateZ(float(MathUtil::TAU / -4 * IsSideways())) *
+         Common::Matrix33::RotateX(float(MathUtil::TAU / 4 * IsUpright()));
+}
+
 }  // namespace WiimoteEmu
diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
index 4ba86a7217..9db4e9aae2 100644
--- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
+++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.h
@@ -148,6 +148,9 @@ private:
   // Does not include orientation transformations.
   Common::Matrix44 GetTransformation() const;
 
+  // Returns the world rotation from the effects of sideways/upright settings.
+  Common::Matrix33 GetOrientation() const;
+
   void HIDOutputReport(const void* data, u32 size);
 
   void HandleReportRumble(const WiimoteCommon::OutputReportRumble&);
diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
index ca6bab91ba..e69b93c9d1 100644
--- a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
+++ b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
@@ -217,7 +217,7 @@ void MappingIndicator::DrawCursor(ControllerEmu::Cursor& cursor)
       QRectF(-scale, raw_coord.z * scale - INPUT_DOT_RADIUS / 2, scale * 2, INPUT_DOT_RADIUS));
 
   // Adjusted Z (if not hidden):
-  if (adj_coord.z && adj_coord.x < 10000)
+  if (adj_coord.IsVisible())
   {
     p.setBrush(GetAdjustedInputColor());
     p.drawRect(
@@ -250,7 +250,7 @@ void MappingIndicator::DrawCursor(ControllerEmu::Cursor& cursor)
   p.drawEllipse(QPointF{raw_coord.x, raw_coord.y} * scale, INPUT_DOT_RADIUS, INPUT_DOT_RADIUS);
 
   // Adjusted cursor position (if not hidden):
-  if (adj_coord.x < 10000)
+  if (adj_coord.IsVisible())
   {
     p.setPen(Qt::NoPen);
     p.setBrush(GetAdjustedInputColor());
diff --git a/Source/Core/DolphinQt/Config/Mapping/WiimoteEmuGeneral.cpp b/Source/Core/DolphinQt/Config/Mapping/WiimoteEmuGeneral.cpp
index 0c3ffe5deb..29ba6f19ef 100644
--- a/Source/Core/DolphinQt/Config/Mapping/WiimoteEmuGeneral.cpp
+++ b/Source/Core/DolphinQt/Config/Mapping/WiimoteEmuGeneral.cpp
@@ -55,7 +55,7 @@ void WiimoteEmuGeneral::CreateMainLayout()
 
   extension->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
 
-  static_cast<QFormLayout*>(extension->layout())->addRow(m_extension_combo);
+  static_cast<QFormLayout*>(extension->layout())->insertRow(0, m_extension_combo);
 
   layout->addWidget(extension, 0, 3);
   layout->addWidget(CreateGroupBox(tr("Rumble"), Wiimote::GetWiimoteGroup(
diff --git a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Cursor.cpp b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Cursor.cpp
index 5818f35d87..dd5bc87887 100644
--- a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Cursor.cpp
+++ b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Cursor.cpp
@@ -6,6 +6,7 @@
 
 #include <algorithm>
 #include <cmath>
+#include <limits>
 #include <memory>
 #include <string>
 
@@ -153,8 +154,7 @@ Cursor::StateData Cursor::GetState(const bool adjusted)
   // If auto-hide time is up or hide button is held:
   if (!m_auto_hide_timer || controls[6]->control_ref->State() > BUTTON_THRESHOLD)
   {
-    // TODO: Use NaN or something:
-    result.x = 10000;
+    result.x = std::numeric_limits<ControlState>::quiet_NaN();
     result.y = 0;
   }
 
@@ -176,4 +176,9 @@ ControlState Cursor::GetVerticalOffset() const
   return m_vertical_offset_setting.GetValue() / 100;
 }
 
+bool Cursor::StateData::IsVisible() const
+{
+  return !std::isnan(x);
+}
+
 }  // namespace ControllerEmu
diff --git a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Cursor.h b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Cursor.h
index 7def851cc4..4b0d13eb6c 100644
--- a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Cursor.h
+++ b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Cursor.h
@@ -20,6 +20,8 @@ public:
     ControlState x{};
     ControlState y{};
     ControlState z{};
+
+    bool IsVisible() const;
   };
 
   explicit Cursor(const std::string& name);

From ba1b3351184864ed3d5692a461b00edd2f3a21f6 Mon Sep 17 00:00:00 2001
From: Jordan Woyak <jordan.woyak@gmail.com>
Date: Sun, 7 Apr 2019 07:57:04 -0500
Subject: [PATCH 4/4] WiimoteEmu: Improve emulated swing.

---
 Source/Core/Common/MathUtil.h                 |  12 ++
 Source/Core/Common/Matrix.h                   |  54 ++++++---
 Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp   | 110 +++++++++++++-----
 Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp |   5 -
 Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp |  17 ++-
 .../Config/Mapping/MappingIndicator.cpp       |  16 ++-
 .../ControllerEmu/ControlGroup/Force.cpp      |  45 ++++---
 .../ControllerEmu/ControlGroup/Force.h        |   8 +-
 8 files changed, 199 insertions(+), 68 deletions(-)

diff --git a/Source/Core/Common/MathUtil.h b/Source/Core/Common/MathUtil.h
index bfbf0a8a77..990a5dd145 100644
--- a/Source/Core/Common/MathUtil.h
+++ b/Source/Core/Common/MathUtil.h
@@ -24,6 +24,18 @@ constexpr T Clamp(const T val, const T& min, const T& max)
   return std::max(min, std::min(max, val));
 }
 
+template <typename T>
+constexpr auto Sign(const T& val) -> decltype((T{} < val) - (val < T{}))
+{
+  return (T{} < val) - (val < T{});
+}
+
+template <typename T, typename F>
+constexpr auto Lerp(const T& x, const T& y, const F& a) -> decltype(x + (y - x) * a)
+{
+  return x + (y - x) * a;
+}
+
 template <typename T>
 constexpr bool IsPow2(T imm)
 {
diff --git a/Source/Core/Common/Matrix.h b/Source/Core/Common/Matrix.h
index f96624fef3..bf23963b3d 100644
--- a/Source/Core/Common/Matrix.h
+++ b/Source/Core/Common/Matrix.h
@@ -6,6 +6,7 @@
 
 #include <array>
 #include <cmath>
+#include <functional>
 #include <type_traits>
 
 // Tiny matrix/vector library.
@@ -58,6 +59,25 @@ union TVec3
 
   TVec3 operator-() const { return {-x, -y, -z}; }
 
+  // Apply function to each element and return the result.
+  template <typename F>
+  auto Map(F&& f) const -> TVec3<decltype(f(T{}))>
+  {
+    return {f(x), f(y), f(z)};
+  }
+
+  template <typename F, typename T2>
+  auto Map(F&& f, const TVec3<T2>& t) const -> TVec3<decltype(f(T{}, t.x))>
+  {
+    return {f(x, t.x), f(y, t.y), f(z, t.z)};
+  }
+
+  template <typename F, typename T2>
+  auto Map(F&& f, T2 scalar) const -> TVec3<decltype(f(T{}, scalar))>
+  {
+    return {f(x, scalar), f(y, scalar), f(z, scalar)};
+  }
+
   std::array<T, 3> data = {};
 
   struct
@@ -69,39 +89,45 @@ union TVec3
 };
 
 template <typename T>
-TVec3<T> operator+(TVec3<T> lhs, const TVec3<T>& rhs)
+TVec3<bool> operator<(const TVec3<T>& lhs, const TVec3<T>& rhs)
 {
-  return lhs += rhs;
+  return lhs.Map(std::less<T>{}, rhs);
 }
 
 template <typename T>
-TVec3<T> operator-(TVec3<T> lhs, const TVec3<T>& rhs)
+auto operator+(const TVec3<T>& lhs, const TVec3<T>& rhs) -> TVec3<decltype(lhs.x + rhs.x)>
 {
-  return lhs -= rhs;
+  return lhs.Map(std::plus<decltype(lhs.x + rhs.x)>{}, rhs);
 }
 
 template <typename T>
-TVec3<T> operator*(TVec3<T> lhs, const TVec3<T>& rhs)
+auto operator-(const TVec3<T>& lhs, const TVec3<T>& rhs) -> TVec3<decltype(lhs.x - rhs.x)>
 {
-  return lhs *= rhs;
+  return lhs.Map(std::minus<decltype(lhs.x - rhs.x)>{}, rhs);
+}
+
+template <typename T1, typename T2>
+auto operator*(const TVec3<T1>& lhs, const TVec3<T2>& rhs) -> TVec3<decltype(lhs.x * rhs.x)>
+{
+  return lhs.Map(std::multiplies<decltype(lhs.x * rhs.x)>{}, rhs);
 }
 
 template <typename T>
-inline TVec3<T> operator/(TVec3<T> lhs, const TVec3<T>& rhs)
+auto operator/(const TVec3<T>& lhs, const TVec3<T>& rhs) -> TVec3<decltype(lhs.x / rhs.x)>
 {
-  return lhs /= rhs;
+  return lhs.Map(std::divides<decltype(lhs.x / rhs.x)>{}, rhs);
 }
 
-template <typename T>
-TVec3<T> operator*(TVec3<T> lhs, std::common_type_t<T> scalar)
+template <typename T1, typename T2>
+auto operator*(const TVec3<T1>& lhs, T2 scalar) -> TVec3<decltype(lhs.x * scalar)>
 {
-  return lhs *= TVec3<T>{scalar, scalar, scalar};
+  return lhs.Map(std::multiplies<decltype(lhs.x * scalar)>{}, scalar);
 }
 
-template <typename T>
-TVec3<T> operator/(TVec3<T> lhs, std::common_type_t<T> scalar)
+template <typename T1, typename T2>
+auto operator/(const TVec3<T1>& lhs, T2 scalar) -> TVec3<decltype(lhs.x / scalar)>
 {
-  return lhs /= TVec3<T>{scalar, scalar, scalar};
+  return lhs.Map(std::divides<decltype(lhs.x / scalar)>{}, scalar);
 }
 
 using Vec3 = TVec3<float>;
diff --git a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
index c109e2dcf6..e4c74668fa 100644
--- a/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp
@@ -57,7 +57,7 @@ namespace WiimoteEmu
 void EmulateShake(PositionalState* state, ControllerEmu::Shake* const shake_group,
                   float time_elapsed)
 {
-  auto target_position = shake_group->GetState() * shake_group->GetIntensity() / 2;
+  auto target_position = shake_group->GetState() * float(shake_group->GetIntensity() / 2);
   for (std::size_t i = 0; i != target_position.data.size(); ++i)
   {
     if (state->velocity.data[i] * std::copysign(1.f, target_position.data[i]) < 0 ||
@@ -90,7 +90,9 @@ void EmulateTilt(RotationalState* state, ControllerEmu::Tilt* const tilt_group,
   const ControlState roll = target.x * MathUtil::PI;
   const ControlState pitch = target.y * MathUtil::PI;
 
-  // TODO: expose this setting in UI:
+  // Higher values will be more responsive but will increase rate of M+ "desync".
+  // I'd rather not expose this value in the UI if not needed.
+  // Desync caused by tilt seems not as severe as accelerometer data can estimate pitch/yaw.
   constexpr auto MAX_ACCEL = float(MathUtil::TAU * 50);
 
   ApproachAngleWithAccel(state, Common::Vec3(pitch, -roll, 0), MAX_ACCEL, time_elapsed);
@@ -98,22 +100,80 @@ void EmulateTilt(RotationalState* state, ControllerEmu::Tilt* const tilt_group,
 
 void EmulateSwing(MotionState* state, ControllerEmu::Force* swing_group, float time_elapsed)
 {
-  const auto target = swing_group->GetState();
+  const auto input_state = swing_group->GetState();
+  const float max_distance = swing_group->GetMaxDistance();
+  const float max_angle = swing_group->GetTwistAngle();
 
-  // Note. Y/Z swapped because X/Y axis to the swing_group is X/Z to the wiimote.
+  // Note: Y/Z swapped because X/Y axis to the swing_group is X/Z to the wiimote.
   // X is negated because Wiimote X+ is to the left.
-  ApproachPositionWithJerk(state, {-target.x, -target.z, target.y},
-                           Common::Vec3{1, 1, 1} * swing_group->GetMaxJerk(), time_elapsed);
+  const auto target_position = Common::Vec3{-input_state.x, -input_state.z, input_state.y};
 
-  // Just jump to our target angle scaled by our progress to the target position.
-  // TODO: If we wanted to be less hacky we could use ApproachAngleWithAccel.
-  const auto angle = state->position / swing_group->GetMaxDistance() * swing_group->GetTwistAngle();
+  // Jerk is scaled based on input distance from center.
+  // X and Z scale is connected for sane movement about the circle.
+  const auto xz_target_dist = Common::Vec2{target_position.x, target_position.z}.Length();
+  const auto y_target_dist = std::abs(target_position.y);
+  const auto target_dist = Common::Vec3{xz_target_dist, y_target_dist, xz_target_dist};
+  const auto speed = MathUtil::Lerp(Common::Vec3{1, 1, 1} * float(swing_group->GetReturnSpeed()),
+                                    Common::Vec3{1, 1, 1} * float(swing_group->GetSpeed()),
+                                    target_dist / max_distance);
 
-  const auto old_angle = state->angle;
-  state->angle = {-angle.z, 0, angle.x};
+  // Convert our m/s "speed" to the jerk required to reach this speed when traveling 1 meter.
+  const auto max_jerk = speed * speed * speed * 4;
 
-  // Update velocity based on change in angle.
-  state->angular_velocity = state->angle - old_angle;
+  // Rotational acceleration to approximately match the completion time of our swing.
+  const auto max_accel = max_angle * speed.x * speed.x;
+
+  // Apply rotation based on amount of swing.
+  const auto target_angle =
+      Common::Vec3{-target_position.z, 0, target_position.x} / max_distance * max_angle;
+
+  // Angular acceleration * 2 seems to reduce "spurious stabs" in ZSS.
+  // TODO: Fix properly.
+  ApproachAngleWithAccel(state, target_angle, max_accel * 2, time_elapsed);
+
+  // Clamp X and Z rotation.
+  for (const int c : {0, 2})
+  {
+    if (std::abs(state->angle.data[c] / max_angle) > 1 &&
+        MathUtil::Sign(state->angular_velocity.data[c]) == MathUtil::Sign(state->angle.data[c]))
+    {
+      state->angular_velocity.data[c] = 0;
+    }
+  }
+
+  // Adjust target position backwards based on swing progress and max angle
+  // to simulate a swing with an outstretched arm.
+  const auto backwards_angle = std::max(std::abs(state->angle.x), std::abs(state->angle.z));
+  const auto backwards_movement = (1 - std::cos(backwards_angle)) * max_distance;
+
+  // TODO: Backswing jerk should be based on x/z speed.
+
+  ApproachPositionWithJerk(state, target_position + Common::Vec3{0, backwards_movement, 0},
+                           max_jerk, time_elapsed);
+
+  // Clamp Left/Right/Up/Down movement within the configured circle.
+  const auto xz_progress =
+      Common::Vec2{state->position.x, state->position.z}.Length() / max_distance;
+  if (xz_progress > 1)
+  {
+    state->position.x /= xz_progress;
+    state->position.z /= xz_progress;
+
+    state->acceleration.x = state->acceleration.z = 0;
+    state->velocity.x = state->velocity.z = 0;
+  }
+
+  // Clamp Forward/Backward movement within the configured distance.
+  // We allow additional backwards movement for the back swing.
+  const auto y_progress = state->position.y / max_distance;
+  const auto max_y_progress = 2 - std::cos(max_angle);
+  if (y_progress > max_y_progress || y_progress < -1)
+  {
+    state->position.y =
+        MathUtil::Clamp(state->position.y, -1.f * max_distance, max_y_progress * max_distance);
+    state->velocity.y = 0;
+    state->acceleration.y = 0;
+  }
 }
 
 WiimoteCommon::DataReportBuilder::AccelData ConvertAccelData(const Common::Vec3& accel, u16 zero_g,
@@ -174,8 +234,10 @@ void EmulateCursor(MotionState* state, ControllerEmu::Cursor* ir_group, float ti
   state->acceleration = new_position - state->position;
   state->position = new_position;
 
-  // TODO: expose this setting in UI:
-  constexpr auto MAX_ACCEL = float(MathUtil::TAU * 100);
+  // Higher values will be more responsive but increase rate of M+ "desync".
+  // I'd rather not expose this value in the UI if not needed.
+  // At this value, sync is very good and responsiveness still appears instant.
+  constexpr auto MAX_ACCEL = float(MathUtil::TAU * 8);
 
   ApproachAngleWithAccel(state, target_angle, MAX_ACCEL, time_elapsed);
 }
@@ -190,10 +252,7 @@ void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& angle_ta
 
   const auto offset = angle_target - state->angle;
   const auto stop_offset = offset - stop_distance;
-
-  const Common::Vec3 accel{std::copysign(max_accel, stop_offset.x),
-                           std::copysign(max_accel, stop_offset.y),
-                           std::copysign(max_accel, stop_offset.z)};
+  const auto accel = MathUtil::Sign(stop_offset) * max_accel;
 
   state->angular_velocity += accel * time_elapsed;
 
@@ -202,11 +261,11 @@ void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& angle_ta
 
   for (std::size_t i = 0; i != offset.data.size(); ++i)
   {
-    // If new velocity will overshoot assume we would have stopped right on target.
-    // TODO: Improve check to see if less accel would have caused undershoot.
-    if ((change_in_angle.data[i] / offset.data[i]) > 1.0)
+    // If new angle will overshoot stop right on target.
+    if (std::abs(offset.data[i]) < 0.0001 || (change_in_angle.data[i] / offset.data[i] > 1.0))
     {
-      state->angular_velocity.data[i] = 0;
+      state->angular_velocity.data[i] =
+          (angle_target.data[i] - state->angle.data[i]) / time_elapsed;
       state->angle.data[i] = angle_target.data[i];
     }
     else
@@ -226,10 +285,7 @@ void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& positi
 
   const auto offset = position_target - state->position;
   const auto stop_offset = offset - stop_distance;
-
-  const Common::Vec3 jerk{std::copysign(max_jerk.x, stop_offset.x),
-                          std::copysign(max_jerk.y, stop_offset.y),
-                          std::copysign(max_jerk.z, stop_offset.z)};
+  const auto jerk = MathUtil::Sign(stop_offset) * max_jerk;
 
   state->acceleration += jerk * time_elapsed;
 
diff --git a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp
index 1afe31b2ad..de5e55f69e 100644
--- a/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp
@@ -646,11 +646,6 @@ void MotionPlus::PrepareInput(const Common::Vec3& angular_velocity)
     roll_value = MathUtil::Clamp(roll_value + ZERO_VALUE, 0, MAX_VALUE);
     pitch_value = MathUtil::Clamp(pitch_value + ZERO_VALUE, 0, MAX_VALUE);
 
-    // TODO: Remove before merge.
-    // 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 = u8(yaw_value);
     mplus_data.roll1 = u8(roll_value);
diff --git a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
index 096247e8e6..6d24824608 100644
--- a/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
+++ b/Source/Core/Core/HW/WiimoteEmu/WiimoteEmu.cpp
@@ -688,15 +688,28 @@ void Wiimote::StepDynamics()
 
 Common::Vec3 Wiimote::GetAcceleration()
 {
-  // TODO: Cursor movement should produce acceleration.
+  // TODO: Cursor forward/backward movement should produce acceleration.
 
   Common::Vec3 accel =
       GetOrientation() *
       GetTransformation().Transform(
           m_swing_state.acceleration + Common::Vec3(0, 0, float(GRAVITY_ACCELERATION)), 0);
 
+  // Our shake effects have never been affected by orientation. Should they be?
   accel += m_shake_state.acceleration;
 
+  // Simulate centripetal acceleration caused by an offset of the accelerometer sensor.
+  // Estimate of sensor position based on an image of the wii remote board:
+  constexpr float ACCELEROMETER_Y_OFFSET = 0.1f;
+
+  const auto angular_velocity = GetAngularVelocity();
+  const auto centripetal_accel =
+      // TODO: Is this the proper way to combine the x and z angular velocities?
+      std::pow(std::abs(angular_velocity.x) + std::abs(angular_velocity.z), 2) *
+      ACCELEROMETER_Y_OFFSET;
+
+  accel.y += centripetal_accel;
+
   return accel;
 }
 
@@ -709,7 +722,7 @@ Common::Vec3 Wiimote::GetAngularVelocity()
 Common::Matrix44 Wiimote::GetTransformation() const
 {
   // Includes positional and rotational effects of:
-  // IR, Swing, Tilt, Shake
+  // Cursor, Swing, Tilt, Shake
 
   // TODO: think about and clean up matrix order, make nunchuk match.
   return Common::Matrix44::Translate(-m_shake_state.position) *
diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
index e69b93c9d1..2cd768feea 100644
--- a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
+++ b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
@@ -492,11 +492,19 @@ void MappingIndicator::DrawForce(ControllerEmu::Force& force)
       QRectF(-scale, raw_coord.z * scale - INPUT_DOT_RADIUS / 2, scale * 2, INPUT_DOT_RADIUS));
 
   // Adjusted Z:
-  if (adj_coord.y)
+  const auto curve_point =
+      std::max(std::abs(m_motion_state.angle.x), std::abs(m_motion_state.angle.z)) / MathUtil::TAU;
+  if (adj_coord.y || curve_point)
   {
-    p.setBrush(GetAdjustedInputColor());
-    p.drawRect(
-        QRectF(-scale, adj_coord.y * -scale - INPUT_DOT_RADIUS / 2, scale * 2, INPUT_DOT_RADIUS));
+    // Show off the angle somewhat with a curved line.
+    QPainterPath path;
+    path.moveTo(-scale, (adj_coord.y + curve_point) * -scale);
+    path.quadTo({0, (adj_coord.y - curve_point) * -scale},
+                {scale, (adj_coord.y + curve_point) * -scale});
+
+    p.setBrush(Qt::NoBrush);
+    p.setPen(QPen(GetAdjustedInputColor(), INPUT_DOT_RADIUS));
+    p.drawPath(path);
   }
 
   // Draw "gate" shape.
diff --git a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.cpp b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.cpp
index 93a9d06cb3..7ddbeba335 100644
--- a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.cpp
+++ b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.cpp
@@ -31,16 +31,30 @@ Force::Force(const std::string& name_) : ReshapableInput(name_, name_, GroupType
               _trans("cm"),
               // i18n: Refering to emulated wii remote swing movement.
               _trans("Distance of travel from neutral position.")},
-             25, 0, 100);
+             50, 1, 100);
 
-  AddSetting(&m_jerk_setting,
-             // i18n: "Jerk" as it relates to physics. The time derivative of acceleration.
-             {_trans("Jerk"),
-              // i18n: The symbol/abbreviation for meters per second to the 3rd power.
-              _trans("m/s³"),
+  // These speed settings are used to calculate a maximum jerk (change in acceleration).
+  // The calculation uses a travel distance of 1 meter.
+  // The maximum value of 40 m/s is the approximate speed of the head of a golf club.
+  // Games seem to not even properly detect motions at this speed.
+  // Values result in an exponentially increasing jerk.
+
+  AddSetting(&m_speed_setting,
+             {_trans("Speed"),
+              // i18n: The symbol/abbreviation for meters per second.
+              _trans("m/s"),
               // i18n: Refering to emulated wii remote swing movement.
-              _trans("Maximum change in acceleration.")},
-             500, 1, 1000);
+              _trans("Peak velocity of outward swing movements.")},
+             16, 1, 40);
+
+  // "Return Speed" allows for a "slow return" that won't trigger additional actions.
+  AddSetting(&m_return_speed_setting,
+             {_trans("Return Speed"),
+              // i18n: The symbol/abbreviation for meters per second.
+              _trans("m/s"),
+              // i18n: Refering to emulated wii remote swing movement.
+              _trans("Peak velocity of movements to neutral position.")},
+             2, 1, 40);
 
   AddSetting(&m_angle_setting,
              {_trans("Angle"),
@@ -48,7 +62,7 @@ Force::Force(const std::string& name_) : ReshapableInput(name_, name_, GroupType
               _trans("°"),
               // i18n: Refering to emulated wii remote swing movement.
               _trans("Rotation applied at extremities of swing.")},
-             45, 0, 180);
+             90, 1, 180);
 }
 
 Force::ReshapeData Force::GetReshapableState(bool adjusted)
@@ -70,8 +84,8 @@ Force::StateData Force::GetState(bool adjusted)
 
   if (adjusted)
   {
-    // Apply deadzone to z.
-    z = ApplyDeadzone(z, GetDeadzonePercentage());
+    // Apply deadzone to z and scale.
+    z = ApplyDeadzone(z, GetDeadzonePercentage()) * GetMaxDistance();
   }
 
   return {float(state.x), float(state.y), float(z)};
@@ -83,9 +97,14 @@ ControlState Force::GetGateRadiusAtAngle(double) const
   return GetMaxDistance();
 }
 
-ControlState Force::GetMaxJerk() const
+ControlState Force::GetSpeed() const
 {
-  return m_jerk_setting.GetValue();
+  return m_speed_setting.GetValue();
+}
+
+ControlState Force::GetReturnSpeed() const
+{
+  return m_return_speed_setting.GetValue();
 }
 
 ControlState Force::GetTwistAngle() const
diff --git a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.h b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.h
index da66dc034e..ae9cabe340 100644
--- a/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.h
+++ b/Source/Core/InputCommon/ControllerEmu/ControlGroup/Force.h
@@ -26,8 +26,9 @@ public:
 
   StateData GetState(bool adjusted = true);
 
-  // Return jerk in m/s^3.
-  ControlState GetMaxJerk() const;
+  // Velocities returned in m/s.
+  ControlState GetSpeed() const;
+  ControlState GetReturnSpeed() const;
 
   // Return twist angle in radians.
   ControlState GetTwistAngle() const;
@@ -37,7 +38,8 @@ public:
 
 private:
   SettingValue<double> m_distance_setting;
-  SettingValue<double> m_jerk_setting;
+  SettingValue<double> m_speed_setting;
+  SettingValue<double> m_return_speed_setting;
   SettingValue<double> m_angle_setting;
 };