WiimoteEmu: Update MotionPlus status from DesiredWiimoteState.

Source/Core/Core/HW/WiimoteEmu/DesiredWiimoteState.h

@@ -4,9 +4,11 @@
 #pragma once
 
 #include <array>
+#include <optional>
 
 #include "Core/HW/WiimoteCommon/WiimoteReport.h"
 #include "Core/HW/WiimoteEmu/Camera.h"
+#include "Core/HW/WiimoteEmu/MotionPlus.h"
 #include "Core/HW/WiimoteEmu/WiimoteEmu.h"
 
 namespace WiimoteEmu
@@ -23,5 +25,6 @@ struct DesiredWiimoteState
   WiimoteCommon::ButtonData buttons{};  // non-button state in this is ignored
   WiimoteCommon::AccelData acceleration = DEFAULT_ACCELERATION;
   std::array<CameraPoint, 2> camera_points = DEFAULT_CAMERA;
+  std::optional<MotionPlus::DataFormat::Data> motion_plus = std::nullopt;
 };
 }  // namespace WiimoteEmu

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

@@ -142,7 +142,8 @@ void Wiimote::SendAck(OutputReportID rpt_id, ErrorCode error_code)
   InterruptDataInputCallback(rpt.GetData(), rpt.GetSize());
 }
 
-void Wiimote::HandleExtensionSwap()
+void Wiimote::HandleExtensionSwap(ExtensionNumber desired_extension_number,
+                                  bool desired_motion_plus)
 {
   if (WIIMOTE_BALANCE_BOARD == m_index)
   {
@@ -151,11 +152,6 @@ void Wiimote::HandleExtensionSwap()
     return;
   }
 
-  ExtensionNumber desired_extension_number =
-      static_cast<ExtensionNumber>(m_attachments->GetSelectedAttachment());
-
-  const bool desired_motion_plus = m_motion_plus_setting.GetValue();
-
   // FYI: AttachExtension also connects devices to the i2c bus
 
   if (m_is_motion_plus_attached && !desired_motion_plus)

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

@@ -522,9 +522,56 @@ void MotionPlus::Update()
   }
 }
 
+MotionPlus::DataFormat::Data MotionPlus::GetGyroscopeData(const Common::Vec3& angular_velocity)
+{
+  // Conversion from radians to the calibrated values in degrees.
+  constexpr float VALUE_SCALE =
+      (CALIBRATION_SCALE_OFFSET >> (CALIBRATION_BITS - BITS_OF_PRECISION)) / float(MathUtil::TAU) *
+      360;
+
+  constexpr float SLOW_SCALE = VALUE_SCALE / CALIBRATION_SLOW_SCALE_DEGREES;
+  constexpr float FAST_SCALE = VALUE_SCALE / CALIBRATION_FAST_SCALE_DEGREES;
+
+  static_assert(ZERO_VALUE == 1 << (BITS_OF_PRECISION - 1),
+                "SLOW_MAX_RAD_PER_SEC assumes calibrated zero is at center of sensor values.");
+
+  constexpr u16 SENSOR_RANGE = 1 << (BITS_OF_PRECISION - 1);
+  constexpr float SLOW_MAX_RAD_PER_SEC = SENSOR_RANGE / SLOW_SCALE;
+
+  // Slow (high precision) scaling can be used if it fits in the sensor range.
+  const float yaw = angular_velocity.z;
+  const bool yaw_slow = (std::abs(yaw) < SLOW_MAX_RAD_PER_SEC);
+  const s32 yaw_value = yaw * (yaw_slow ? SLOW_SCALE : FAST_SCALE);
+
+  const float roll = angular_velocity.y;
+  const bool roll_slow = (std::abs(roll) < SLOW_MAX_RAD_PER_SEC);
+  const s32 roll_value = roll * (roll_slow ? SLOW_SCALE : FAST_SCALE);
+
+  const float pitch = angular_velocity.x;
+  const bool pitch_slow = (std::abs(pitch) < SLOW_MAX_RAD_PER_SEC);
+  const s32 pitch_value = pitch * (pitch_slow ? SLOW_SCALE : FAST_SCALE);
+
+  const u16 clamped_yaw_value = u16(std::clamp(yaw_value + ZERO_VALUE, 0, MAX_VALUE));
+  const u16 clamped_roll_value = u16(std::clamp(roll_value + ZERO_VALUE, 0, MAX_VALUE));
+  const u16 clamped_pitch_value = u16(std::clamp(pitch_value + ZERO_VALUE, 0, MAX_VALUE));
+
+  return MotionPlus::DataFormat::Data{
+      MotionPlus::DataFormat::GyroRawValue{MotionPlus::DataFormat::GyroType(
+          clamped_pitch_value, clamped_roll_value, clamped_yaw_value)},
+      MotionPlus::DataFormat::SlowType(pitch_slow, roll_slow, yaw_slow)};
+}
+
+MotionPlus::DataFormat::Data MotionPlus::GetDefaultGyroscopeData()
+{
+  return MotionPlus::DataFormat::Data{
+      MotionPlus::DataFormat::GyroRawValue{
+          MotionPlus::DataFormat::GyroType(u16(ZERO_VALUE), u16(ZERO_VALUE), u16(ZERO_VALUE))},
+      MotionPlus::DataFormat::SlowType(true, true, true)};
+}
+
 // This is something that is triggered by a read of 0x00 on real hardware.
 // But we do it here for determinism reasons.
