Merge pull request #8575 from jordan-woyak/ciface-wiimotes

InputCommon: Add support for Wii Remotes in ControllerInterface

Source/Core/Common/BitUtils.h

@@ -300,6 +300,12 @@ void SetBit(T& value, size_t bit_number, bool bit_value)
     value &= ~(T{1} << bit_number);
 }
 
+template <size_t bit_number, typename T>
+void SetBit(T& value, bool bit_value)
+{
+  SetBit(value, bit_number, bit_value);
+}
+
 template <typename T>
 class FlagBit
 {
@@ -340,4 +346,15 @@ public:
   std::underlying_type_t<T> m_hex = 0;
 };
 
+// Left-shift a value and set new LSBs to that of the supplied LSB.
+// Converts a value from a N-bit range to an (N+X)-bit range. e.g. 0x101 -> 0x10111
+template <typename T>
+T ExpandValue(T value, size_t left_shift_amount)
+{
+  static_assert(std::is_unsigned<T>(), "ExpandValue is only sane on unsigned types.");
+
+  return (value << left_shift_amount) |
+         (T(-ExtractBit<0>(value)) >> (BitSize<T>() - left_shift_amount));
+}
+
 }  // namespace Common

Source/Core/Common/MathUtil.h

@@ -5,6 +5,7 @@
 #pragma once
 
 #include <algorithm>
+#include <cmath>
 #include <vector>
 
 #include "Common/CommonTypes.h"
@@ -93,6 +94,45 @@ struct Rectangle
   }
 };
 
+template <typename T>
+class RunningMean
+{
+public:
+  constexpr void Clear() { *this = {}; }
+
+  constexpr void Push(T x) { m_mean = m_mean + (x - m_mean) / ++m_count; }
+
+  constexpr size_t Count() const { return m_count; }
+  constexpr T Mean() const { return m_mean; }
+
+private:
+  size_t m_count = 0;
+  T m_mean{};
+};
+
+template <typename T>
+class RunningVariance
+{
+public:
+  constexpr void Clear() { *this = {}; }
+
+  constexpr void Push(T x)
+  {
+    const auto old_mean = m_running_mean.Mean();
+    m_running_mean.Push(x);
+    m_variance += (x - old_mean) * (x - m_running_mean.Mean());
+  }
+
+  constexpr size_t Count() const { return m_running_mean.Count(); }
+  constexpr T Mean() const { return m_running_mean.Mean(); }
+  constexpr T Variance() const { return m_variance / (Count() - 1); }
+  constexpr T StandardDeviation() const { return std::sqrt(Variance()); }
+
+private:
+  RunningMean<T> m_running_mean;
+  T m_variance{};
+};
+
 }  // namespace MathUtil
 
 float MathFloatVectorSum(const std::vector<float>&);

Source/Core/Common/Matrix.h

@@ -20,6 +20,11 @@ union TVec3
   TVec3() = default;
   TVec3(T _x, T _y, T _z) : data{_x, _y, _z} {}
 
+  template <typename OtherT>
+  explicit TVec3(const TVec3<OtherT>& other) : TVec3(other.x, other.y, other.z)
+  {
+  }
+
   TVec3 Cross(const TVec3& rhs) const
   {
     return {(y * rhs.z) - (rhs.y * z), (z * rhs.x) - (rhs.z * x), (x * rhs.y) - (rhs.x * y)};
@@ -98,6 +103,11 @@ TVec3<bool> operator<(const TVec3<T>& lhs, const TVec3<T>& rhs)
   return lhs.Map(std::less<T>{}, rhs);
 }
 
+inline TVec3<bool> operator!(const TVec3<bool>& vec)
+{
+  return {!vec.x, !vec.y, !vec.z};
+}
+
 template <typename T>
 auto operator+(const TVec3<T>& lhs, const TVec3<T>& rhs) -> TVec3<decltype(lhs.x + rhs.x)>
 {
@@ -197,6 +207,11 @@ union TVec2
   TVec2() = default;
   TVec2(T _x, T _y) : data{_x, _y} {}
 
+  template <typename OtherT>
+  explicit TVec2(const TVec2<OtherT>& other) : TVec2(other.x, other.y)
+  {
+  }
+
   T Cross(const TVec2& rhs) const { return (x * rhs.y) - (y * rhs.x); }
   T Dot(const TVec2& rhs) const { return (x * rhs.x) + (y * rhs.y); }
   T LengthSquared() const { return Dot(*this); }
@@ -217,6 +232,20 @@ union TVec2
     return *this;
   }
 
+  TVec2& operator*=(const TVec2& rhs)
+  {
+    x *= rhs.x;
+    y *= rhs.y;
+    return *this;
+  }
+
+  TVec2& operator/=(const TVec2& rhs)
+  {
+    x /= rhs.x;
+    y /= rhs.y;
+    return *this;
+  }
+
   TVec2& operator*=(T scalar)
   {
     x *= scalar;
@@ -242,6 +271,17 @@ union TVec2
   };
 };
 
+template <typename T>
+TVec2<bool> operator<(const TVec2<T>& lhs, const TVec2<T>& rhs)
+{
+  return {lhs.x < rhs.x, lhs.y < rhs.y};
+}
+
+inline TVec2<bool> operator!(const TVec2<bool>& vec)
+{
+  return {!vec.x, !vec.y};
+}
+
 template <typename T>
 TVec2<T> operator+(TVec2<T> lhs, const TVec2<T>& rhs)
 {
@@ -255,15 +295,27 @@ TVec2<T> operator-(TVec2<T> lhs, const TVec2<T>& rhs)
 }
 
 template <typename T>
-TVec2<T> operator*(TVec2<T> lhs, T scalar)
+TVec2<T> operator*(TVec2<T> lhs, const TVec2<T>& rhs)
 {
-  return lhs *= scalar;
+  return lhs *= rhs;
 }
 
 template <typename T>
-TVec2<T> operator/(TVec2<T> lhs, T scalar)
+TVec2<T> operator/(TVec2<T> lhs, const TVec2<T>& rhs)
 {
-  return lhs /= scalar;
+  return lhs /= rhs;
+}
+
+template <typename T, typename T2>
+auto operator*(TVec2<T> lhs, T2 scalar)
+{
+  return TVec2<decltype(lhs.x * scalar)>(lhs) *= scalar;
+}
+
+template <typename T, typename T2>
+auto operator/(TVec2<T> lhs, T2 scalar)
+{
+  return TVec2<decltype(lhs.x / scalar)>(lhs) /= scalar;
 }
 
 using Vec2 = TVec2<float>;

Source/Core/Core/ConfigManager.cpp

@@ -234,6 +234,7 @@ void SConfig::SaveCoreSettings(IniFile& ini)
   core->Set("WiiKeyboard", m_WiiKeyboard);
   core->Set("WiimoteContinuousScanning", m_WiimoteContinuousScanning);
   core->Set("WiimoteEnableSpeaker", m_WiimoteEnableSpeaker);
+  core->Set("WiimoteControllerInterface", connect_wiimotes_for_ciface);
   core->Set("RunCompareServer", bRunCompareServer);
   core->Set("RunCompareClient", bRunCompareClient);
   core->Set("MMU", bMMU);
@@ -511,6 +512,7 @@ void SConfig::LoadCoreSettings(IniFile& ini)
   core->Get("WiiKeyboard", &m_WiiKeyboard, false);
   core->Get("WiimoteContinuousScanning", &m_WiimoteContinuousScanning, false);
   core->Get("WiimoteEnableSpeaker", &m_WiimoteEnableSpeaker, false);
+  core->Get("WiimoteControllerInterface", &connect_wiimotes_for_ciface, false);
   core->Get("RunCompareServer", &bRunCompareServer, false);
   core->Get("RunCompareClient", &bRunCompareClient, false);
   core->Get("MMU", &bMMU, bMMU);

Source/Core/Core/ConfigManager.h

@@ -72,6 +72,7 @@ struct SConfig
   bool m_WiiKeyboard;
   bool m_WiimoteContinuousScanning;
   bool m_WiimoteEnableSpeaker;
