129 lines
2.5 KiB
C
129 lines
2.5 KiB
C
#include "irs.h"
|
|
#include <math.h>
|
|
|
|
void IRS_init(IRS* irs)
|
|
{
|
|
irs->q.w = 1.0f;
|
|
irs->q.x = 0.0f;
|
|
irs->q.y = 0.0f;
|
|
irs->q.z = 0.0f;
|
|
}
|
|
|
|
void IRS_update(IRS* irs, float dt)
|
|
{
|
|
Quaternion qConjugate = QuatConjugate(&irs->q);
|
|
|
|
// gyro update
|
|
Vector3 gyro =
|
|
{
|
|
irs->gyro.x * DEG2RAD,
|
|
irs->gyro.y * DEG2RAD,
|
|
irs->gyro.z * DEG2RAD
|
|
};
|
|
|
|
Quaternion g = {gyro.x, gyro.y, gyro.z, 0};
|
|
g = QuatProd(&irs->q, &g);
|
|
g = QuatConstProd(&g, dt * 0.5f);
|
|
irs->q = QuatSum(&irs->q, &g);
|
|
irs->q = QuatNormalize(&irs->q, 1.0f);
|
|
|
|
// /gyro update
|
|
|
|
// accel update
|
|
|
|
Vector3 accel = {irs->accel.x, irs->accel.y, -irs->accel.z};
|
|
restoreQuat(irs, &accel);
|
|
|
|
// /accel update
|
|
}
|
|
|
|
void restoreQuat(IRS* irs, const Vector3* accel)
|
|
{
|
|
float len = lengthV3(accel);
|
|
|
|
static float quat_acc_alpha = 0.03f;
|
|
static float quat_acc_max = 0.02f;
|
|
|
|
float dyn = fabsf(len - 1.0f);
|
|
if (dyn > quat_acc_max) dyn = 1.0f; else dyn /= quat_acc_max;
|
|
|
|
float gain = quat_acc_alpha * (1.0f - dyn);
|
|
|
|
if (gain < 0.0001f) return;
|
|
|
|
Vector3 acc = normalizeV3(accel, 1.0f);
|
|
|
|
Vector3 est;
|
|
est.x = 2.0f * (irs->q.x * irs->q.z - irs->q.w * irs->q.y);
|
|
est.y = 2.0f * (irs->q.w * irs->q.x - irs->q.y * irs->q.z);
|
|
est.z = irs->q.w * irs->q.w - irs->q.x * irs->q.x - irs->q.y * irs->q.y + irs->q.z * irs->q.z;
|
|
|
|
Vector3 cross = Cross(&acc, &est);
|
|
float dot = DotV3(&acc, &est);
|
|
|
|
if (dot < 0.0f)
|
|
{
|
|
float error_len = lengthV3(&cross);
|
|
if (error_len < 0.001f) {
|
|
cross.x = 1.0f;
|
|
cross.y = 0.0f;
|
|
cross.z = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
cross = constProdV3(&cross, 1.0f / error_len);
|
|
}
|
|
}
|
|
|
|
Vector3 axis = constProdV3(&cross, gain * 0.5f);
|
|
|
|
Quaternion correction =
|
|
{
|
|
axis.x,
|
|
axis.y,
|
|
axis.z,
|
|
1.0f
|
|
};
|
|
|
|
irs->q = QuatProd(&irs->q, &correction);
|
|
irs->q = QuatNormalize(&irs->q, 1.0f);
|
|
}
|
|
|
|
void setAccelShift(IRS* irs, const float roll, const float pitch, const float yaw)
|
|
{
|
|
float h_roll = (roll * DEG2RAD) / 2.0f;
|
|
float h_pitch = (pitch * DEG2RAD) / 2.0f;
|
|
float h_yaw = (yaw * DEG2RAD) / 2.0f;
|
|
|
|
Quaternion q_roll = {0.0f, sinf(h_roll), 0.0f, cosf(h_roll)};
|
|
Quaternion q_pitch = {sinf(h_pitch), 0.0f, 0.0f, cosf(h_pitch)};
|
|
Quaternion q_yaw = {0.0f, 0.0f, sinf(h_yaw), cosf(h_yaw)};
|
|
|
|
Quaternion prProd = QuatProd(&q_pitch, &q_roll);
|
|
Quaternion pryProd = QuatProd(&prProd, &q_yaw);
|
|
|
|
irs->shiftAccelPRY = pryProd;
|
|
}
|
|
|
|
Vector3 IRS_getGravity(const Quaternion* q)
|
|
{
|
|
Vector3 g =
|
|
{
|
|
2 * (q->x*q->z - q->w*q->y),
|
|
2 * (q->w*q->x + q->y*q->z),
|
|
q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z
|
|
};
|
|
|
|
return g;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|