WiimoteEmu: MotionPlus is now working.

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,

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

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

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

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

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

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

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,