core: hid: Allow to calibrate gyro sensor

This commit is contained in:
Narr the Reg 2023-05-08 11:17:27 -06:00
parent b70a205a96
commit 97bd6d6418
5 changed files with 43 additions and 1 deletions

View file

@ -688,6 +688,12 @@ void EmulatedController::SetMotionParam(std::size_t index, Common::ParamPackage
ReloadInput(); ReloadInput();
} }
void EmulatedController::StartMotionCalibration() {
for (ControllerMotionInfo& motion : controller.motion_values) {
motion.emulated.Calibrate();
}
}
void EmulatedController::SetButton(const Common::Input::CallbackStatus& callback, std::size_t index, void EmulatedController::SetButton(const Common::Input::CallbackStatus& callback, std::size_t index,
Common::UUID uuid) { Common::UUID uuid) {
if (index >= controller.button_values.size()) { if (index >= controller.button_values.size()) {

View file

@ -290,6 +290,9 @@ public:
*/ */
void SetMotionParam(std::size_t index, Common::ParamPackage param); void SetMotionParam(std::size_t index, Common::ParamPackage param);
/// Auto calibrates the current motion devices
void StartMotionCalibration();
/// Returns the latest button status from the controller with parameters /// Returns the latest button status from the controller with parameters
ButtonValues GetButtonsValues() const; ButtonValues GetButtonsValues() const;

View file

@ -37,11 +37,17 @@ void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) {
gyro.y = std::clamp(gyro.y, -GyroMaxValue, GyroMaxValue); gyro.y = std::clamp(gyro.y, -GyroMaxValue, GyroMaxValue);
gyro.z = std::clamp(gyro.z, -GyroMaxValue, GyroMaxValue); gyro.z = std::clamp(gyro.z, -GyroMaxValue, GyroMaxValue);
// Auto adjust drift to minimize drift // Auto adjust gyro_bias to minimize drift
if (!IsMoving(IsAtRestRelaxed)) { if (!IsMoving(IsAtRestRelaxed)) {
gyro_bias = (gyro_bias * 0.9999f) + (gyroscope * 0.0001f); gyro_bias = (gyro_bias * 0.9999f) + (gyroscope * 0.0001f);
} }
// Adjust drift when calibration mode is enabled
if (calibration_mode) {
gyro_bias = (gyro_bias * 0.99f) + (gyroscope * 0.01f);
StopCalibration();
}
if (gyro.Length() < gyro_threshold * user_gyro_threshold) { if (gyro.Length() < gyro_threshold * user_gyro_threshold) {
gyro = {}; gyro = {};
} else { } else {
@ -107,6 +113,19 @@ void MotionInput::UpdateRotation(u64 elapsed_time) {
rotations += gyro * sample_period; rotations += gyro * sample_period;
} }
void MotionInput::Calibrate() {
calibration_mode = true;
calibration_counter = 0;
}
void MotionInput::StopCalibration() {
if (calibration_counter++ > CalibrationSamples) {
calibration_mode = false;
ResetQuaternion();
ResetRotations();
}
}
// Based on Madgwick's implementation of Mayhony's AHRS algorithm. // Based on Madgwick's implementation of Mayhony's AHRS algorithm.
// https://github.com/xioTechnologies/Open-Source-AHRS-With-x-IMU/blob/master/x-IMU%20IMU%20and%20AHRS%20Algorithms/x-IMU%20IMU%20and%20AHRS%20Algorithms/AHRS/MahonyAHRS.cs // https://github.com/xioTechnologies/Open-Source-AHRS-With-x-IMU/blob/master/x-IMU%20IMU%20and%20AHRS%20Algorithms/x-IMU%20IMU%20and%20AHRS%20Algorithms/AHRS/MahonyAHRS.cs
void MotionInput::UpdateOrientation(u64 elapsed_time) { void MotionInput::UpdateOrientation(u64 elapsed_time) {

View file

@ -23,6 +23,8 @@ public:
static constexpr float GyroMaxValue = 5.0f; static constexpr float GyroMaxValue = 5.0f;
static constexpr float AccelMaxValue = 7.0f; static constexpr float AccelMaxValue = 7.0f;
static constexpr std::size_t CalibrationSamples = 300;
explicit MotionInput(); explicit MotionInput();
MotionInput(const MotionInput&) = default; MotionInput(const MotionInput&) = default;
@ -49,6 +51,8 @@ public:
void UpdateRotation(u64 elapsed_time); void UpdateRotation(u64 elapsed_time);
void UpdateOrientation(u64 elapsed_time); void UpdateOrientation(u64 elapsed_time);
void Calibrate();
[[nodiscard]] std::array<Common::Vec3f, 3> GetOrientation() const; [[nodiscard]] std::array<Common::Vec3f, 3> GetOrientation() const;
[[nodiscard]] Common::Vec3f GetAcceleration() const; [[nodiscard]] Common::Vec3f GetAcceleration() const;
[[nodiscard]] Common::Vec3f GetGyroscope() const; [[nodiscard]] Common::Vec3f GetGyroscope() const;
@ -61,6 +65,7 @@ public:
[[nodiscard]] bool IsCalibrated(f32 sensitivity) const; [[nodiscard]] bool IsCalibrated(f32 sensitivity) const;
private: private:
void StopCalibration();
void ResetOrientation(); void ResetOrientation();
void SetOrientationFromAccelerometer(); void SetOrientationFromAccelerometer();
@ -103,6 +108,12 @@ private:
// Use accelerometer values to calculate position // Use accelerometer values to calculate position
bool only_accelerometer = true; bool only_accelerometer = true;
// When enabled it will aggressively adjust for gyro drift
bool calibration_mode = false;
// Used to auto disable calibration mode
std::size_t calibration_counter = 0;
}; };
} // namespace Core::HID } // namespace Core::HID

View file

@ -479,6 +479,9 @@ ConfigureInputPlayer::ConfigureInputPlayer(QWidget* parent, std::size_t player_i
param.Set("threshold", new_threshold / 1000.0f); param.Set("threshold", new_threshold / 1000.0f);
emulated_controller->SetMotionParam(motion_id, param); emulated_controller->SetMotionParam(motion_id, param);
}); });
context_menu.addAction(tr("Calibrate sensor"), [&] {
emulated_controller->StartMotionCalibration();
});
} }
context_menu.exec(motion_map[motion_id]->mapToGlobal(menu_location)); context_menu.exec(motion_map[motion_id]->mapToGlobal(menu_location));
}); });