+  bool connect_wiimotes_for_ciface;
 
   // ISO folder
   std::vector<std::string> m_ISOFolder;

Source/Core/Core/HW/WiimoteCommon/DataReport.cpp

@@ -5,6 +5,7 @@
 #include <cassert>
 
 #include "Common/BitUtils.h"
+#include "Common/MathUtil.h"
 #include "Core/HW/WiimoteCommon/DataReport.h"
 
 namespace WiimoteCommon
@@ -75,40 +76,35 @@ struct IncludeAccel : virtual DataReportManipulator
   void GetAccelData(AccelData* result) const override
   {
     const AccelMSB accel = Common::BitCastPtr<AccelMSB>(data_ptr + 2);
-    result->x = accel.x << 2;
-    result->y = accel.y << 2;
-    result->z = accel.z << 2;
-
-    // LSBs
     const CoreData core = Common::BitCastPtr<CoreData>(data_ptr);
-    result->x |= core.acc_bits & 0b11;
-    result->y |= (core.acc_bits2 & 0b1) << 1;
-    result->z |= core.acc_bits2 & 0b10;
+
+    // X has 10 bits of precision.
+    result->value.x = accel.x << 2;
+    result->value.x |= core.acc_bits & 0b11;
+
+    // Y and Z only have 9 bits of precision. (convert them to 10)
+    result->value.y =
+        Common::ExpandValue<u16>(accel.y << 1 | Common::ExtractBit<0>(core.acc_bits2), 1);
+    result->value.z =
+        Common::ExpandValue<u16>(accel.z << 1 | Common::ExtractBit<1>(core.acc_bits2), 1);
   }
 
   void SetAccelData(const AccelData& new_accel) override
   {
-    AccelMSB accel = {};
-    accel.x = new_accel.x >> 2;
-    accel.y = new_accel.y >> 2;
-    accel.z = new_accel.z >> 2;
-    Common::BitCastPtr<AccelMSB>(data_ptr + 2) = accel;
+    Common::BitCastPtr<AccelMSB>(data_ptr + 2) = AccelMSB(new_accel.value / 4);
 
     // LSBs
     CoreData core = Common::BitCastPtr<CoreData>(data_ptr);
-    core.acc_bits = (new_accel.x >> 0) & 0b11;
-    core.acc_bits2 = (new_accel.y >> 1) & 0x1;
-    core.acc_bits2 |= (new_accel.z & 0xb10);
+    core.acc_bits = (new_accel.value.x >> 0) & 0b11;
+    core.acc_bits2 = (new_accel.value.y >> 1) & 0x1;
+    core.acc_bits2 |= (new_accel.value.z & 0xb10);
     Common::BitCastPtr<CoreData>(data_ptr) = core;
   }
 
   bool HasAccel() const override { return true; }
 
 private:
-  struct AccelMSB
-  {
-    u8 x, y, z;
-  };
+  using AccelMSB = Common::TVec3<u8>;
   static_assert(sizeof(AccelMSB) == 3, "Wrong size");
 };
 
