bisultanov/RaDrone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Исправлена ошибка в реализации функции Cross, из-за которой коррекция кватерниона проходила неправильно

Browse Source
This commit is contained in:
Раджаб Бисултанов
2026-04-01 13:01:39 +03:00
parent 99552ceedb
commit e11c94c357
3 changed files with 5 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Source/BSP/Src/imu_processing.c
View File

@@ -37,7 +37,7 @@ void imu_read_scaled(imu_scaled_t* out)
out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x; out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x;
out->ay = raw.ay / ACCEL_SENS_SCALE_FACTOR - accel_bias_y; 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->ax = biquad_apply(&accel_x_lpf, out->ax);
out->ay = biquad_apply(&accel_y_lpf, out->ay); out->ay = biquad_apply(&accel_y_lpf, out->ay);

11
Source/INS/IRS.c
View File

@@ -53,10 +53,7 @@ void restoreQuat(IRS* irs, const Vector3* accel)
Vector3 acc = normalizeV3(accel, 1.0f); Vector3 acc = normalizeV3(accel, 1.0f);
Vector3 est; Vector3 est = IRS_getGravity(&irs->q);
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); Vector3 cross = Cross(&acc, &est);
float dot = DotV3(&acc, &est); float dot = DotV3(&acc, &est);
@@ -70,10 +67,8 @@ void restoreQuat(IRS* irs, const Vector3* accel)
cross.z = 0.0f; cross.z = 0.0f;
} }
else else
{
cross = constProdV3(&cross, 1.0f / error_len); cross = constProdV3(&cross, 1.0f / error_len);
} }
}
Vector3 axis = constProdV3(&cross, gain * 0.5f); Vector3 axis = constProdV3(&cross, gain * 0.5f);
@@ -109,8 +104,8 @@ Vector3 IRS_getGravity(const Quaternion* q)
{ {
Vector3 g = Vector3 g =
{ {
2 * (q->x*q->z - q->w*q->y), 2.0f * (q->x*q->z - q->w*q->y),
2 * (q->w*q->x + q->y*q->z), 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 q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z
}; };

2
Source/INS/geometry/vector.c
View File

@@ -154,7 +154,7 @@ Vector3 Cross(const Vector3* v1, const Vector3* v2)
{ {
Vector3 res = 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->z * v2->x - v1->x * v2->z,
v1->x * v2->y - v1->y * v2->x v1->x * v2->y - v1->y * v2->x
}; };