WiimoteEmu/MotionPlus: Build non-hardcoded calibration data and other cleanups.

3 files changed, 175 insertions, 131 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;
+  };
+
+  struct CalibrationData
+  {
+    CalibrationBlock fast;
+    u8 uid_1;
+    Common::BigEndianValue<u16> crc32_msb;
+    CalibrationBlock slow;
+    u8 uid_2;
+    Common::BigEndianValue<u16> crc32_lsb;
   };
-  // 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;
+#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
+      break;
+
+    case offsetof(Register, challenge_type):
+      if (CHALLENGE_X_READY == m_reg_data.challenge_progress)
       {
-        // 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.");
+        if (0 == m_reg_data.challenge_type)
+        {
+          ERROR_LOG(WIIMOTE, "M+ parameter y0 is not yet supported! Deactivating.");
 
-        Deactivate();
+          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.
-
-    // 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};
+  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;
 
-    std::copy(std::begin(init_data), std::end(init_data), data);
+    // 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.init_progress = 0x4;
+    m_reg_data.challenge_progress = CHALLENGE_PREPARE_X;
+    break;
 
-    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;
-    constexpr s32 MAX_VALUE = (1 << BITS_OF_PRECISION) - 1;
 
-    // constexpr u16 NEUTRAL_YAW = 0x1f66;
-    // constexpr u16 NEUTRAL_ROLL = 0x2058;
-    // constexpr u16 NEUTRAL_PITCH = 0x1fa8;
+    // 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 u16 NEUTRAL_YAW = 0x1f2e;
-    constexpr u16 NEUTRAL_ROLL = 0x1f72;
-    constexpr u16 NEUTRAL_PITCH = 0x1f9d;
+    constexpr float SLOW_SCALE = VALUE_SCALE / CALIBRATION_SLOW_SCALE_DEGREES;
+    constexpr float FAST_SCALE = VALUE_SCALE / CALIBRATION_FAST_SCALE_DEGREES;
 
-    // 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 s32 ZERO_VALUE = CALIBRATION_ZERO >> (CALIBRATION_BITS - BITS_OF_PRECISION);
+    constexpr s32 MAX_VALUE = (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;
+    static_assert(ZERO_VALUE == 1 << (BITS_OF_PRECISION - 1),
+                  "SLOW_MAX_RAD_PER_SEC assumes calibrated zero is at center of sensor values.");
 
-    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;
+    constexpr u16 SENSOR_RANGE = 1 << (BITS_OF_PRECISION - 1);
+    constexpr float SLOW_MAX_RAD_PER_SEC = SENSOR_RANGE / SLOW_SCALE;
 
-    // Slow scaling can be used if it fits in the sensor range.
+    // Slow (high precision) scaling can be used if it fits in the sensor range.
+    const float yaw = angular_velocity.z;
     mplus_data.yaw_slow = (std::abs(yaw) < SLOW_MAX_RAD_PER_SEC);
     s32 yaw_value = yaw * (mplus_data.yaw_slow ? SLOW_SCALE : FAST_SCALE);
 
+    const float roll = angular_velocity.y;
     mplus_data.roll_slow = (std::abs(roll) < SLOW_MAX_RAD_PER_SEC);
     s32 roll_value = roll * (mplus_data.roll_slow ? SLOW_SCALE : FAST_SCALE);
 
+    const float pitch = angular_velocity.x;
     mplus_data.pitch_slow = (std::abs(pitch) < SLOW_MAX_RAD_PER_SEC);
     s32 pitch_value = pitch * (mplus_data.pitch_slow ? SLOW_SCALE : FAST_SCALE);
 
-    yaw_value = 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 *