@@ -195,26 +191,28 @@ struct ReportExt21 : NoCore, NoAccel, NoIR, IncludeExt<0, 21>
 struct ReportInterleave1 : IncludeCore, IncludeIR<3, 18, 0>, NoExt
 {
   // FYI: Only 8-bits of precision in this report, and no Y axis.
-  // Only contains 4 MSB of Z axis.
-
   void GetAccelData(AccelData* accel) const override
   {
-    accel->x = data_ptr[2] << 2;
+    // X axis only has 8 bits of precision. (converted to 10)
+    accel->value.x = Common::ExpandValue<u16>(data_ptr[2], 2);
 
-    // Retain lower 6 bits.
-    accel->z &= 0b111111;
+    // Y axis is not contained in this report. (provided by "Interleave2")
 
+    // Clear upper bits, retain lower bits. (provided by "Interleave2")
+    accel->value.z &= 0b111111;
+
+    // Report only contains 4 MSB of Z axis.
     const CoreData core = Common::BitCastPtr<CoreData>(data_ptr);
-    accel->z |= (core.acc_bits << 6) | (core.acc_bits2 << 8);
+    accel->value.z |= (core.acc_bits << 6) | (core.acc_bits2 << 8);
   }
 
   void SetAccelData(const AccelData& accel) override
   {
-    data_ptr[2] = accel.x >> 2;
+    data_ptr[2] = accel.value.x >> 2;
 
     CoreData core = Common::BitCastPtr<CoreData>(data_ptr);
-    core.acc_bits = (accel.z >> 6) & 0b11;
-    core.acc_bits2 = (accel.z >> 8) & 0b11;
+    core.acc_bits = (accel.value.z >> 6) & 0b11;
+    core.acc_bits2 = (accel.value.z >> 8) & 0b11;
     Common::BitCastPtr<CoreData>(data_ptr) = core;
   }
 
@@ -226,26 +224,28 @@ struct ReportInterleave1 : IncludeCore, IncludeIR<3, 18, 0>, NoExt
 struct ReportInterleave2 : IncludeCore, IncludeIR<3, 18, 18>, NoExt
 {
   // FYI: Only 8-bits of precision in this report, and no X axis.
-  // Only contains 4 LSB of Z axis.
-
   void GetAccelData(AccelData* accel) const override
   {
-    accel->y = data_ptr[2] << 2;
+    // X axis is not contained in this report. (provided by "Interleave1")
 
-    // Retain upper 4 bits.
-    accel->z &= ~0b111111;
+    // Y axis only has 8 bits of precision. (converted to 10)
+    accel->value.y = Common::ExpandValue<u16>(data_ptr[2], 2);
 
+    // Clear lower bits, retain upper bits. (provided by "Interleave1")
+    accel->value.z &= ~0b111111;
+
+    // Report only contains 4 LSBs of Z axis. (converted to 6)
     const CoreData core = Common::BitCastPtr<CoreData>(data_ptr);
-    accel->z |= (core.acc_bits << 2) | (core.acc_bits2 << 4);
+    accel->value.z |= Common::ExpandValue<u16>(core.acc_bits | core.acc_bits2 << 2, 2);
   }
 
   void SetAccelData(const AccelData& accel) override
   {
-    data_ptr[2] = accel.y >> 2;
+    data_ptr[2] = accel.value.y >> 2;
 
     CoreData core = Common::BitCastPtr<CoreData>(data_ptr);
-    core.acc_bits = (accel.z >> 2) & 0b11;
-    core.acc_bits2 = (accel.z >> 4) & 0b11;
+    core.acc_bits = (accel.value.z >> 2) & 0b11;
+    core.acc_bits2 = (accel.value.z >> 4) & 0b11;
     Common::BitCastPtr<CoreData>(data_ptr) = core;
   }
 

Source/Core/Core/HW/WiimoteCommon/DataReport.h

@@ -8,9 +8,11 @@
 #include <memory>
 
 #include "Common/CommonTypes.h"
+#include "Common/Matrix.h"
 #include "Core/HW/WiimoteCommon/WiimoteConstants.h"
 #include "Core/HW/WiimoteCommon/WiimoteHid.h"
 #include "Core/HW/WiimoteCommon/WiimoteReport.h"
+#include "InputCommon/ControllerEmu/ControllerEmu.h"
 
 namespace WiimoteCommon
 {
@@ -21,12 +23,6 @@ class DataReportManipulator
 public:
   virtual ~DataReportManipulator() = default;
 
-  // Accel data handled as if there were always 10 bits of precision.
-  struct AccelData
-  {
-    u16 x, y, z;
-  };
-
   using CoreData = ButtonData;
 
   virtual bool HasCore() const = 0;
@@ -66,7 +62,6 @@ public:
   explicit DataReportBuilder(InputReportID rpt_id);
 
   using CoreData = ButtonData;
-  using AccelData = DataReportManipulator::AccelData;
 
   void SetMode(InputReportID rpt_id);
   InputReportID GetMode() const;
@@ -99,11 +94,10 @@ public:
 
   u32 GetDataSize() const;
 
-private:
   static constexpr int HEADER_SIZE = 2;
-
   static constexpr int MAX_DATA_SIZE = MAX_PAYLOAD - 2;
 
+private:
   TypedHIDInputData<std::array<u8, MAX_DATA_SIZE>> m_data;
 
   std::unique_ptr<DataReportManipulator> m_manip;

Source/Core/Core/HW/WiimoteCommon/WiimoteConstants.h

@@ -10,6 +10,10 @@ namespace WiimoteCommon
 {
 constexpr u8 MAX_PAYLOAD = 23;
 
+// Based on testing, old WiiLi.org docs, and WiiUse library:
+// Max battery level seems to be 0xc8 (decimal 200)
+constexpr u8 MAX_BATTERY_LEVEL = 0xc8;
+
 enum class InputReportID : u8
 {
   Status = 0x20,

Source/Core/Core/HW/WiimoteCommon/WiimoteReport.h

@@ -7,7 +7,9 @@
 #include <vector>
 
 #include "Common/CommonTypes.h"
+#include "Common/Matrix.h"
 #include "Core/HW/WiimoteCommon/WiimoteConstants.h"
+#include "InputCommon/ControllerEmu/ControllerEmu.h"
 
 #ifdef _MSC_VER
 #pragma warning(push)
@@ -41,6 +43,8 @@ static_assert(sizeof(OutputReportGeneric) == 2, "Wrong size");
 
 struct OutputReportRumble
 {
+  static constexpr OutputReportID REPORT_ID = OutputReportID::Rumble;
+
   u8 rumble : 1;
 };
 static_assert(sizeof(OutputReportRumble) == 1, "Wrong size");
@@ -55,8 +59,34 @@ struct OutputReportEnableFeature
 };
 static_assert(sizeof(OutputReportEnableFeature) == 1, "Wrong size");
 
+struct OutputReportIRLogicEnable : OutputReportEnableFeature
+{
+  static constexpr OutputReportID REPORT_ID = OutputReportID::IRLogicEnable;
+};
+static_assert(sizeof(OutputReportIRLogicEnable) == 1, "Wrong size");
+
+struct OutputReportIRLogicEnable2 : OutputReportEnableFeature
+{
+  static constexpr OutputReportID REPORT_ID = OutputReportID::IRLogicEnable2;
+};
+static_assert(sizeof(OutputReportIRLogicEnable2) == 1, "Wrong size");
+
+struct OutputReportSpeakerEnable : OutputReportEnableFeature
+{
+  static constexpr OutputReportID REPORT_ID = OutputReportID::SpeakerEnable;
+};
+static_assert(sizeof(OutputReportSpeakerEnable) == 1, "Wrong size");
+
+struct OutputReportSpeakerMute : OutputReportEnableFeature
+{
+  static constexpr OutputReportID REPORT_ID = OutputReportID::SpeakerMute;
+};
+static_assert(sizeof(OutputReportSpeakerMute) == 1, "Wrong size");
+
 struct OutputReportLeds
 {
+  static constexpr OutputReportID REPORT_ID = OutputReportID::LED;
+
   u8 rumble : 1;
   u8 ack : 1;
   u8 : 2;
@@ -66,6 +96,8 @@ static_assert(sizeof(OutputReportLeds) == 1, "Wrong size");
 
 struct OutputReportMode
 {
+  static constexpr OutputReportID REPORT_ID = OutputReportID::ReportMode;
+
   u8 rumble : 1;
   u8 ack : 1;
   u8 continuous : 1;
@@ -76,6 +108,8 @@ static_assert(sizeof(OutputReportMode) == 2, "Wrong size");
 
 struct OutputReportRequestStatus
 {
+  static constexpr OutputReportID REPORT_ID = OutputReportID::RequestStatus;
+
   u8 rumble : 1;
   u8 : 7;
 };
@@ -83,6 +117,8 @@ static_assert(sizeof(OutputReportRequestStatus) == 1, "Wrong size");
 
 struct OutputReportWriteData
 {
+  static constexpr OutputReportID REPORT_ID = OutputReportID::WriteData;
+
   u8 rumble : 1;
   u8 : 1;
   u8 space : 2;
@@ -100,6 +136,8 @@ static_assert(sizeof(OutputReportWriteData) == 21, "Wrong size");
 
 struct OutputReportReadData
 {
+  static constexpr OutputReportID REPORT_ID = OutputReportID::ReadData;
+
   u8 rumble : 1;
   u8 : 1;
   u8 space : 2;
@@ -116,6 +154,8 @@ static_assert(sizeof(OutputReportReadData) == 6, "Wrong size");
 
 struct OutputReportSpeakerData
 {
+  static constexpr OutputReportID REPORT_ID = OutputReportID::SpeakerData;
+
   u8 rumble : 1;
   u8 : 2;
   u8 length : 5;
@@ -157,6 +197,8 @@ static_assert(sizeof(ButtonData) == 2, "Wrong size");
 
 struct InputReportStatus
 {
+  static constexpr InputReportID REPORT_ID = InputReportID::Status;
+
   ButtonData buttons;
   u8 battery_low : 1;
   u8 extension : 1;
@@ -170,6 +212,8 @@ static_assert(sizeof(InputReportStatus) == 6, "Wrong size");
 
 struct InputReportAck
 {
+  static constexpr InputReportID REPORT_ID = InputReportID::Ack;
+
   ButtonData buttons;
   OutputReportID rpt_id;
   ErrorCode error_code;
@@ -178,6 +222,8 @@ static_assert(sizeof(InputReportAck) == 4, "Wrong size");
 
 struct InputReportReadDataReply
 {
+  static constexpr InputReportID REPORT_ID = InputReportID::ReadDataReply;
+
   ButtonData buttons;
   u8 error : 4;
   u8 size_minus_one : 4;
@@ -187,6 +233,64 @@ struct InputReportReadDataReply
 };
 static_assert(sizeof(InputReportReadDataReply) == 21, "Wrong size");
 
+// Accel data handled as if there were always 10 bits of precision.
+using AccelType = Common::TVec3<u16>;
+using AccelData = ControllerEmu::RawValue<AccelType, 10>;
+
+// Found in Wiimote EEPROM and Nunchuk "register".
+// 0g and 1g points exist.
+struct AccelCalibrationPoint
+{
+  // All components have 10 bits of precision.
+  u16 GetX() const { return x2 << 2 | x1; }
+  u16 GetY() const { return y2 << 2 | y1; }
+  u16 GetZ() const { return z2 << 2 | z1; }
+  auto Get() const { return AccelType{GetX(), GetY(), GetZ()}; }
+
+  void SetX(u16 x)
+  {
+    x2 = x >> 2;
+    x1 = x;
+  }
+  void SetY(u16 y)
+  {
+    y2 = y >> 2;
+    y1 = y;
+  }
+  void SetZ(u16 z)
+  {
+    z2 = z >> 2;
+    z1 = z;
+  }
+  void Set(AccelType accel)
+  {
+    SetX(accel.x);
+    SetY(accel.y);
+    SetZ(accel.z);
+  }
+
+  u8 x2, y2, z2;
+  u8 z1 : 2;
+  u8 y1 : 2;
+  u8 x1 : 2;
+  u8 : 2;
+};
+
+// Located at 0x16 and 0x20 of Wii Remote EEPROM.
+struct AccelCalibrationData
+{
+  using Calibration = ControllerEmu::TwoPointCalibration<AccelType, 10>;
+
+  auto GetCalibration() const { return Calibration(zero_g.Get(), one_g.Get()); }
+
+  AccelCalibrationPoint zero_g;
+  AccelCalibrationPoint one_g;
+
+  u8 volume : 7;
+  u8 motor : 1;
+  u8 checksum;
+};
+
 }  // namespace WiimoteCommon
 
 #pragma pack(pop)

Source/Core/Core/HW/WiimoteEmu/Camera.cpp

@@ -60,14 +60,6 @@ void CameraLogic::Update(const Common::Matrix44& transform)
   using Common::Vec3;
   using Common::Vec4;
 
-  constexpr int CAMERA_WIDTH = 1024;
-  constexpr int CAMERA_HEIGHT = 768;
-
-  // Wiibrew claims the camera FOV is about 33 deg by 23 deg.
-  // Unconfirmed but it seems to work well enough.
-  constexpr int CAMERA_FOV_X_DEG = 33;
-  constexpr int CAMERA_FOV_Y_DEG = 23;
-
   constexpr auto CAMERA_FOV_Y = float(CAMERA_FOV_Y_DEG * MathUtil::TAU / 360);
   constexpr auto CAMERA_ASPECT_RATIO = float(CAMERA_FOV_X_DEG) / CAMERA_FOV_Y_DEG;
 
@@ -112,12 +104,12 @@ void CameraLogic::Update(const Common::Matrix44& transform)
       if (point.z > 0)
       {
         // FYI: Casting down vs. rounding seems to produce more symmetrical output.
-        const auto x = s32((1 - point.x / point.w) * CAMERA_WIDTH / 2);
-        const auto y = s32((1 - point.y / point.w) * CAMERA_HEIGHT / 2);
+        const auto x = s32((1 - point.x / point.w) * CAMERA_RES_X / 2);
+        const auto y = s32((1 - point.y / point.w) * CAMERA_RES_Y / 2);
 
         const auto point_size = std::lround(MAX_POINT_SIZE / point.w / 2);
 
-        if (x >= 0 && y >= 0 && x < CAMERA_WIDTH && y < CAMERA_HEIGHT)
+        if (x >= 0 && y >= 0 && x < CAMERA_RES_X && y < CAMERA_RES_Y)
           return CameraPoint{u16(x), u16(y), u8(point_size)};
       }
 
@@ -165,7 +157,7 @@ void CameraLogic::Update(const Common::Matrix44& transform)
       for (std::size_t i = 0; i != camera_points.size(); ++i)
       {
         const auto& p = camera_points[i];
-        if (p.x < CAMERA_WIDTH)
+        if (p.x < CAMERA_RES_X)
         {
           IRExtended irdata = {};
 
@@ -186,7 +178,7 @@ void CameraLogic::Update(const Common::Matrix44& transform)
       for (std::size_t i = 0; i != camera_points.size(); ++i)
       {
         const auto& p = camera_points[i];
-        if (p.x < CAMERA_WIDTH)
+        if (p.x < CAMERA_RES_X)
         {
           IRFull irdata = {};
 
@@ -203,8 +195,8 @@ void CameraLogic::Update(const Common::Matrix44& transform)
 
           irdata.xmin = std::max(p.x - p.size, 0);
           irdata.ymin = std::max(p.y - p.size, 0);
-          irdata.xmax = std::min(p.x + p.size, CAMERA_WIDTH);
-          irdata.ymax = std::min(p.y + p.size, CAMERA_HEIGHT);
+          irdata.xmax = std::min(p.x + p.size, CAMERA_RES_X);
+          irdata.ymax = std::min(p.y + p.size, CAMERA_RES_Y);
 
           // TODO: Is this maybe MSbs of the "intensity" value?
           irdata.zero = 0;

Source/Core/Core/HW/WiimoteEmu/Camera.h

@@ -20,6 +20,8 @@ namespace WiimoteEmu
 // Four bytes for two objects. Filled with 0xFF if empty
 struct IRBasic
 {
+  using IRObject = Common::TVec2<u16>;
+
   u8 x1;
   u8 y1;
   u8 x2hi : 2;
@@ -28,6 +30,9 @@ struct IRBasic
   u8 y1hi : 2;
   u8 x2;
   u8 y2;
+
+  auto GetObject1() const { return IRObject(x1hi << 8 | x1, y1hi << 8 | y1); }
+  auto GetObject2() const { return IRObject(x2hi << 8 | x2, y2hi << 8 | y2); }
 };
 static_assert(sizeof(IRBasic) == 5, "Wrong size");
 
@@ -62,6 +67,14 @@ static_assert(sizeof(IRFull) == 9, "Wrong size");
 class CameraLogic : public I2CSlave
 {
 public:
+  static constexpr int CAMERA_RES_X = 1024;
+  static constexpr int CAMERA_RES_Y = 768;
+
+  // Wiibrew claims the camera FOV is about 33 deg by 23 deg.
+  // Unconfirmed but it seems to work well enough.
+  static constexpr int CAMERA_FOV_X_DEG = 33;
+  static constexpr int CAMERA_FOV_Y_DEG = 23;
+
   enum : u8
   {
     IR_MODE_BASIC = 1,

Source/Core/Core/HW/WiimoteEmu/Dynamics.cpp

@@ -54,11 +54,15 @@ double CalculateStopDistance(double velocity, double max_accel)
   return velocity * velocity / (2 * std::copysign(max_accel, velocity));
 }
 
-// Note that 'gyroscope' is rotation of world around device.
-Common::Matrix33 ComplementaryFilter(const Common::Vec3& accelerometer,
-                                     const Common::Matrix33& gyroscope, float accel_weight)
+}  // namespace
+
+namespace WiimoteEmu
 {
-  const auto gyro_vec = gyroscope * Common::Vec3{0, 0, 1};
+Common::Matrix33 ComplementaryFilter(const Common::Matrix33& gyroscope,
+                                     const Common::Vec3& accelerometer, float accel_weight,
+                                     const Common::Vec3& accelerometer_normal)
+{
+  const auto gyro_vec = gyroscope * accelerometer_normal;
   const auto normalized_accel = accelerometer.Normalized();
 
   const auto cos_angle = normalized_accel.Dot(gyro_vec);
@@ -76,10 +80,6 @@ Common::Matrix33 ComplementaryFilter(const Common::Vec3& accelerometer,
   }
 }
 
-}  // namespace
-
-namespace WiimoteEmu
-{
 IMUCursorState::IMUCursorState() : rotation{Common::Matrix33::Identity()}
 {
 }
@@ -203,17 +203,17 @@ void EmulateSwing(MotionState* state, ControllerEmu::Force* swing_group, float t
   }
 }
 
-WiimoteCommon::DataReportBuilder::AccelData ConvertAccelData(const Common::Vec3& accel, u16 zero_g,
-                                                             u16 one_g)
+WiimoteCommon::AccelData ConvertAccelData(const Common::Vec3& accel, u16 zero_g, u16 one_g)
 {
   const auto scaled_accel = accel * (one_g - zero_g) / float(GRAVITY_ACCELERATION);
 
   // 10-bit integers.
   constexpr long MAX_VALUE = (1 << 10) - 1;
 
-  return {u16(std::clamp(std::lround(scaled_accel.x + zero_g), 0l, MAX_VALUE)),
-          u16(std::clamp(std::lround(scaled_accel.y + zero_g), 0l, MAX_VALUE)),
-          u16(std::clamp(std::lround(scaled_accel.z + zero_g), 0l, MAX_VALUE))};
+  return WiimoteCommon::AccelData(
+      {u16(std::clamp(std::lround(scaled_accel.x + zero_g), 0l, MAX_VALUE)),
+       u16(std::clamp(std::lround(scaled_accel.y + zero_g), 0l, MAX_VALUE)),
+       u16(std::clamp(std::lround(scaled_accel.z + zero_g), 0l, MAX_VALUE))});
 }
 
 void EmulateCursor(MotionState* state, ControllerEmu::Cursor* ir_group, float time_elapsed)
@@ -311,28 +311,24 @@ void EmulateIMUCursor(IMUCursorState* state, ControllerEmu::IMUCursor* imu_ir_gr
   }
 
   // Apply rotation from gyro data.
-  const auto gyro_rotation = Common::Matrix33::FromQuaternion(ang_vel->x * time_elapsed / -2,
-                                                              ang_vel->y * time_elapsed / -2,
-                                                              ang_vel->z * time_elapsed / -2, 1);
+  const auto gyro_rotation = GetMatrixFromGyroscope(*ang_vel * -1 * time_elapsed);
   state->rotation = gyro_rotation * state->rotation;
 
   // If we have some non-zero accel data use it to adjust gyro drift.
   constexpr auto ACCEL_WEIGHT = 0.02f;
   auto const accel = imu_accelerometer_group->GetState().value_or(Common::Vec3{});
   if (accel.LengthSquared())
-    state->rotation = ComplementaryFilter(accel, state->rotation, ACCEL_WEIGHT);
-
-  const auto inv_rotation = state->rotation.Inverted();
+    state->rotation = ComplementaryFilter(state->rotation, accel, ACCEL_WEIGHT);
 
   // Clamp yaw within configured bounds.
-  const auto yaw = std::asin((inv_rotation * Common::Vec3{0, 1, 0}).x);
+  const auto yaw = GetYaw(state->rotation);
   const auto max_yaw = float(imu_ir_group->GetTotalYaw() / 2);
   auto target_yaw = std::clamp(yaw, -max_yaw, max_yaw);
 
   // Handle the "Recenter" button being pressed.
   if (imu_ir_group->controls[0]->GetState<bool>())
   {
-    state->recentered_pitch = std::asin((inv_rotation * Common::Vec3{0, 1, 0}).z);
+    state->recentered_pitch = GetPitch(state->rotation);
     target_yaw = 0;
   }
 
@@ -390,10 +386,33 @@ Common::Matrix33 GetMatrixFromAcceleration(const Common::Vec3& accel)
                                   axis.LengthSquared() ? axis.Normalized() : Common::Vec3{0, 1, 0});
 }
 
+Common::Matrix33 GetMatrixFromGyroscope(const Common::Vec3& gyro)
+{
+  return Common::Matrix33::FromQuaternion(gyro.x / 2, gyro.y / 2, gyro.z / 2, 1);
+}
+
 Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle)
 {
   return Common::Matrix33::RotateZ(angle.z) * Common::Matrix33::RotateY(angle.y) *
          Common::Matrix33::RotateX(angle.x);
 }
 
+float GetPitch(const Common::Matrix33& world_rotation)
+{
+  const auto vec = world_rotation * Common::Vec3{0, 0, 1};
+  return std::atan2(vec.y, Common::Vec2(vec.x, vec.z).Length());
+}
+
+float GetRoll(const Common::Matrix33& world_rotation)
+{
+  const auto vec = world_rotation * Common::Vec3{0, 0, 1};
+  return std::atan2(vec.x, vec.z);
+}
+
+float GetYaw(const Common::Matrix33& world_rotation)
+{
+  const auto vec = world_rotation.Inverted() * Common::Vec3{0, 1, 0};
+  return std::atan2(vec.x, vec.y);
+}
+
 }  // namespace WiimoteEmu

Source/Core/Core/HW/WiimoteEmu/Dynamics.h

@@ -54,12 +54,26 @@ struct MotionState : PositionalState, RotationalState
 {
 };
 
+// Note that 'gyroscope' is rotation of world around device.
+// Alternative accelerometer_normal can be supplied to correct from non-accelerometer data.
+// e.g. Used for yaw/pitch correction with IR data.
+Common::Matrix33 ComplementaryFilter(const Common::Matrix33& gyroscope,
+                                     const Common::Vec3& accelerometer, float accel_weight,
+                                     const Common::Vec3& accelerometer_normal = {0, 0, 1});
+
 // Estimate orientation from accelerometer data.
 Common::Matrix33 GetMatrixFromAcceleration(const Common::Vec3& accel);
 
+// Get a rotation matrix from current gyro data.
+Common::Matrix33 GetMatrixFromGyroscope(const Common::Vec3& gyro);
+
 // Build a rotational matrix from euler angles.
 Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle);
 
+float GetPitch(const Common::Matrix33& world_rotation);
+float GetRoll(const Common::Matrix33& world_rotation);
+float GetYaw(const Common::Matrix33& world_rotation);
+
 void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& target,
                               const Common::Vec3& max_jerk, float time_elapsed);
 
@@ -75,7 +89,6 @@ void EmulateIMUCursor(IMUCursorState* state, ControllerEmu::IMUCursor* imu_ir_gr
                       ControllerEmu::IMUGyroscope* imu_gyroscope_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);
+WiimoteCommon::AccelData ConvertAccelData(const Common::Vec3& accel, u16 zero_g, u16 one_g);
 
 }  // namespace WiimoteEmu

Source/Core/Core/HW/WiimoteEmu/EmuSubroutines.cpp

@@ -236,10 +236,6 @@ void Wiimote::HandleRequestStatus(const OutputReportRequestStatus&)
   // Update status struct
   m_status.extension = m_extension_port.IsDeviceConnected();
 
-  // Based on testing, old WiiLi.org docs, and WiiUse library:
-  // Max battery level seems to be 0xc8 (decimal 200)
-  constexpr u8 MAX_BATTERY_LEVEL = 0xc8;
-
   m_status.battery = u8(std::lround(m_battery_setting.GetValue() / 100 * MAX_BATTERY_LEVEL));
 
   if (Core::WantsDeterminism())

Source/Core/Core/HW/WiimoteEmu/Extension/Classic.cpp

@@ -114,8 +114,10 @@ void Classic::Update()
   {
     const ControllerEmu::AnalogStick::StateData left_stick_state = m_left_stick->GetState();
 
-    classic_data.lx = static_cast<u8>(LEFT_STICK_CENTER + (left_stick_state.x * LEFT_STICK_RADIUS));
-    classic_data.ly = static_cast<u8>(LEFT_STICK_CENTER + (left_stick_state.y * LEFT_STICK_RADIUS));
+    const u8 x = static_cast<u8>(LEFT_STICK_CENTER + (left_stick_state.x * LEFT_STICK_RADIUS));
+    const u8 y = static_cast<u8>(LEFT_STICK_CENTER + (left_stick_state.y * LEFT_STICK_RADIUS));
+
+    classic_data.SetLeftStick({x, y});
   }
 
   // right stick
@@ -125,10 +127,7 @@ void Classic::Update()
     const u8 x = static_cast<u8>(RIGHT_STICK_CENTER + (right_stick_data.x * RIGHT_STICK_RADIUS));
     const u8 y = static_cast<u8>(RIGHT_STICK_CENTER + (right_stick_data.y * RIGHT_STICK_RADIUS));
 
-    classic_data.rx1 = x;
-    classic_data.rx2 = x >> 1;
-    classic_data.rx3 = x >> 3;
-    classic_data.ry = y;
+    classic_data.SetRightStick({x, y});
   }
 
   // triggers
@@ -139,18 +138,15 @@ void Classic::Update()
     const u8 lt = static_cast<u8>(trigs[0] * TRIGGER_RANGE);
     const u8 rt = static_cast<u8>(trigs[1] * TRIGGER_RANGE);
 
-    classic_data.lt1 = lt;
-    classic_data.lt2 = lt >> 3;
-    classic_data.rt = rt;
+    classic_data.SetLeftTrigger(lt);
+    classic_data.SetRightTrigger(rt);
   }
 
-  // buttons
-  m_buttons->GetState(&classic_data.bt.hex, classic_button_bitmasks.data());
-  // dpad
-  m_dpad->GetState(&classic_data.bt.hex, classic_dpad_bitmasks.data());
-
-  // flip button bits
-  classic_data.bt.hex ^= 0xFFFF;
+  // buttons and dpad
+  u16 buttons = 0;
+  m_buttons->GetState(&buttons, classic_button_bitmasks.data());
+  m_dpad->GetState(&buttons, classic_dpad_bitmasks.data());
+  classic_data.SetButtons(buttons);
 
   Common::BitCastPtr<DataFormat>(&m_reg.controller_data) = classic_data;
 }

Source/Core/Core/HW/WiimoteEmu/Extension/Classic.h

@@ -4,6 +4,9 @@
 
 #pragma once
 
+#include <limits>
+
+#include "Common/Matrix.h"
 #include "Core/HW/WiimoteCommon/WiimoteReport.h"
 #include "Core/HW/WiimoteEmu/Extension/Extension.h"
 
@@ -56,12 +59,59 @@ public:
   };
   static_assert(sizeof(ButtonFormat) == 2, "Wrong size");
 
+  static constexpr int LEFT_STICK_BITS = 6;
+  static constexpr int RIGHT_STICK_BITS = 5;
+  static constexpr int TRIGGER_BITS = 5;
+
   struct DataFormat
   {
-    // lx/ly/lz; left joystick
-    // rx/ry/rz; right joystick
-    // lt; left trigger
-    // rt; left trigger
+    using StickType = Common::TVec2<u8>;
+    using LeftStickRawValue = ControllerEmu::RawValue<StickType, LEFT_STICK_BITS>;
+    using RightStickRawValue = ControllerEmu::RawValue<StickType, RIGHT_STICK_BITS>;
+
+    using TriggerType = u8;
+    using TriggerRawValue = ControllerEmu::RawValue<TriggerType, TRIGGER_BITS>;
+
+    // 6-bit X and Y values (0-63)
+    auto GetLeftStick() const { return LeftStickRawValue{StickType(lx, ly)}; };
+    void SetLeftStick(const StickType& value)
+    {
+      lx = value.x;
+      ly = value.y;
+    }
+    // 5-bit X and Y values (0-31)
+    auto GetRightStick() const
+    {
+      return RightStickRawValue{StickType(rx1 | rx2 << 1 | rx3 << 3, ry)};
+    };
+    void SetRightStick(const StickType& value)
+    {
+      rx1 = value.x & 0b1;
+      rx2 = (value.x >> 1) & 0b11;
+      rx3 = (value.x >> 3) & 0b11;
+      ry = value.y;
+    }
+    // 5-bit values (0-31)
+    auto GetLeftTrigger() const { return TriggerRawValue(lt1 | lt2 << 3); }
+    void SetLeftTrigger(TriggerType value)
+    {
+      lt1 = value & 0b111;
+      lt2 = (value >> 3) & 0b11;
+    }
+    auto GetRightTrigger() const { return TriggerRawValue(rt); }
+    void SetRightTrigger(TriggerType value) { rt = value; }
+
+    u16 GetButtons() const
+    {
+      // 0 == pressed.
+      return ~bt.hex;
+    }
+
+    void SetButtons(u16 value)
+    {
+      // 0 == pressed.
+      bt.hex = ~value;
+    }
 
     u8 lx : 6;  // byte 0
     u8 rx3 : 2;
@@ -80,6 +130,53 @@ public:
   };
   static_assert(sizeof(DataFormat) == 6, "Wrong size");
 
+  static constexpr int CAL_STICK_BITS = 8;
+  static constexpr int CAL_TRIGGER_BITS = 8;
+
+  struct CalibrationData
+  {
+    using StickType = DataFormat::StickType;
+    using TriggerType = DataFormat::TriggerType;
+
+    using StickCalibration = ControllerEmu::ThreePointCalibration<StickType, CAL_STICK_BITS>;
+    using TriggerCalibration = ControllerEmu::TwoPointCalibration<TriggerType, CAL_TRIGGER_BITS>;
+
+    static constexpr TriggerType TRIGGER_MAX = std::numeric_limits<TriggerType>::max();
+
+    struct StickAxis
+    {
+      u8 max;
+      u8 min;
+      u8 center;
+    };
+
+    auto GetLeftStick() const
+    {
+      return StickCalibration{StickType{left_stick_x.min, left_stick_y.min},
+                              StickType{left_stick_x.center, left_stick_y.center},
+                              StickType{left_stick_x.max, left_stick_y.max}};
+    }
+    auto GetRightStick() const
+    {
+      return StickCalibration{StickType{right_stick_x.min, right_stick_y.min},
+                              StickType{right_stick_x.center, right_stick_y.center},
+                              StickType{right_stick_x.max, right_stick_y.max}};
+    }
+    auto GetLeftTrigger() const { return TriggerCalibration{left_trigger_zero, TRIGGER_MAX}; }
+    auto GetRightTrigger() const { return TriggerCalibration{right_trigger_zero, TRIGGER_MAX}; }
+
+    StickAxis left_stick_x;
+    StickAxis left_stick_y;
+    StickAxis right_stick_x;
+    StickAxis right_stick_y;
+
+    u8 left_trigger_zero;
+    u8 right_trigger_zero;
+
+    std::array<u8, 2> checksum;
+  };
+  static_assert(sizeof(CalibrationData) == 16, "Wrong size");
+
   Classic();
 
   void Update() override;
@@ -110,13 +207,10 @@ public:
 
   static constexpr u8 CAL_STICK_CENTER = 0x80;
   static constexpr u8 CAL_STICK_RANGE = 0x7f;
-  static constexpr int CAL_STICK_BITS = 8;
 
-  static constexpr int LEFT_STICK_BITS = 6;
   static constexpr u8 LEFT_STICK_CENTER = CAL_STICK_CENTER >> (CAL_STICK_BITS - LEFT_STICK_BITS);
   static constexpr u8 LEFT_STICK_RADIUS = CAL_STICK_RANGE >> (CAL_STICK_BITS - LEFT_STICK_BITS);
 
-  static constexpr int RIGHT_STICK_BITS = 5;
   static constexpr u8 RIGHT_STICK_CENTER = CAL_STICK_CENTER >> (CAL_STICK_BITS - RIGHT_STICK_BITS);
   static constexpr u8 RIGHT_STICK_RADIUS = CAL_STICK_RANGE >> (CAL_STICK_BITS - RIGHT_STICK_BITS);
 

Source/Core/Core/HW/WiimoteEmu/Extension/Nunchuk.cpp

@@ -87,10 +87,9 @@ void Nunchuk::Update()
   }
 
   // buttons
-  m_buttons->GetState(&nc_data.bt.hex, nunchuk_button_bitmasks.data());
-
-  // flip the button bits :/
-  nc_data.bt.hex ^= 0x03;
+  u8 buttons = 0;
+  m_buttons->GetState(&buttons, nunchuk_button_bitmasks.data());
+  nc_data.SetButtons(buttons);
 
   // Acceleration data:
   EmulateSwing(&m_swing_state, m_swing, 1.f / ::Wiimote::UPDATE_FREQ);
@@ -109,13 +108,7 @@ void Nunchuk::Update()
 
   // Calibration values are 8-bit but we want 10-bit precision, so << 2.
   const auto acc = ConvertAccelData(accel, ACCEL_ZERO_G << 2, ACCEL_ONE_G << 2);
-
-  nc_data.ax = (acc.x >> 2) & 0xFF;
-  nc_data.ay = (acc.y >> 2) & 0xFF;
-  nc_data.az = (acc.z >> 2) & 0xFF;
-  nc_data.bt.acc_x_lsb = acc.x & 0x3;
-  nc_data.bt.acc_y_lsb = acc.y & 0x3;
-  nc_data.bt.acc_z_lsb = acc.z & 0x3;
+  nc_data.SetAccel(acc.value);
 
   Common::BitCastPtr<DataFormat>(&m_reg.controller_data) = nc_data;
 }

Source/Core/Core/HW/WiimoteEmu/Extension/Nunchuk.h

@@ -51,25 +51,103 @@ public:
   };
   static_assert(sizeof(ButtonFormat) == 1, "Wrong size");
 
