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-31 16:24:32 +03:00
parent b6945b83f4
commit 3066b28f75
4 changed files with 17 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
Source/INS/IRS.c
View File

@@ -1,14 +1,6 @@
#include "irs.h"
#include <math.h>
#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)

2
Source/INS/IRS.h
View File

@@ -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);

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

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

11
Source/main.c
View File

@@ -62,10 +62,15 @@ int main(void)
imu_read_scaled(&imu);
Vector3 gyro = {imu.gx, imu.gy, imu.gz};
Vector3 accel = {imu.ax, imu.ay, imu.az};
irs.gyro.x = imu.gx;
irs.gyro.y = imu.gy;
irs.gyro.z = imu.gz;
IRS_update(&irs, &gyro, &accel, IMU_DT);
irs.accel.x = imu.ax;
irs.accel.y = imu.ay;
irs.accel.z = imu.az;
IRS_update(&irs, IMU_DT);
}
euler = QuatToEuler(&irs.q);