InputCommon: Add calibration functionality to IMUGyroscope.

4 files changed, 132 insertions, 15 deletions

diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
index 61b291c96b..3030601331 100644
--- a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
+++ b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.cpp
@@ -159,10 +159,12 @@ bool IsCalibrationDataSensible(const ControllerEmu::ReshapableInput::Calibration
   // Even the GC controller's small range would pass this test.
   constexpr double REASONABLE_AVERAGE_RADIUS = 0.6;
 
-  const double sum = std::accumulate(data.begin(), data.end(), 0.0);
-  const double mean = sum / data.size();
+  MathUtil::RunningVariance<ControlState> stats;
 
-  if (mean < REASONABLE_AVERAGE_RADIUS)
+  for (auto& x : data)
+    stats.Push(x);
+
+  if (stats.Mean() < REASONABLE_AVERAGE_RADIUS)
   {
     return false;
   }
@@ -173,11 +175,7 @@ bool IsCalibrationDataSensible(const ControllerEmu::ReshapableInput::Calibration
   // Approx. deviation of a square input gate, anything much more than that would be unusual.
   constexpr double REASONABLE_DEVIATION = 0.14;
 
-  // Population standard deviation.
-  const double square_sum = std::inner_product(data.begin(), data.end(), data.begin(), 0.0);
-  const double standard_deviation = std::sqrt(square_sum / data.size() - mean * mean);
-
-  return standard_deviation < REASONABLE_DEVIATION;
+  return stats.StandardDeviation() < REASONABLE_DEVIATION;
 }
 
 // Used to test for a miscalibrated stick so the user can be informed.
@@ -805,6 +803,8 @@ void GyroMappingIndicator::paintEvent(QPaintEvent*)
   const auto gyro_state = m_gyro_group.GetState();
   const auto raw_gyro_state = m_gyro_group.GetRawState();
   const auto angular_velocity = gyro_state.value_or(Common::Vec3{});
+  const auto jitter = raw_gyro_state - m_previous_velocity;
+  m_previous_velocity = raw_gyro_state;
 
   m_state *= Common::Matrix33::FromQuaternion(angular_velocity.x / -INDICATOR_UPDATE_FREQ / 2,
                                               angular_velocity.y / INDICATOR_UPDATE_FREQ / 2,
@@ -855,12 +855,12 @@ void GyroMappingIndicator::paintEvent(QPaintEvent*)
 
     if (gyro_state.has_value())
     {
-      const auto max_velocity = std::max(
-          {std::abs(raw_gyro_state.x), std::abs(raw_gyro_state.y), std::abs(raw_gyro_state.z)});
-      const auto max_velocity_line_y =
-          std::min(max_velocity / deadzone_value * DEADZONE_DRAW_SIZE - DEADZONE_DRAW_BOTTOM, 1.0);
+      const auto max_jitter =
+          std::max({std::abs(jitter.x), std::abs(jitter.y), std::abs(jitter.z)});
+      const auto jitter_line_y =
+          std::min(max_jitter / deadzone_value * DEADZONE_DRAW_SIZE - DEADZONE_DRAW_BOTTOM, 1.0);
       p.setPen(QPen(GetRawInputColor(), INPUT_DOT_RADIUS));
-      p.drawLine(-scale, max_velocity_line_y * -scale, scale, max_velocity_line_y * -scale);
+      p.drawLine(-scale, jitter_line_y * -scale, scale, jitter_line_y * -scale);
 
       // Sphere background.
       p.setPen(Qt::NoPen);
@@ -881,7 +881,10 @@ void GyroMappingIndicator::paintEvent(QPaintEvent*)
   });
 
   // Sphere outline.
-  p.setPen(is_stable ? GetRawInputColor() : GetAdjustedInputColor());
+  const auto outline_color = is_stable ?
+                                 (m_gyro_group.IsCalibrating() ? Qt::blue : GetRawInputColor()) :
+                                 GetAdjustedInputColor();
+  p.setPen(outline_color);
   p.setBrush(Qt::NoBrush);
   p.drawEllipse(QPointF{}, scale * SPHERE_SIZE, scale * SPHERE_SIZE);
 
diff --git a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h
index 8928f8db72..2405ebcc0c 100644
--- a/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h
+++ b/Source/Core/DolphinQt/Config/Mapping/MappingIndicator.h
@@ -101,6 +101,7 @@ public:
 private:
   ControllerEmu::IMUGyroscope& m_gyro_group;
   Common::Matrix33 m_state;
+  Common::Vec3 m_previous_velocity = {};
   u32 m_stable_steps = 0;
 };
 
