service/hid: Improve console motion accuracy

This commit is contained in:
Narr the Reg 2021-11-29 12:40:29 -06:00 committed by german77
parent 815189eaf3
commit 316f80af87
6 changed files with 32 additions and 20 deletions

View file

@ -157,7 +157,10 @@ void EmulatedConsole::SetMotion(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);
} }

View file

@ -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{};
}; };

View file

@ -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;
} }

View file

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

View file

@ -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

View file

@ -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{};
}; };