From e11c94c357539692f56ef6b64720755d5906727d Mon Sep 17 00:00:00 2001 From: Radzhab Bisultanov Date: Wed, 1 Apr 2026 13:01:39 +0300 Subject: [PATCH] =?UTF-8?q?=D0=98=D1=81=D0=BF=D1=80=D0=B0=D0=B2=D0=BB?= =?UTF-8?q?=D0=B5=D0=BD=D0=B0=20=D0=BE=D1=88=D0=B8=D0=B1=D0=BA=D0=B0=20?= =?UTF-8?q?=D0=B2=20=D1=80=D0=B5=D0=B0=D0=BB=D0=B8=D0=B7=D0=B0=D1=86=D0=B8?= =?UTF-8?q?=D0=B8=20=D1=84=D1=83=D0=BD=D0=BA=D1=86=D0=B8=D0=B8=20Cross,=20?= =?UTF-8?q?=D0=B8=D0=B7-=D0=B7=D0=B0=20=D0=BA=D0=BE=D1=82=D0=BE=D1=80?= =?UTF-8?q?=D0=BE=D0=B9=20=D0=BA=D0=BE=D1=80=D1=80=D0=B5=D0=BA=D1=86=D0=B8?= =?UTF-8?q?=D1=8F=20=D0=BA=D0=B2=D0=B0=D1=82=D0=B5=D1=80=D0=BD=D0=B8=D0=BE?= =?UTF-8?q?=D0=BD=D0=B0=20=D0=BF=D1=80=D0=BE=D1=85=D0=BE=D0=B4=D0=B8=D0=BB?= =?UTF-8?q?=D0=B0=20=D0=BD=D0=B5=D0=BF=D1=80=D0=B0=D0=B2=D0=B8=D0=BB=D1=8C?= =?UTF-8?q?=D0=BD=D0=BE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Source/BSP/Src/imu_processing.c | 2 +- Source/INS/IRS.c | 11 +++-------- Source/INS/geometry/vector.c | 2 +- 3 files changed, 5 insertions(+), 10 deletions(-) diff --git a/Source/BSP/Src/imu_processing.c b/Source/BSP/Src/imu_processing.c index 4d9e37f..876673c 100644 --- a/Source/BSP/Src/imu_processing.c +++ b/Source/BSP/Src/imu_processing.c @@ -37,7 +37,7 @@ void imu_read_scaled(imu_scaled_t* out) out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x; out->ay = raw.ay / ACCEL_SENS_SCALE_FACTOR - accel_bias_y; - out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_bias_z + 1; + out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_bias_z; out->ax = biquad_apply(&accel_x_lpf, out->ax); out->ay = biquad_apply(&accel_y_lpf, out->ay); diff --git a/Source/INS/IRS.c b/Source/INS/IRS.c index c6a5fd7..a18f290 100644 --- a/Source/INS/IRS.c +++ b/Source/INS/IRS.c @@ -53,10 +53,7 @@ void restoreQuat(IRS* irs, const Vector3* accel) 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 est = IRS_getGravity(&irs->q); Vector3 cross = Cross(&acc, &est); float dot = DotV3(&acc, &est); @@ -70,9 +67,7 @@ void restoreQuat(IRS* irs, const Vector3* accel) cross.z = 0.0f; } else - { cross = constProdV3(&cross, 1.0f / error_len); - } } Vector3 axis = constProdV3(&cross, gain * 0.5f); @@ -109,8 +104,8 @@ 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), + 2.0f * (q->x*q->z - q->w*q->y), + 2.0f * (q->w*q->x + q->y*q->z), q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z }; diff --git a/Source/INS/geometry/vector.c b/Source/INS/geometry/vector.c index 3ce6d17..eca9cae 100644 --- a/Source/INS/geometry/vector.c +++ b/Source/INS/geometry/vector.c @@ -154,7 +154,7 @@ Vector3 Cross(const Vector3* v1, const Vector3* v2) { Vector3 res = { - v1->x * v2->z - v1->z * v2->y, + v1->y * v2->z - v1->z * v2->y, v1->z * v2->x - v1->x * v2->z, v1->x * v2->y - v1->y * v2->x };