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

Осуществлён переход на кватернионы. Но нужно протестировать

Browse Source
This commit is contained in:
Раджаб Бисултанов
2026-03-13 16:28:06 +03:00
parent 469ec64d45
commit c3a7907e16
8 changed files with 217 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
Source/INS/geometry/quaternion.c
View File

@@ -3,7 +3,7 @@
Quaternion QuatNormalize(const Quaternion* q, const float gain)
{
Quaternion res;
Quaternion res = {};
float norm = sqrtf(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
@@ -74,11 +74,12 @@ Quaternion QuatConstProd(const Quaternion* q, const float value)
Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2)
{
Quaternion res = {
.x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y,
.y = q1->w * q2->y + q1->x * q2->z + q1->y * q2->w - q1->z * q2->x,
.z = q1->w * q2->z + q1->x * q2->y + q1->y * q2->x - q1->z * q2->w,
.w = q1->w * q2->w + q1->x * q2->x + q1->y * q2->y - q1->z * q2->z
Quaternion res =
{
q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y,
q1->w * q2->y + q1->x * q2->z + q1->y * q2->w - q1->z * q2->x,
q1->w * q2->z + q1->x * q2->y + q1->y * q2->x - q1->z * q2->w,
q1->w * q2->w + q1->x * q2->x + q1->y * q2->y - q1->z * q2->z
};
@@ -99,13 +100,13 @@ Vector3 QuatRotateAroundZ(const Quaternion* q, const Vector3* vec, bool CCW)
return res;
}
Quaternion QuatCreateRollPitchYaw(const Vector3* RollPitchYawRad)
Quaternion QuatCreateFromEuler(const Vector3* eulerAngels)
{
Quaternion res;
float h_r = 0.5f * RollPitchYawRad->y;
float h_p = 0.5f * RollPitchYawRad->x;
float h_y = 0.5f * RollPitchYawRad->z;
float h_r = 0.5f * eulerAngels->y;
float h_p = 0.5f * eulerAngels->x;
float h_y = 0.5f * eulerAngels->z;
float c_r = cosf(h_r), s_r = sinf(h_r);
float c_p = cosf(h_p), s_p = sinf(h_p);