WiimoteEmu: Clean up ComplementaryFilter math.

This commit is contained in:
Jordan Woyak 2020-01-03 16:16:26 -06:00
parent 120c6dc850
commit 72c2be52ed
5 changed files with 64 additions and 95 deletions

View File

@ -54,6 +54,29 @@ double CalculateStopDistance(double velocity, double max_accel)
return velocity * velocity / (2 * std::copysign(max_accel, velocity)); return velocity * velocity / (2 * std::copysign(max_accel, velocity));
} }
Common::Matrix33 ComplementaryFilter(const Common::Vec3& accelerometer,
const Common::Matrix33& gyroscope, float accel_weight)
{
const auto gyro_vec = gyroscope * Common::Vec3{0, 0, 1};
const auto normalized_accel = accelerometer.Normalized();
const auto cos_angle = normalized_accel.Dot(gyro_vec);
// If gyro to accel angle difference is betwen 0 and 180 degrees we make an adjustment.
const auto abs_cos_angle = std::abs(cos_angle);
if (abs_cos_angle > 0 && abs_cos_angle < 1)
{
INFO_LOG(WIIMOTE, "angle diff: %lf", std::acos(cos_angle) * 360 / MathUtil::TAU);
const auto axis = gyro_vec.Cross(normalized_accel).Normalized();
return Common::Matrix33::Rotate(std::acos(cos_angle) * accel_weight, axis) * gyroscope;
}
else
{
return gyroscope;
}
}
} // namespace } // namespace
namespace WiimoteEmu namespace WiimoteEmu
@ -271,94 +294,31 @@ void ApproachAngleWithAccel(RotationalState* state, const Common::Vec3& angle_ta
} }
} }
static Common::Vec3 NormalizeAngle(Common::Vec3 angle) void EmulateIMUCursor(Common::Matrix33* state, ControllerEmu::IMUCursor* imu_ir_group,
{
// TODO: There must be a more elegant way to do this
angle.x = fmod(angle.x, float(MathUtil::TAU));
angle.y = fmod(angle.y, float(MathUtil::TAU));
angle.z = fmod(angle.z, float(MathUtil::TAU));
angle.x += angle.x < 0 ? float(MathUtil::TAU) : 0;
angle.y += angle.y < 0 ? float(MathUtil::TAU) : 0;
angle.z += angle.z < 0 ? float(MathUtil::TAU) : 0;
return angle;
}
static Common::Vec3 ComplementaryFilter(const Common::Vec3& angle,
const Common::Vec3& accelerometer,
const Common::Vec3& gyroscope, float time_elapsed)
{
Common::Vec3 gyroangle = angle + gyroscope * time_elapsed;
gyroangle = NormalizeAngle(gyroangle);
// Calculate accelerometer tilt angles
Common::Vec3 accangle = gyroangle;
if ((accelerometer.x != 0 && accelerometer.y != 0) || accelerometer.z != 0)
{
float accpitch = -atan2(accelerometer.y, -accelerometer.z) + float(MathUtil::PI);
float accroll = atan2(accelerometer.x, -accelerometer.z) + float(MathUtil::PI);
accangle = {accpitch, accroll, gyroangle.z};
}
// Massage accelerometer and gyroscope angle values so that averaging them works when they are on
// opposite sides of TAU / zero (which both represent the same angle)
// TODO: There must be a more elegant way to do this
constexpr float DEG360 = float(MathUtil::TAU);
constexpr float DEG270 = DEG360 * 0.75f;
constexpr float DEG90 = DEG360 * 0.25f;
if (accangle.x < DEG90 && gyroangle.x > DEG270)
accangle.x += DEG360;
else if (gyroangle.x < DEG90 && accangle.x > DEG270)
gyroangle.x += DEG360;
if (accangle.y < DEG90 && gyroangle.y > DEG270)
accangle.y += DEG360;
else if (gyroangle.y < DEG90 && accangle.y > DEG270)
gyroangle.y += DEG360;
// Combine accelerometer and gyroscope angles
return NormalizeAngle((gyroangle * 0.98f) + (accangle * 0.02f));
}
void EmulateIMUCursor(std::optional<RotationalState>* state, ControllerEmu::IMUCursor* imu_ir_group,
ControllerEmu::IMUAccelerometer* imu_accelerometer_group, ControllerEmu::IMUAccelerometer* imu_accelerometer_group,
ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed) ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed)
{ {
// Avoid having to double dereference
auto& st = *state;
if (!imu_ir_group->enabled)
{
st = std::nullopt;
return;
}
auto accel = imu_accelerometer_group->GetState();
auto ang_vel = imu_gyroscope_group->GetState(); auto ang_vel = imu_gyroscope_group->GetState();
// The IMU Cursor requires both an accelerometer and a gyroscope to function correctly. // Recenter if we have no gyro data or the "Recenter" button is pressed.
if (!(accel.has_value() && ang_vel.has_value())) if (!ang_vel.has_value() || imu_ir_group->controls[0]->control_ref->State() >
ControllerEmu::Buttons::ACTIVATION_THRESHOLD)
{ {
st = std::nullopt; *state = Common::Matrix33::Identity();
return; return;
} }
if (!st.has_value()) // Apply rotation from gyro data.
st = RotationalState{}; const auto rotation = Common::Matrix33::FromQuaternion(ang_vel->x * time_elapsed / -2,
ang_vel->y * time_elapsed / -2,
ang_vel->z * time_elapsed / -2, 1);
*state = rotation * *state;
st->angle = ComplementaryFilter(st->angle, accel.value(), ang_vel.value(), time_elapsed); // If we have usable accel data use it to adjust gyro drift.
constexpr float accel_weight = 0.02;
// Reset camera yaw angle auto const accel = imu_accelerometer_group->GetState().value_or(Common::Vec3{});
constexpr ControlState BUTTON_THRESHOLD = 0.5; if (accel.LengthSquared())
if (imu_ir_group->controls[0]->control_ref->State() > BUTTON_THRESHOLD) *state = ComplementaryFilter(accel, *state, accel_weight);
st->angle.z = 0;
// Limit camera yaw angle
float totalyaw = float(imu_ir_group->GetTotalYaw());
float yawmax = totalyaw / 2;
float yawmin = float(MathUtil::TAU) - totalyaw / 2;
if (st->angle.z > yawmax && st->angle.z <= float(MathUtil::PI))
st->angle.z = yawmax;
if (st->angle.z < yawmin && st->angle.z > float(MathUtil::PI))
st->angle.z = yawmin;
} }
void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& position_target, void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& position_target,
@ -398,6 +358,18 @@ void ApproachPositionWithJerk(PositionalState* state, const Common::Vec3& positi
} }
} }
Common::Matrix33 GetMatrixFromAcceleration(const Common::Vec3& accel)
{
const auto normalized_accel = accel.Normalized();
const auto angle = std::acos(normalized_accel.Dot({0, 0, 1}));
const auto axis = normalized_accel.Cross({0, 0, 1});
// Check that axis is non-zero to handle perfect up/down orientations.
return Common::Matrix33::Rotate(angle,
axis.LengthSquared() ? axis.Normalized() : Common::Vec3{0, 1, 0});
}
Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle) Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle)
{ {
return Common::Matrix33::RotateZ(angle.z) * Common::Matrix33::RotateY(angle.y) * return Common::Matrix33::RotateZ(angle.z) * Common::Matrix33::RotateY(angle.y) *

View File

@ -44,6 +44,9 @@ struct MotionState : PositionalState, RotationalState
{ {
}; };
// Estimate orientation from accelerometer data.
Common::Matrix33 GetMatrixFromAcceleration(const Common::Vec3& accel);
// Build a rotational matrix from euler angles. // Build a rotational matrix from euler angles.
Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle); Common::Matrix33 GetRotationalMatrix(const Common::Vec3& angle);
@ -57,7 +60,7 @@ void EmulateShake(PositionalState* state, ControllerEmu::Shake* shake_group, flo
void EmulateTilt(RotationalState* state, ControllerEmu::Tilt* tilt_group, float time_elapsed); void EmulateTilt(RotationalState* state, ControllerEmu::Tilt* tilt_group, float time_elapsed);
void EmulateSwing(MotionState* state, ControllerEmu::Force* swing_group, float time_elapsed); void EmulateSwing(MotionState* state, ControllerEmu::Force* swing_group, float time_elapsed);
void EmulateCursor(MotionState* state, ControllerEmu::Cursor* ir_group, float time_elapsed); void EmulateCursor(MotionState* state, ControllerEmu::Cursor* ir_group, float time_elapsed);
void EmulateIMUCursor(std::optional<RotationalState>* state, ControllerEmu::IMUCursor* imu_ir_group, void EmulateIMUCursor(Common::Matrix33* state, ControllerEmu::IMUCursor* imu_ir_group,
ControllerEmu::IMUAccelerometer* imu_accelerometer_group, ControllerEmu::IMUAccelerometer* imu_accelerometer_group,
ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed); ControllerEmu::IMUGyroscope* imu_gyroscope_group, float time_elapsed);

View File

@ -190,7 +190,7 @@ void Wiimote::Reset()
m_tilt_state = {}; m_tilt_state = {};
m_cursor_state = {}; m_cursor_state = {};
m_shake_state = {}; m_shake_state = {};
m_imu_cursor_state = {}; m_imu_cursor_state = Common::Matrix33::Identity();
} }
Wiimote::Wiimote(const unsigned int index) : m_index(index) Wiimote::Wiimote(const unsigned int index) : m_index(index)
@ -812,12 +812,13 @@ Common::Vec3 Wiimote::GetAngularVelocity(Common::Vec3 extra_angular_velocity)
Common::Matrix44 Wiimote::GetTransformation(Common::Vec3 extra_rotation) const Common::Matrix44 Wiimote::GetTransformation(Common::Vec3 extra_rotation) const
{ {
// Includes positional and rotational effects of: // Includes positional and rotational effects of:
// Cursor, Swing, Tilt, Shake // Point, Swing, Tilt, Shake
// TODO: Think about and clean up matrix order + make nunchuk match. // TODO: Think about and clean up matrix order + make nunchuk match.
return Common::Matrix44::Translate(-m_shake_state.position) * return Common::Matrix44::Translate(-m_shake_state.position) *
Common::Matrix44::FromMatrix33(GetRotationalMatrix( Common::Matrix44::FromMatrix33(
-m_tilt_state.angle - m_swing_state.angle - m_cursor_state.angle - extra_rotation)) * m_imu_cursor_state * GetRotationalMatrix(-m_tilt_state.angle - m_swing_state.angle -
m_cursor_state.angle - extra_rotation)) *
Common::Matrix44::Translate(-m_swing_state.position - m_cursor_state.position); Common::Matrix44::Translate(-m_swing_state.position - m_cursor_state.position);
} }
@ -836,6 +837,7 @@ Common::Vec3 Wiimote::GetTotalAcceleration()
return GetAcceleration(); return GetAcceleration();
} }
// TODO: Kill this function.
Common::Vec3 Wiimote::GetTotalAngularVelocity() Common::Vec3 Wiimote::GetTotalAngularVelocity()
{ {
auto ang_vel = m_imu_gyroscope->GetState(); auto ang_vel = m_imu_gyroscope->GetState();
@ -845,13 +847,10 @@ Common::Vec3 Wiimote::GetTotalAngularVelocity()
return GetAngularVelocity(); return GetAngularVelocity();
} }
// TODO: Kill this function.
Common::Matrix44 Wiimote::GetTotalTransformation() const Common::Matrix44 Wiimote::GetTotalTransformation() const
{ {
auto state = m_imu_cursor_state; return GetTransformation();
if (state.has_value())
return GetTransformation(state->angle);
else
return GetTransformation();
} }
} // namespace WiimoteEmu } // namespace WiimoteEmu

