From 3066b28f75b2495537374af8ef52cf1202318717 Mon Sep 17 00:00:00 2001 From: Radzhab Bisultanov Date: Tue, 31 Mar 2026 16:24:32 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9E=D0=BF=D1=80=D0=B5=D0=B4=D0=B5=D0=BB?= =?UTF-8?q?=D0=B5=D0=BD=D0=B8=D0=B5=20=D0=BE=D1=80=D0=B8=D0=B5=D0=BD=D1=82?= =?UTF-8?q?=D0=B0=D1=86=D0=B8=D0=B8=20=D0=BA=D0=B0=D0=BA=20=D0=B2=20=D1=80?= =?UTF-8?q?=D0=B0=D0=B1=D0=BE=D1=87=D0=B5=D0=B9=20=D0=BF=D1=80=D0=BE=D1=88?= =?UTF-8?q?=D0=B8=D0=B2=D0=BA=D0=B5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Source/INS/IRS.c | 91 +++----------------------------- Source/INS/IRS.h | 2 +- Source/INS/geometry/quaternion.c | 2 +- Source/main.c | 13 +++-- 4 files changed, 17 insertions(+), 91 deletions(-) diff --git a/Source/INS/IRS.c b/Source/INS/IRS.c index a287fe9..5a105b1 100644 --- a/Source/INS/IRS.c +++ b/Source/INS/IRS.c @@ -1,14 +1,6 @@ #include "irs.h" #include -#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; @@ -17,103 +9,32 @@ void IRS_init(IRS* irs) irs->q.z = 0.0f; } - -void IRS_update(IRS* irs, const Vector3* gyro_in, const Vector3* accel_in, float dt) +void IRS_update(IRS* irs, float dt) { Quaternion qConjugate = QuatConjugate(&irs->q); // gyro update - - irs->gyro = *gyro_in; - Vector3 gyro = { - gyro_in->x * dt * DEG2RAD, - gyro_in->y * dt * DEG2RAD, - gyro_in->z * dt * DEG2RAD + 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, 0.5f); + g = QuatConstProd(&g, dt * 0.5f); irs->q = QuatSum(&irs->q, &g); irs->q = QuatNormalize(&irs->q, 1.0f); - Quaternion q0010 = {0.0f, 0.0f, 1.0f, 0.0f}; - Quaternion conj0010prod = QuatProd(&qConjugate, &q0010); - Quaternion prtilts = QuatProd(&conj0010prod, &irs->q); - - irs->oriPRT.x = prtilts.x; - irs->oriPRT.y = prtilts.y; - irs->oriPRT.z = prtilts.z; - // /gyro update // accel update - Vector3 accel = {accel_in->x, accel_in->y, -accel_in->z}; - - irs->accel = accel; - + Vector3 accel = {irs->accel.x, irs->accel.y, -irs->accel.z}; restoreQuat(irs, &accel); - - - // /accel update - - - - /*Vector3 accel = {accel_in->x, accel_in->y, -accel_in->z}; - - 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;*/ } void restoreQuat(IRS* irs, const Vector3* accel) diff --git a/Source/INS/IRS.h b/Source/INS/IRS.h index ad59307..4a40770 100644 --- a/Source/INS/IRS.h +++ b/Source/INS/IRS.h @@ -20,7 +20,7 @@ typedef struct void IRS_init(IRS* irs); -void IRS_update(IRS* irs, const Vector3* gyro, const Vector3* accel, float dt); +void IRS_update(IRS* irs, float dt); void restoreQuat(IRS* irs, const Vector3* accel); void setAccelShift(IRS* irs, const float pitch, const float roll, const float yaw); diff --git a/Source/INS/geometry/quaternion.c b/Source/INS/geometry/quaternion.c index 2fc2f25..a4eaddd 100644 --- a/Source/INS/geometry/quaternion.c +++ b/Source/INS/geometry/quaternion.c @@ -58,7 +58,7 @@ Quaternion QuatNegate(const Quaternion* q) Quaternion QuatSum(const Quaternion* q1, const Quaternion* q2) { - Quaternion res = {.x = q1->x + q2->x, .y = q1->y + q2->y, .z = q1->z + q2->z, .w = q1->w + q2->w}; + Quaternion res = {q1->x + q2->x, q1->y + q2->y, q1->z + q2->z, q1->w + q2->w}; return res; } diff --git a/Source/main.c b/Source/main.c index 2a0408e..3d044b4 100644 --- a/Source/main.c +++ b/Source/main.c @@ -61,11 +61,16 @@ int main(void) irs_update_flag = 0; imu_read_scaled(&imu); + + irs.gyro.x = imu.gx; + irs.gyro.y = imu.gy; + irs.gyro.z = imu.gz; + + irs.accel.x = imu.ax; + irs.accel.y = imu.ay; + irs.accel.z = imu.az; - Vector3 gyro = {imu.gx, imu.gy, imu.gz}; - Vector3 accel = {imu.ax, imu.ay, imu.az}; - - IRS_update(&irs, &gyro, &accel, IMU_DT); + IRS_update(&irs, IMU_DT); } euler = QuatToEuler(&irs.q);