82 lines
1.7 KiB
C
82 lines
1.7 KiB
C
#include "irs.h"
|
|
#include <math.h>
|
|
|
|
#define MAHONY_KP 2.5f
|
|
#define MAHONY_KI 0.01f
|
|
|
|
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 IRS* irs)
|
|
{
|
|
const Quaternion* q = &irs->q;
|
|
|
|
Vector3 g;
|
|
g.x = 2 * (q->x*q->z - q->w*q->y);
|
|
g.y = 2 * (q->w*q->x + q->y*q->z);
|
|
g.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;
|
|
|
|
// --- нормализация акселя
|
|
float norm = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
|
|
|
|
if (norm > 1e-6f)
|
|
{
|
|
accel.x /= norm;
|
|
accel.y /= norm;
|
|
accel.z /= norm;
|
|
|
|
Vector3 g = IRS_getGravity(irs);
|
|
|
|
Vector3 error =
|
|
{
|
|
accel.y*g.z - accel.z*g.y,
|
|
accel.z*g.x - accel.x*g.z,
|
|
accel.x*g.y - accel.y*g.x
|
|
};
|
|
|
|
gyro_bias.x += MAHONY_KI * error.x * dt;
|
|
gyro_bias.y += MAHONY_KI * error.y * dt;
|
|
gyro_bias.z += MAHONY_KI * error.z * dt;
|
|
|
|
gyro.x += MAHONY_KP * error.x + gyro_bias.x;
|
|
gyro.y += MAHONY_KP * error.y + gyro_bias.y;
|
|
gyro.z += MAHONY_KP * error.z + gyro_bias.z;
|
|
}
|
|
|
|
irs->gyro = gyro;
|
|
irs->accel = accel;
|
|
|
|
// --- интеграция кватерниона
|
|
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);
|
|
|
|
irs->q = q;
|
|
} |