View File

@ -300,6 +300,6 @@ private:
RotationalState m_tilt_state; RotationalState m_tilt_state;
MotionState m_cursor_state; MotionState m_cursor_state;
PositionalState m_shake_state; PositionalState m_shake_state;
std::optional<RotationalState> m_imu_cursor_state; Common::Matrix33 m_imu_cursor_state;
}; };
} // namespace WiimoteEmu } // namespace WiimoteEmu

View File

@ -723,12 +723,7 @@ void AccelerometerMappingIndicator::paintEvent(QPaintEvent*)
p.setRenderHint(QPainter::Antialiasing, true); p.setRenderHint(QPainter::Antialiasing, true);
p.setRenderHint(QPainter::SmoothPixmapTransform, true); p.setRenderHint(QPainter::SmoothPixmapTransform, true);
const auto angle = std::acos(state.Normalized().Dot({0, 0, 1})); const auto rotation = WiimoteEmu::GetMatrixFromAcceleration(state);
const auto axis = state.Normalized().Cross({0, 0, 1});
// Check that axis is non-zero to handle perfect up/down orientations.
const auto rotation = Common::Matrix33::Rotate(
angle, axis.LengthSquared() ? axis.Normalized() : Common::Vec3{0, 1, 0});
// Draw sphere. // Draw sphere.
p.setPen(Qt::NoPen); p.setPen(Qt::NoPen);