-  union DataFormat
+  struct DataFormat
   {
-    struct
+    using StickType = Common::TVec2<u8>;
+    using StickRawValue = ControllerEmu::RawValue<StickType, 8>;
+
+    using AccelType = WiimoteCommon::AccelType;
+    using AccelData = WiimoteCommon::AccelData;
+
+    auto GetStick() const { return StickRawValue(StickType(jx, jy)); }
+
+    // Components have 10 bits of precision.
+    u16 GetAccelX() const { return ax << 2 | bt.acc_x_lsb; }
+    u16 GetAccelY() const { return ay << 2 | bt.acc_y_lsb; }
+    u16 GetAccelZ() const { return az << 2 | bt.acc_z_lsb; }
+    auto GetAccel() const { return AccelData{AccelType{GetAccelX(), GetAccelY(), GetAccelZ()}}; }
+
+    void SetAccelX(u16 val)
     {
-      // joystick x, y
-      u8 jx;
-      u8 jy;
+      ax = val >> 2;
+      bt.acc_x_lsb = val & 0b11;
+    }
+    void SetAccelY(u16 val)
+    {
+      ay = val >> 2;
+      bt.acc_y_lsb = val & 0b11;
+    }
+    void SetAccelZ(u16 val)
+    {
+      az = val >> 2;
+      bt.acc_z_lsb = val & 0b11;
+    }
+    void SetAccel(const AccelType& accel)
+    {
+      SetAccelX(accel.x);
+      SetAccelY(accel.y);
+      SetAccelZ(accel.z);
+    }
 
-      // accelerometer
-      u8 ax;
-      u8 ay;
-      u8 az;
+    u8 GetButtons() const
+    {
+      // 0 == pressed.
+      return ~bt.hex & (BUTTON_C | BUTTON_Z);
+    }
+    void SetButtons(u8 value)
+    {
+      // 0 == pressed.
+      bt.hex |= (BUTTON_C | BUTTON_Z);
+      bt.hex ^= value & (BUTTON_C | BUTTON_Z);
+    }
 
-      // buttons + accelerometer LSBs
-      ButtonFormat bt;
-    };
+    // joystick x, y
+    u8 jx;
+    u8 jy;
+
+    // accelerometer
+    u8 ax;
+    u8 ay;
+    u8 az;
+
+    // buttons + accelerometer LSBs
+    ButtonFormat bt;
   };
   static_assert(sizeof(DataFormat) == 6, "Wrong size");
 
