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

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 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
};