Merge pull request #7481 from german77/gyro-bias
service/hid: Improve console motion accuracy
This commit is contained in:
commit
5e24f7ed31
|
@ -161,7 +161,10 @@ void EmulatedConsole::SetMotion(const Common::Input::CallbackStatus& callback) {
|
||||||
motion.rotation = emulated.GetGyroscope();
|
motion.rotation = emulated.GetGyroscope();
|
||||||
motion.orientation = emulated.GetOrientation();
|
motion.orientation = emulated.GetOrientation();
|
||||||
motion.quaternion = emulated.GetQuaternion();
|
motion.quaternion = emulated.GetQuaternion();
|
||||||
|
motion.gyro_bias = emulated.GetGyroBias();
|
||||||
motion.is_at_rest = !emulated.IsMoving(motion_sensitivity);
|
motion.is_at_rest = !emulated.IsMoving(motion_sensitivity);
|
||||||
|
// Find what is this value
|
||||||
|
motion.verticalization_error = 0.0f;
|
||||||
|
|
||||||
TriggerOnChange(ConsoleTriggerType::Motion);
|
TriggerOnChange(ConsoleTriggerType::Motion);
|
||||||
}
|
}
|
||||||
|
|
|
@ -50,6 +50,8 @@ struct ConsoleMotion {
|
||||||
Common::Vec3f rotation{};
|
Common::Vec3f rotation{};
|
||||||
std::array<Common::Vec3f, 3> orientation{};
|
std::array<Common::Vec3f, 3> orientation{};
|
||||||
Common::Quaternion<f32> quaternion{};
|
Common::Quaternion<f32> quaternion{};
|
||||||
|
Common::Vec3f gyro_bias{};
|
||||||
|
f32 verticalization_error{};
|
||||||
bool is_at_rest{};
|
bool is_at_rest{};
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -23,11 +23,11 @@ void MotionInput::SetAcceleration(const Common::Vec3f& acceleration) {
|
||||||
}
|
}
|
||||||
|
|
||||||
void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) {
|
void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) {
|
||||||
gyro = gyroscope - gyro_drift;
|
gyro = gyroscope - gyro_bias;
|
||||||
|
|
||||||
// Auto adjust drift to minimize drift
|
// Auto adjust drift to minimize drift
|
||||||
if (!IsMoving(0.1f)) {
|
if (!IsMoving(0.1f)) {
|
||||||
gyro_drift = (gyro_drift * 0.9999f) + (gyroscope * 0.0001f);
|
gyro_bias = (gyro_bias * 0.9999f) + (gyroscope * 0.0001f);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (gyro.Length2() < gyro_threshold) {
|
if (gyro.Length2() < gyro_threshold) {
|
||||||
|
@ -41,8 +41,8 @@ void MotionInput::SetQuaternion(const Common::Quaternion<f32>& quaternion) {
|
||||||
quat = quaternion;
|
quat = quaternion;
|
||||||
}
|
}
|
||||||
|
|
||||||
void MotionInput::SetGyroDrift(const Common::Vec3f& drift) {
|
void MotionInput::SetGyroBias(const Common::Vec3f& bias) {
|
||||||
gyro_drift = drift;
|
gyro_bias = bias;
|
||||||
}
|
}
|
||||||
|
|
||||||
void MotionInput::SetGyroThreshold(f32 threshold) {
|
void MotionInput::SetGyroThreshold(f32 threshold) {
|
||||||
|
@ -192,6 +192,10 @@ Common::Vec3f MotionInput::GetGyroscope() const {
|
||||||
return gyro;
|
return gyro;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Common::Vec3f MotionInput::GetGyroBias() const {
|
||||||
|
return gyro_bias;
|
||||||
|
}
|
||||||
|
|
||||||
Common::Quaternion<f32> MotionInput::GetQuaternion() const {
|
Common::Quaternion<f32> MotionInput::GetQuaternion() const {
|
||||||
return quat;
|
return quat;
|
||||||
}
|
}
|
||||||
|
|
|
@ -24,7 +24,7 @@ public:
|
||||||
void SetAcceleration(const Common::Vec3f& acceleration);
|
void SetAcceleration(const Common::Vec3f& acceleration);
|
||||||
void SetGyroscope(const Common::Vec3f& gyroscope);
|
void SetGyroscope(const Common::Vec3f& gyroscope);
|
||||||
void SetQuaternion(const Common::Quaternion<f32>& quaternion);
|
void SetQuaternion(const Common::Quaternion<f32>& quaternion);
|
||||||
void SetGyroDrift(const Common::Vec3f& drift);
|
void SetGyroBias(const Common::Vec3f& bias);
|
||||||
void SetGyroThreshold(f32 threshold);
|
void SetGyroThreshold(f32 threshold);
|
||||||
|
|
||||||
void EnableReset(bool reset);
|
void EnableReset(bool reset);
|
||||||
|
@ -36,6 +36,7 @@ public:
|
||||||
[[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;
|
||||||
|
[[nodiscard]] Common::Vec3f GetGyroBias() const;
|
||||||
[[nodiscard]] Common::Vec3f GetRotations() const;
|
[[nodiscard]] Common::Vec3f GetRotations() const;
|
||||||
[[nodiscard]] Common::Quaternion<f32> GetQuaternion() const;
|
[[nodiscard]] Common::Quaternion<f32> GetQuaternion() const;
|
||||||
|
|
||||||
|
@ -69,7 +70,7 @@ private:
|
||||||
Common::Vec3f gyro;
|
Common::Vec3f gyro;
|
||||||
|
|
||||||
// Vector to be substracted from gyro measurements
|
// Vector to be substracted from gyro measurements
|
||||||
Common::Vec3f gyro_drift;
|
Common::Vec3f gyro_bias;
|
||||||
|
|
||||||
// Minimum gyro amplitude to detect if the device is moving
|
// Minimum gyro amplitude to detect if the device is moving
|
||||||
f32 gyro_threshold = 0.0f;
|
f32 gyro_threshold = 0.0f;
|
||||||
|
|
|
@ -33,15 +33,14 @@ void Controller_ConsoleSixAxis::OnUpdate(const Core::Timing::CoreTiming& core_ti
|
||||||
const auto& last_entry = seven_sixaxis_lifo.ReadCurrentEntry().state;
|
const auto& last_entry = seven_sixaxis_lifo.ReadCurrentEntry().state;
|
||||||
next_seven_sixaxis_state.sampling_number = last_entry.sampling_number + 1;
|
next_seven_sixaxis_state.sampling_number = last_entry.sampling_number + 1;
|
||||||
|
|
||||||
// Try to read sixaxis sensor states
|
|
||||||
const auto motion_status = console->GetMotion();
|
const auto motion_status = console->GetMotion();
|
||||||
|
last_global_timestamp = core_timing.GetGlobalTimeNs().count();
|
||||||
|
|
||||||
console_six_axis.is_seven_six_axis_sensor_at_rest = motion_status.is_at_rest;
|
// This value increments every time the switch goes to sleep
|
||||||
|
next_seven_sixaxis_state.unknown = 1;
|
||||||
|
next_seven_sixaxis_state.timestamp = last_global_timestamp - last_saved_timestamp;
|
||||||
next_seven_sixaxis_state.accel = motion_status.accel;
|
next_seven_sixaxis_state.accel = motion_status.accel;
|
||||||
// Zero gyro values as they just mess up with the camera
|
next_seven_sixaxis_state.gyro = motion_status.gyro;
|
||||||
// Note: Probably a correct sensivity setting must be set
|
|
||||||
next_seven_sixaxis_state.gyro = {};
|
|
||||||
next_seven_sixaxis_state.quaternion = {
|
next_seven_sixaxis_state.quaternion = {
|
||||||
{
|
{
|
||||||
motion_status.quaternion.xyz.y,
|
motion_status.quaternion.xyz.y,
|
||||||
|
@ -52,9 +51,9 @@ void Controller_ConsoleSixAxis::OnUpdate(const Core::Timing::CoreTiming& core_ti
|
||||||
};
|
};
|
||||||
|
|
||||||
console_six_axis.sampling_number++;
|
console_six_axis.sampling_number++;
|
||||||
// TODO(German77): Find the purpose of those values
|
console_six_axis.is_seven_six_axis_sensor_at_rest = motion_status.is_at_rest;
|
||||||
console_six_axis.verticalization_error = 0.0f;
|
console_six_axis.verticalization_error = motion_status.verticalization_error;
|
||||||
console_six_axis.gyro_bias = {0.0f, 0.0f, 0.0f};
|
console_six_axis.gyro_bias = motion_status.gyro_bias;
|
||||||
|
|
||||||
// Update console six axis shared memory
|
// Update console six axis shared memory
|
||||||
std::memcpy(data + SHARED_MEMORY_OFFSET, &console_six_axis, sizeof(console_six_axis));
|
std::memcpy(data + SHARED_MEMORY_OFFSET, &console_six_axis, sizeof(console_six_axis));
|
||||||
|
@ -69,7 +68,6 @@ void Controller_ConsoleSixAxis::SetTransferMemoryPointer(u8* t_mem) {
|
||||||
}
|
}
|
||||||
|
|
||||||
void Controller_ConsoleSixAxis::ResetTimestamp() {
|
void Controller_ConsoleSixAxis::ResetTimestamp() {
|
||||||
seven_sixaxis_lifo.buffer_count = 0;
|
last_saved_timestamp = last_global_timestamp;
|
||||||
seven_sixaxis_lifo.buffer_tail = 0;
|
|
||||||
}
|
}
|
||||||
} // namespace Service::HID
|
} // namespace Service::HID
|
||||||
|
|
|
@ -39,8 +39,9 @@ public:
|
||||||
|
|
||||||
private:
|
private:
|
||||||
struct SevenSixAxisState {
|
struct SevenSixAxisState {
|
||||||
INSERT_PADDING_WORDS(4); // unused
|
INSERT_PADDING_WORDS(2); // unused
|
||||||
s64 sampling_number{};
|
u64 timestamp{};
|
||||||
|
u64 sampling_number{};
|
||||||
u64 unknown{};
|
u64 unknown{};
|
||||||
Common::Vec3f accel{};
|
Common::Vec3f accel{};
|
||||||
Common::Vec3f gyro{};
|
Common::Vec3f gyro{};
|
||||||
|
@ -52,9 +53,10 @@ private:
|
||||||
struct ConsoleSharedMemory {
|
struct ConsoleSharedMemory {
|
||||||
u64 sampling_number{};
|
u64 sampling_number{};
|
||||||
bool is_seven_six_axis_sensor_at_rest{};
|
bool is_seven_six_axis_sensor_at_rest{};
|
||||||
INSERT_PADDING_BYTES(4); // padding
|
INSERT_PADDING_BYTES(3); // padding
|
||||||
f32 verticalization_error{};
|
f32 verticalization_error{};
|
||||||
Common::Vec3f gyro_bias{};
|
Common::Vec3f gyro_bias{};
|
||||||
|
INSERT_PADDING_BYTES(4); // padding
|
||||||
};
|
};
|
||||||
static_assert(sizeof(ConsoleSharedMemory) == 0x20, "ConsoleSharedMemory is an invalid size");
|
static_assert(sizeof(ConsoleSharedMemory) == 0x20, "ConsoleSharedMemory is an invalid size");
|
||||||
|
|
||||||
|
@ -64,6 +66,8 @@ private:
|
||||||
Core::HID::EmulatedConsole* console;
|
Core::HID::EmulatedConsole* console;
|
||||||
u8* transfer_memory = nullptr;
|
u8* transfer_memory = nullptr;
|
||||||
bool is_transfer_memory_set = false;
|
bool is_transfer_memory_set = false;
|
||||||
|
u64 last_saved_timestamp{};
|
||||||
|
u64 last_global_timestamp{};
|
||||||
ConsoleSharedMemory console_six_axis{};
|
ConsoleSharedMemory console_six_axis{};
|
||||||
SevenSixAxisState next_seven_sixaxis_state{};
|
SevenSixAxisState next_seven_sixaxis_state{};
|
||||||
};
|
};
|
||||||
|
|
Reference in New Issue