diff --git a/Source/Core/InputCommon/ControllerEmu/ControlGroup/IMUGyroscope.cpp b/Source/Core/InputCommon/ControllerEmu/ControlGroup/IMUGyroscope.cpp
index 6109f20b8c..8837c67f13 100644
--- a/Source/Core/InputCommon/ControllerEmu/ControlGroup/IMUGyroscope.cpp
+++ b/Source/Core/InputCommon/ControllerEmu/ControlGroup/IMUGyroscope.cpp
@@ -15,6 +15,15 @@
 
 namespace ControllerEmu
 {
+// Maximum period for calculating an average stable value.
+// Just to prevent failures due to timer overflow.
+static constexpr auto MAXIMUM_CALIBRATION_DURATION = std::chrono::hours(1);
+
+// If calibration updates do not happen at this rate, restart calibration period.
+// This prevents calibration across periods of no regular updates. (e.g. between game sessions)
+// This is made slightly lower than the UI update frequency of 30.
+static constexpr auto WORST_ACCEPTABLE_CALIBRATION_UPDATE_FREQUENCY = 25;
+
 IMUGyroscope::IMUGyroscope(std::string name, std::string ui_name)
     : ControlGroup(std::move(name), std::move(ui_name), GroupType::IMUGyroscope)
 {
@@ -31,7 +40,79 @@ IMUGyroscope::IMUGyroscope(std::string name, std::string ui_name)
               _trans("°/s"),
               // i18n: Refers to the dead-zone setting of gyroscope input.
               _trans("Angular velocity to ignore.")},
-             1, 0, 180);
+             2, 0, 180);
+
+  AddSetting(&m_calibration_period_setting,
+             {_trans("Calibration Period"),
+              // i18n: "s" is the symbol for seconds.
+              _trans("s"),
+              // i18n: Refers to the "Calibration" setting of gyroscope input.
+              _trans("Time period of stable input to trigger calibration. (zero to disable)")},
+             3, 0, 30);
+}
+
+void IMUGyroscope::RestartCalibration() const
+{
+  m_calibration_period_start = Clock::now();
+  m_running_calibration.Clear();
+}
+
+void IMUGyroscope::UpdateCalibration(const StateData& state) const
+{
+  const auto now = Clock::now();
+  const auto calibration_period = m_calibration_period_setting.GetValue();
+
+  // If calibration time is zero. User is choosing to not calibrate.
+  if (!calibration_period)
+  {
+    // Set calibration to zero.
+    m_calibration = {};
+    RestartCalibration();
+    return;
+  }
+
+  // If there is no running calibration a new gyro was just mapped or calibration was just enabled,
+  // apply the current state as calibration, it's often better than zeros.
+  if (!m_running_calibration.Count())
+  {
+    m_calibration = state;
+  }
+  else
+  {
+    const auto calibration_freq =
+        m_running_calibration.Count() /
+        std::chrono::duration_cast<std::chrono::duration<double>>(now - m_calibration_period_start)
+            .count();
+
+    const auto potential_calibration = m_running_calibration.Mean();
+    const auto current_difference = state - potential_calibration;
+    const auto deadzone = GetDeadzone();
+
+    // Check for required calibration update frequency
+    // and if current data is within deadzone distance of mean stable value.
+    if (calibration_freq < WORST_ACCEPTABLE_CALIBRATION_UPDATE_FREQUENCY ||
+        std::any_of(current_difference.data.begin(), current_difference.data.end(),
+                    [&](auto c) { return std::abs(c) > deadzone; }))
+    {
+      RestartCalibration();
+    }
+  }
+
+  // Update running mean stable value.
+  m_running_calibration.Push(state);
+
+  // Apply calibration after configured time.
+  const auto calibration_duration = now - m_calibration_period_start;
+  if (calibration_duration >= std::chrono::duration<double>(calibration_period))
+  {
+    m_calibration = m_running_calibration.Mean();
+
+    if (calibration_duration >= MAXIMUM_CALIBRATION_DURATION)
+    {
+      RestartCalibration();
+      m_running_calibration.Push(m_calibration);
+    }
+  }
 }
 
 auto IMUGyroscope::GetRawState() const -> StateData
@@ -44,10 +125,21 @@ auto IMUGyroscope::GetRawState() const -> StateData
 std::optional<IMUGyroscope::StateData> IMUGyroscope::GetState() const
 {
   if (controls[0]->control_ref->BoundCount() == 0)
+  {
+    // Set calibration to zero.
+    m_calibration = {};
+    RestartCalibration();
     return std::nullopt;
+  }
 
   auto state = GetRawState();
 
+  // If the input gate is disabled, miscalibration to zero values would occur.
+  if (ControlReference::GetInputGate())
+    UpdateCalibration(state);
+
+  state -= m_calibration;
+
   // Apply "deadzone".
   for (auto& c : state.data)
     c *= std::abs(c) > GetDeadzone();
@@ -60,4 +152,11 @@ ControlState IMUGyroscope::GetDeadzone() const
   return m_deadzone_setting.GetValue() / 360 * MathUtil::TAU;
 }
 
+bool IMUGyroscope::IsCalibrating() const
+{
+  const auto calibration_period = m_calibration_period_setting.GetValue();
+  return calibration_period && (Clock::now() - m_calibration_period_start) >=
+                                   std::chrono::duration<double>(calibration_period);
+}
+
 }  // namespace ControllerEmu
diff --git a/Source/Core/InputCommon/ControllerEmu/ControlGroup/IMUGyroscope.h b/Source/Core/InputCommon/ControllerEmu/ControlGroup/IMUGyroscope.h
index 1258a3eb76..459f885882 100644
--- a/Source/Core/InputCommon/ControllerEmu/ControlGroup/IMUGyroscope.h
+++ b/Source/Core/InputCommon/ControllerEmu/ControlGroup/IMUGyroscope.h
@@ -4,9 +4,11 @@
 
 #pragma once
 
+#include <chrono>
 #include <optional>
 #include <string>
 
+#include "Common/MathUtil.h"
 #include "Common/Matrix.h"
 #include "InputCommon/ControllerEmu/ControlGroup/ControlGroup.h"
 #include "InputCommon/ControllerEmu/Setting/NumericSetting.h"
@@ -26,7 +28,19 @@ public:
   // Value is in rad/s.
   ControlState GetDeadzone() const;
 
+  bool IsCalibrating() const;
+
 private:
+  using Clock = std::chrono::steady_clock;
+
+  void RestartCalibration() const;
+  void UpdateCalibration(const StateData&) const;
+
   SettingValue<double> m_deadzone_setting;
+  SettingValue<double> m_calibration_period_setting;
+
+  mutable StateData m_calibration = {};
+  mutable MathUtil::RunningMean<StateData> m_running_calibration;
+  mutable Clock::time_point m_calibration_period_start = Clock::now();
 };
 }  // namespace ControllerEmu