-void MotionPlus::PrepareInput(const Common::Vec3& angular_velocity)
+void MotionPlus::PrepareInput(const MotionPlus::DataFormat::Data& gyroscope_data)
 {
   if (GetActivationStatus() != ActivationStatus::Active)
     return;
@@ -592,41 +639,16 @@ void MotionPlus::PrepareInput(const Common::Vec3& angular_velocity)
   // If the above logic determined this should be M+ data, update it here.
   if (mplus_data.is_mp_data)
   {
-    constexpr int BITS_OF_PRECISION = 14;
+    const bool pitch_slow = gyroscope_data.is_slow.x;
+    const bool roll_slow = gyroscope_data.is_slow.y;
+    const bool yaw_slow = gyroscope_data.is_slow.z;
+    const u16 pitch_value = gyroscope_data.gyro.value.x;
+    const u16 roll_value = gyroscope_data.gyro.value.y;
+    const u16 yaw_value = gyroscope_data.gyro.value.z;
 
-    // Conversion from radians to the calibrated values in degrees.
+    mplus_data.yaw_slow = u8(yaw_slow);
-    constexpr float VALUE_SCALE =
+    mplus_data.roll_slow = u8(roll_slow);
-        (CALIBRATION_SCALE_OFFSET >> (CALIBRATION_BITS - BITS_OF_PRECISION)) /
+    mplus_data.pitch_slow = u8(pitch_slow);
-        float(MathUtil::TAU) * 360;
-
-    constexpr float SLOW_SCALE = VALUE_SCALE / CALIBRATION_SLOW_SCALE_DEGREES;
-    constexpr float FAST_SCALE = VALUE_SCALE / CALIBRATION_FAST_SCALE_DEGREES;
-
-    constexpr s32 ZERO_VALUE = CALIBRATION_ZERO >> (CALIBRATION_BITS - BITS_OF_PRECISION);
-    constexpr s32 MAX_VALUE = (1 << BITS_OF_PRECISION) - 1;
-
-    static_assert(ZERO_VALUE == 1 << (BITS_OF_PRECISION - 1),
-                  "SLOW_MAX_RAD_PER_SEC assumes calibrated zero is at center of sensor values.");
-
-    constexpr u16 SENSOR_RANGE = 1 << (BITS_OF_PRECISION - 1);
-    constexpr float SLOW_MAX_RAD_PER_SEC = SENSOR_RANGE / SLOW_SCALE;
-
-    // Slow (high precision) scaling can be used if it fits in the sensor range.
-    const float yaw = angular_velocity.z;
-    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 = std::clamp(yaw_value + ZERO_VALUE, 0, MAX_VALUE);
-    roll_value = std::clamp(roll_value + ZERO_VALUE, 0, MAX_VALUE);
-    pitch_value = std::clamp(pitch_value + ZERO_VALUE, 0, MAX_VALUE);
 
     // Bits 0-7
     mplus_data.yaw1 = u8(yaw_value);

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

@@ -125,7 +125,10 @@ 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);
+  static MotionPlus::DataFormat::Data GetGyroscopeData(const Common::Vec3& angular_velocity);
+  static MotionPlus::DataFormat::Data GetDefaultGyroscopeData();
+
+  void PrepareInput(const MotionPlus::DataFormat::Data& gyroscope_data);
 
   // Pointer to 6 bytes is expected.
   static void ApplyPassthroughModifications(PassthroughMode, u8* data);
@@ -218,6 +221,10 @@ private:
   static constexpr u16 CALIBRATION_FAST_SCALE_DEGREES = 0x4b0;
   static constexpr u16 CALIBRATION_SLOW_SCALE_DEGREES = 0x10e;
 
+  static constexpr int BITS_OF_PRECISION = 14;
+  static constexpr s32 ZERO_VALUE = CALIBRATION_ZERO >> (CALIBRATION_BITS - BITS_OF_PRECISION);
+  static constexpr s32 MAX_VALUE = (1 << BITS_OF_PRECISION) - 1;
+
   void Activate();
   void Deactivate();
   void OnPassthroughModeWrite();

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

@@ -174,7 +174,8 @@ void Wiimote::Reset()
 
   // Switch to desired M+ status and extension (if any).
   // M+ and EXT are reset on attachment.
-  HandleExtensionSwap();
+  HandleExtensionSwap(static_cast<ExtensionNumber>(m_attachments->GetSelectedAttachment()),
+                      m_motion_plus_setting.GetValue());
 
   // Reset sub-devices.
   m_speaker_logic.Reset();
@@ -460,6 +461,10 @@ DesiredWiimoteState Wiimote::BuildDesiredWiimoteState()
       Common::Vec2(m_fov_x_setting.GetValue(), m_fov_y_setting.GetValue()) / 360 *
           float(MathUtil::TAU));
 