+  struct CalibrationData
+  {
+    using StickType = DataFormat::StickType;
+    using StickCalibration = ControllerEmu::ThreePointCalibration<StickType, 8>;
+
+    using AccelType = WiimoteCommon::AccelType;
+    using AccelCalibration = ControllerEmu::TwoPointCalibration<AccelType, 10>;
+
+    struct Stick
+    {
+      u8 max;
+      u8 min;
+      u8 center;
+    };
+
+    auto GetStick() const
+    {
+      return StickCalibration(StickType{stick_x.min, stick_y.min},
+                              StickType{stick_x.center, stick_y.center},
+                              StickType{stick_x.max, stick_y.max});
+    }
+    auto GetAccel() const { return AccelCalibration(accel_zero_g.Get(), accel_one_g.Get()); }
+
+    WiimoteCommon::AccelCalibrationPoint accel_zero_g;
+    WiimoteCommon::AccelCalibrationPoint accel_one_g;
+
+    Stick stick_x;
+    Stick stick_y;
+
+    std::array<u8, 2> checksum;
+  };
+  static_assert(sizeof(CalibrationData) == 16, "Wrong size");
+
   Nunchuk();
 
   void Update() override;

Source/Core/Core/HW/WiimoteEmu/MotionPlus.cpp

@@ -16,7 +16,6 @@
 #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"
