103 lines
1.8 KiB
C
103 lines
1.8 KiB
C
#include "irs.h"
|
|
#include <math.h>
|
|
|
|
#define MAHONY_KP 2.5f
|
|
#define MAHONY_KI 0.01f
|
|
|
|
#define ACC_ALPHA 0.01f
|
|
#define ACC_MAX_ERROR 0.2f
|
|
|
|
static Vector3 gyro_bias = {0};
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
void IRS_update(IRS* irs, const Vector3* gyro_in, const Vector3* accel_in, float dt)
|
|
{
|
|
Vector3 gyro = *gyro_in;
|
|
Vector3 accel = {accel_in->x, accel_in->y, -accel_in->z};
|
|
|
|
// gyro intergate
|
|
Quaternion q = irs->q;
|
|
|
|
Quaternion omega =
|
|
{
|
|
gyro.x * DEG2RAD,
|
|
gyro.y * DEG2RAD,
|
|
gyro.z * DEG2RAD,
|
|
0
|
|
};
|
|
|
|
Quaternion dq = QuatProd(&q, &omega);
|
|
dq = QuatConstProd(&dq, 0.5f * dt);
|
|
|
|
q = QuatSum(&q, &dq);
|
|
q = QuatNormalize(&q, 1.0f);
|
|
|
|
// accel correction
|
|
float acc_len = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
|
|
|
|
if (acc_len > 1e-6f)
|
|
{
|
|
Vector3 acc_norm =
|
|
{
|
|
accel.x / acc_len,
|
|
accel.y / acc_len,
|
|
accel.z / acc_len
|
|
};
|
|
|
|
float dyn = fabsf(acc_len - 1.0f);
|
|
|
|
float trust;
|
|
if (dyn > ACC_MAX_ERROR)
|
|
trust = 0.0f;
|
|
else
|
|
trust = 1.0f - (dyn / ACC_MAX_ERROR);
|
|
|
|
float gain = ACC_ALPHA * trust;
|
|
|
|
if (gain > 1e-5f)
|
|
{
|
|
Vector3 est = IRS_getGravity(&q);
|
|
|
|
Vector3 error =
|
|
{
|
|
acc_norm.y*est.z - acc_norm.z*est.y,
|
|
acc_norm.z*est.x - acc_norm.x*est.z,
|
|
acc_norm.x*est.y - acc_norm.y*est.x
|
|
};
|
|
|
|
Quaternion corretrion =
|
|
{
|
|
error.x * gain * 0.5f,
|
|
error.y * gain * 0.5f,
|
|
error.z * gain * 0.5f,
|
|
1.0f
|
|
};
|
|
|
|
q = QuatProd(&corretrion, &q);
|
|
q = QuatNormalize(&q, 1.0f);
|
|
}
|
|
}
|
|
|
|
irs->q = q;
|
|
irs->gyro = gyro;
|
|
irs->accel = accel;
|
|
} |