+  // Calculate MotionPlus state.
+  if (m_motion_plus_setting.GetValue())
+    wiimote_state.motion_plus = MotionPlus::GetGyroscopeData(GetTotalAngularVelocity());
+
   return wiimote_state;
 }
 
@@ -475,7 +480,8 @@ void Wiimote::Update()
   UpdateButtonsStatus(target_state);
 
   // If a new extension is requested in the GUI the change will happen here.
-  HandleExtensionSwap();
+  HandleExtensionSwap(static_cast<ExtensionNumber>(m_attachments->GetSelectedAttachment()),
+                      target_state.motion_plus.has_value());
 
   // Allow extension to perform any regular duties it may need.
   // (e.g. Nunchuk motion simulation step)
@@ -579,7 +585,9 @@ void Wiimote::SendDataReport(const DesiredWiimoteState& target_state)
       if (m_is_motion_plus_attached)
       {
         // TODO: Make input preparation triggered by bus read.
-        m_motion_plus.PrepareInput(GetTotalAngularVelocity());
+        m_motion_plus.PrepareInput(target_state.motion_plus.has_value() ?
+                                       target_state.motion_plus.value() :
+                                       MotionPlus::GetDefaultGyroscopeData());
       }
 
       u8* ext_data = rpt_builder.GetExtDataPtr();

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

@@ -190,7 +190,7 @@ private:
   template <typename T, typename H>
   void InvokeHandler(H&& handler, const WiimoteCommon::OutputReportGeneric& rpt, u32 size);
 
-  void HandleExtensionSwap();
+  void HandleExtensionSwap(ExtensionNumber desired_extension_number, bool desired_motion_plus);
   bool ProcessExtensionPortEvent();
   void SendDataReport(const DesiredWiimoteState& target_state);
   bool ProcessReadDataRequest();