@@ -56,6 +55,41 @@ struct MPI : mbedtls_mpi
 
 namespace WiimoteEmu
 {
+Common::Vec3 MotionPlus::DataFormat::Data::GetAngularVelocity(const CalibrationBlocks& blocks) const
+{
+  // Each axis may be using either slow or fast calibration.
+  const auto calibration = blocks.GetRelevantCalibration(is_slow);
+
+  // It seems M+ calibration data does not follow the "right-hand rule".
+  const auto sign_fix = Common::Vec3(-1, +1, -1);
+
+  // Adjust deg/s to rad/s.
+  constexpr auto scalar = float(MathUtil::TAU / 360);
+
+  return gyro.GetNormalizedValue(calibration.value) * sign_fix * Common::Vec3(calibration.degrees) *
+         scalar;
+}
+
+auto MotionPlus::CalibrationBlocks::GetRelevantCalibration(SlowType is_slow) const
+    -> RelevantCalibration
+{
+  RelevantCalibration result;
+
+  const auto& pitch_block = is_slow.x ? slow : fast;
+  const auto& roll_block = is_slow.y ? slow : fast;
+  const auto& yaw_block = is_slow.z ? slow : fast;
+
+  result.value.max = {pitch_block.pitch_scale, roll_block.roll_scale, yaw_block.yaw_scale};
+
+  result.value.zero = {pitch_block.pitch_zero, roll_block.roll_zero, yaw_block.yaw_zero};
+
+  result.degrees.x = pitch_block.degrees_div_6 * 6;
+  result.degrees.y = roll_block.degrees_div_6 * 6;
+  result.degrees.z = yaw_block.degrees_div_6 * 6;
+
+  return result;
+}
+
 MotionPlus::MotionPlus() : Extension("MotionPlus")
 {
 }
@@ -82,35 +116,20 @@ void MotionPlus::Reset()
   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;
-  };
-#pragma pack(pop)
-
   static_assert(sizeof(CalibrationData) == 0x20, "Bad size.");
 
   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.yaw_zero = calibration.slow.yaw_zero = CALIBRATION_ZERO;
+  calibration.fast.roll_zero = calibration.slow.roll_zero = CALIBRATION_ZERO;
+  calibration.fast.pitch_zero = calibration.slow.pitch_zero = CALIBRATION_ZERO;
+
+  calibration.fast.yaw_scale = calibration.slow.yaw_scale = YAW_SCALE;
+  calibration.fast.roll_scale = calibration.slow.roll_scale = ROLL_SCALE;
+  calibration.fast.pitch_scale = calibration.slow.pitch_scale = PITCH_SCALE;
+
   calibration.fast.degrees_div_6 = CALIBRATION_FAST_SCALE_DEGREES / 6;
   calibration.slow.degrees_div_6 = CALIBRATION_SLOW_SCALE_DEGREES / 6;
 
@@ -120,17 +139,22 @@ void MotionPlus::Reset()
   calibration.uid_1 = 0x0b;
   calibration.uid_2 = 0xe9;
 
-  // Update checksum (crc32 of all data other than the checksum itself):
-  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);
+  calibration.UpdateChecksum();
 
   Common::BitCastPtr<CalibrationData>(m_reg_data.calibration_data.data()) = calibration;
 }
 
+void MotionPlus::CalibrationData::UpdateChecksum()
+{
+  // Checksum is crc32 of all data other than the checksum itself.
+  auto crc_result = crc32(0, Z_NULL, 0);
+  crc_result = crc32(crc_result, reinterpret_cast<const Bytef*>(this), 0xe);
+  crc_result = crc32(crc_result, reinterpret_cast<const Bytef*>(this) + 0x10, 0xe);
+
+  crc32_lsb = u16(crc_result);
+  crc32_msb = u16(crc_result >> 16);
+}
+
 void MotionPlus::DoState(PointerWrap& p)
 {
   p.Do(m_reg_data);
@@ -547,47 +571,10 @@ void MotionPlus::PrepareInput(const Common::Vec3& angular_velocity)
       break;
     }
     case PassthroughMode::Nunchuk:
-    {
-      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 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
-        Common::SetBit(data[5], 7, Common::ExtractBit(data[4], 0));
-        // Bit 3 of byte 5 is moved to  bit 4 of byte 5, overwriting it
-        Common::SetBit(data[5], 4, Common::ExtractBit(data[5], 3));
-        // Bit 1 of byte 5 is moved to bit 3 of byte 5
-        Common::SetBit(data[5], 3, Common::ExtractBit(data[5], 1));
-        // 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;
-      }
-      else
-      {
-        // Read failed (extension unplugged), Send M+ data instead
-        mplus_data.is_mp_data = true;
-      }
-      break;
-    }
     case PassthroughMode::Classic:
-    {
       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.
-        Common::SetBit(data[0], 0, Common::ExtractBit(data[5], 0));
-        Common::SetBit(data[1], 0, Common::ExtractBit(data[5], 1));
-
+        ApplyPassthroughModifications(GetPassthroughMode(), data);
         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.
@@ -599,7 +586,6 @@ void MotionPlus::PrepareInput(const Common::Vec3& angular_velocity)
         mplus_data.is_mp_data = true;
       }
       break;
-    }
     default:
       // This really shouldn't happen as the M+ deactivates on an invalid mode write.
       ERROR_LOG(WIIMOTE, "M+ unknown passthrough-mode %d", int(GetPassthroughMode()));
@@ -664,4 +650,66 @@ void MotionPlus::PrepareInput(const Common::Vec3& angular_velocity)
   Common::BitCastPtr<DataFormat>(data) = mplus_data;
 }
 
+void MotionPlus::ApplyPassthroughModifications(PassthroughMode mode, u8* data)
+{
+  if (mode == PassthroughMode::Nunchuk)
+  {
+    // 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 three accelerometer values.
+    // Bit 7 of byte 5 is moved to bit 6 of byte 5, overwriting it
+    Common::SetBit<6>(data[5], Common::ExtractBit<7>(data[5]));
+    // Bit 0 of byte 4 is moved to bit 7 of byte 5
+    Common::SetBit<7>(data[5], Common::ExtractBit<0>(data[4]));
+    // Bit 3 of byte 5 is moved to  bit 4 of byte 5, overwriting it
+    Common::SetBit<4>(data[5], Common::ExtractBit<3>(data[5]));
+    // Bit 1 of byte 5 is moved to bit 3 of byte 5
+    Common::SetBit<3>(data[5], Common::ExtractBit<1>(data[5]));
+    // Bit 0 of byte 5 is moved to bit 2 of byte 5, overwriting it
+    Common::SetBit<2>(data[5], Common::ExtractBit<0>(data[5]));
+  }
+  else if (mode == PassthroughMode::Classic)
+  {
+    // 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.
+    Common::SetBit<0>(data[0], Common::ExtractBit<0>(data[5]));
+    Common::SetBit<0>(data[1], Common::ExtractBit<1>(data[5]));
+  }
+}
+
+void MotionPlus::ReversePassthroughModifications(PassthroughMode mode, u8* data)
+{
+  if (mode == PassthroughMode::Nunchuk)
+  {
+    // Undo M+'s "nunchuk passthrough" modifications.
+    Common::SetBit<0>(data[5], Common::ExtractBit<2>(data[5]));
+    Common::SetBit<1>(data[5], Common::ExtractBit<3>(data[5]));
+    Common::SetBit<3>(data[5], Common::ExtractBit<4>(data[5]));
+    Common::SetBit<0>(data[4], Common::ExtractBit<7>(data[5]));
+    Common::SetBit<7>(data[5], Common::ExtractBit<6>(data[5]));
+
+    // Set the overwritten bits from the next LSB.
+    Common::SetBit<2>(data[5], Common::ExtractBit<3>(data[5]));
+    Common::SetBit<4>(data[5], Common::ExtractBit<5>(data[5]));
+    Common::SetBit<6>(data[5], Common::ExtractBit<7>(data[5]));
+  }
+  else if (mode == PassthroughMode::Classic)
+  {
+    // Undo M+'s "classic controller passthrough" modifications.
+    Common::SetBit<0>(data[5], Common::ExtractBit<0>(data[0]));
+    Common::SetBit<1>(data[5], Common::ExtractBit<0>(data[1]));
+
+    // Set the overwritten bits from the next LSB.
+    Common::SetBit<0>(data[0], Common::ExtractBit<1>(data[0]));
+    Common::SetBit<0>(data[1], Common::ExtractBit<1>(data[1]));
+
+    // This is an overwritten unused button bit on the Classic Controller.
+    // Note it's a significant bit on the DJ Hero Turntable. (passthrough not feasible)
+    Common::SetBit<0>(data[4], 1);
+  }
+}
+
 }  // namespace WiimoteEmu

Source/Core/Core/HW/WiimoteEmu/MotionPlus.h

@@ -7,59 +7,90 @@
 #include <array>
 
 #include "Common/CommonTypes.h"
+#include "Common/Swap.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:
-  MotionPlus();
-
-  void Update() override;
-  void Reset() override;
-  void DoState(PointerWrap& p) override;
-
-  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:
-  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
   {
+    // Note: `Disabled` is an M+ enabled with no passthrough. Maybe there is a better name.
     Disabled = 0x04,
     Nunchuk = 0x05,
     Classic = 0x07,
   };
 
-  enum class ActivationStatus
+#pragma pack(push, 1)
+  struct CalibrationBlock
   {
-    Inactive,
-    Activating,
-    Deactivating,
-    Active,
+    Common::BigEndianValue<u16> yaw_zero;
+    Common::BigEndianValue<u16> roll_zero;
+    Common::BigEndianValue<u16> pitch_zero;
+    Common::BigEndianValue<u16> yaw_scale;
+    Common::BigEndianValue<u16> roll_scale;
+    Common::BigEndianValue<u16> pitch_scale;
+    u8 degrees_div_6;
   };
 
-#pragma pack(push, 1)
+  struct CalibrationBlocks
+  {
+    using GyroType = Common::TVec3<u16>;
+    using SlowType = Common::TVec3<bool>;
+
+    struct RelevantCalibration
+    {
+      ControllerEmu::TwoPointCalibration<GyroType, 16> value;
+      Common::TVec3<u16> degrees;
+    };
+
+    // Each axis may be using either slow or fast calibration.
+    // This function builds calibration that is relevant for current data.
+    RelevantCalibration GetRelevantCalibration(SlowType is_slow) const;
+
+    CalibrationBlock fast;
+    CalibrationBlock slow;
+  };
+
+  struct CalibrationData
+  {
+    void UpdateChecksum();
+
+    CalibrationBlock fast;
+    u8 uid_1;
+    Common::BigEndianValue<u16> crc32_msb;
+    CalibrationBlock slow;
+    u8 uid_2;
+    Common::BigEndianValue<u16> crc32_lsb;
+  };
+  static_assert(sizeof(CalibrationData) == 0x20, "Wrong size");
+
   struct DataFormat
   {
+    using GyroType = CalibrationBlocks::GyroType;
+    using SlowType = CalibrationBlocks::SlowType;
+    using GyroRawValue = ControllerEmu::RawValue<GyroType, 14>;
+
+    struct Data
+    {
+      // Return radian/s following "right-hand rule" with given calibration blocks.
+      Common::Vec3 GetAngularVelocity(const CalibrationBlocks&) const;
+
+      GyroRawValue gyro;
+      SlowType is_slow;
+    };
+
+    auto GetData() const
+    {
+      return Data{
+          GyroRawValue{GyroType(pitch1 | pitch2 << 8, roll1 | roll2 << 8, yaw1 | yaw2 << 8)},
+          SlowType(pitch_slow, roll_slow, yaw_slow)};
+    }
+
     // yaw1, roll1, pitch1: Bits 0-7
     // yaw2, roll2, pitch2: Bits 8-13
 
@@ -79,7 +110,50 @@ private:
     u8 is_mp_data : 1;
     u8 pitch2 : 6;
   };
+  static_assert(sizeof(DataFormat) == 6, "Wrong size");
+#pragma pack(pop)
 
+  static constexpr u8 INACTIVE_DEVICE_ADDR = 0x53;
+  static constexpr u8 ACTIVE_DEVICE_ADDR = 0x52;
+  static constexpr u8 PASSTHROUGH_MODE_OFFSET = 0xfe;
+
+  MotionPlus();
+
+  void Update() override;
+  void Reset() override;
+  void DoState(PointerWrap& p) override;
+
+  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);
+
+  // Pointer to 6 bytes is expected.
+  static void ApplyPassthroughModifications(PassthroughMode, u8* data);
+  static void ReversePassthroughModifications(PassthroughMode, u8* data);
+
+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 ActivationStatus
+  {
+    Inactive,
+    Activating,
+    Deactivating,
+    Active,
+  };
+
+#pragma pack(push, 1)
   struct Register
   {
     std::array<u8, 21> controller_data;
@@ -135,14 +209,8 @@ private:
     std::array<u8, 6> ext_identifier;
   };
 #pragma pack(pop)
-  static_assert(sizeof(DataFormat) == 6, "Wrong size");
   static_assert(0x100 == sizeof(Register), "Wrong size");
 
-  static constexpr u8 INACTIVE_DEVICE_ADDR = 0x53;
-  static constexpr u8 ACTIVE_DEVICE_ADDR = 0x52;
-
-  static constexpr u8 PASSTHROUGH_MODE_OFFSET = 0xfe;
-
   static constexpr int CALIBRATION_BITS = 16;
 
   static constexpr u16 CALIBRATION_ZERO = 1 << (CALIBRATION_BITS - 1);