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

Обновлена IRS

Browse Source
This commit is contained in:
Раджаб Бисултанов
2026-04-16 14:06:03 +03:00
parent da4dfbfae5
commit 273398ba16
7 changed files with 435 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files

340
Source/INS/IRS.c
View File

@@ -1,111 +1,264 @@
#include "irs.h"
#include "IRS.h"
#include <math.h>
void IRS_init(IRS* irs)
static constexpr float PI = 3.14159265359f;
static constexpr float TO_DEG = 180.0f / PI;
static constexpr float TO_RAD = PI / 180.0f;
static constexpr float G = 9.80665f; // m/c^2
IRS::IRS():
RecordGyro(RecGyr.Rec),
RecordSpeed(RecSpd.Rec),
RecordPosit(RecPos.Rec),
RecordQuat(RecQua.Rec)
{
irs->q.w = 1.0f;
irs->q.x = 0.0f;
irs->q.y = 0.0f;
irs->q.z = 0.0f;
RecordGyro.Init(RecGyr.Buffer, Freq, RecGyr.Past);
RecordSpeed.Init(RecSpd.Buffer, Freq, RecSpd.Past);
RecordPosit.Init(RecPos.Buffer, Freq, RecPos.Past);
RecordQuat.Init(RecQua.Buffer, Freq, RecQua.Past);
}
void IRS_update(IRS* irs, float dt)
// ======================Orientation======================
void IRS::UpdateGyro(const Vector3& Gyr)
{
// gyro update
Vector3 gyro =
{
irs->gyro.x * dt * DEG2RAD,
irs->gyro.y * dt * DEG2RAD,
irs->gyro.z * dt * DEG2RAD
};
Quaternion g = {gyro.x, gyro.y, gyro.z, 0};
g = QuatProd(&irs->q, &g);
g = QuatConstProd(&g, 0.5f);
irs->q = QuatSum(&irs->q, &g);
irs->q = QuatNormalize(&irs->q, 1.0f);
// /gyro update
// accel update
// restoreQuat(irs);
// /accel update
if(!Gyr.IsFinite()) return;
IMU.Gyr = Gyr;
Vector3 gyr = Gyr * (Period * TO_RAD);
RecordGyro.Add(Gyr * TO_RAD);
Quaternion g = gyr;
g = (OriQuat * g);
OriQuat += g * 0.5f;
OriQuat = OriQuat.Norm();
RecordQuat.Add(OriQuat);
OriPRT = OriQuat.Conjugate() * Quaternion(0, 0, 1, 0) * OriQuat;
}
void restoreQuat(IRS* irs)
void IRS::UpdateAccel(const Vector3& Acc)
{
float len = lengthV3(&irs->accel);
static float quat_acc_alpha = 0.03f;
static float quat_acc_max = 0.02f;
float dyn = fabsf(len - 1.0f);
if (dyn > quat_acc_max) dyn = 1.0f; else dyn /= quat_acc_max;
float gain = quat_acc_alpha * (1.0f - dyn);
if (gain < 0.0001f) return;
Vector3 acc = normalizeV3(&irs->accel, 1.0f);
Vector3 est = IRS_getGravity(&irs->q);
Vector3 cross = Cross(&acc, &est);
float dot = DotV3(&acc, &est);
if (dot < 0.0f)
{
float error_len = lengthV3(&cross);
if (error_len < 0.001f) {
cross.x = 1.0f;
cross.y = 0.0f;
cross.z = 0.0f;
}
else
cross = constProdV3(&cross, 1.0f / error_len);
}
Vector3 axis = constProdV3(&cross, gain * 0.5f);
Quaternion correction =
{
axis.x,
axis.y,
axis.z,
1.0f
};
irs->q = QuatProd(&irs->q, &correction);
irs->q = QuatNormalize(&irs->q, 1.0f);
if(!Acc.IsFinite()) return;
Vector3 accel = ShiftAccelPRY * Acc * ShiftAccelPRY.Conjugate();
IMU.Acc = accel;
Quaternion ine = OriQuat * accel * OriQuat.Conjugate();
Vector3 acc = { ine.X, ine.Y, ine.Z - 1.0f }; // без гравитации
Inertial.Acc = acc;//OriQuat.RotateAroundZ(acc, true);
Inertial.Spd += acc * (G * Period); // интегрирование скорости
Vector3 spd = Inertial.Spd * Period;
Inertial.Pos += spd; // интегрирование позиции
RecordSpeed.Add(Inertial.Spd);
RecordPosit.Add(Inertial.Pos);
RestoreQuat(accel);
}
void setAccelShift(IRS* irs, const float roll, const float pitch, const float yaw)
void IRS::RestoreQuat(const Vector3& Acc)
{
float h_roll = (roll * DEG2RAD) / 2.0f;
float h_pitch = (pitch * DEG2RAD) / 2.0f;
float h_yaw = (yaw * DEG2RAD) / 2.0f;
Quaternion q_roll = {0.0f, sinf(h_roll), 0.0f, cosf(h_roll)};
Quaternion q_pitch = {sinf(h_pitch), 0.0f, 0.0f, cosf(h_pitch)};
Quaternion q_yaw = {0.0f, 0.0f, sinf(h_yaw), cosf(h_yaw)};
Quaternion prProd = QuatProd(&q_pitch, &q_roll);
Quaternion pryProd = QuatProd(&prProd, &q_yaw);
irs->shiftAccelPRY = pryProd;
float len = Acc.Length();
static float quat_acc_alpha = 0.03f;
static float quat_acc_max = 0.02f;
float dyn = fabsf(len - 1.0f);
if(dyn>quat_acc_max) dyn=1.0f; else dyn/=quat_acc_max;
float gain = quat_acc_alpha * (1.0f - dyn);
if (gain < 0.0001f) return;
Vector3 acc = Acc.Norm();
Vector3 est;
est.X = 2.0f * (OriQuat.X * OriQuat.Z - OriQuat.W * OriQuat.Y);
est.Y = 2.0f * (OriQuat.W * OriQuat.X + OriQuat.Y * OriQuat.Z);
est.Z = OriQuat.W * OriQuat.W - OriQuat.X * OriQuat.X - OriQuat.Y * OriQuat.Y + OriQuat.Z * OriQuat.Z;
Vector3 cross = acc.Cross(est);
float dot = acc.Dot(est);
if (dot < 0.0f)
{
float error_len = cross.Length();
if (error_len < 0.001f) cross = {1.0f, 0.0f, 0.0f};
else cross = cross * (1.0f / error_len);
}
Vector3 axis = cross * (gain * 0.5f);
Quaternion correction
{
axis.X,
axis.Y,
axis.Z,
1.0f // Для малых углов cos(0) = 1. При нормализации это сработает корректно.
};
OriQuat = OriQuat * correction;
OriQuat = OriQuat.Norm();
}
Vector3 IRS_getGravity(const Quaternion* q)
void IRS::UpdateMagnet(const Vector3& Mag)
{
Vector3 g =
{
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
};
if(!Mag.IsFinite()) return;
IMU.Mag = Mag; // Сохраняем сырые данные (если нужно для отладки)
static int init_count = 100;
float alpha = 0.001f;
return g;
if(init_count > 0)
{
alpha = 0.05f;
init_count--;
}
Vector3 mag_norm = Mag.Norm();
Quaternion mW = OriQuat * mag_norm * OriQuat.Conjugate();
Vector3 mW_horizontal = { mW.X, mW.Y, 0.0f };
if (mW_horizontal.LengthSquared() < 0.0001f) return;
mW_horizontal = mW_horizontal.Norm();
float error_z = mW_horizontal.Y * NorthDeclination.X - mW_horizontal.X * NorthDeclination.Y;
float half_error_step = error_z * alpha * 0.5f;
Quaternion corr
{
0.0f,
0.0f,
half_error_step,
1.0f
};
OriQuat = corr * OriQuat;
OriQuat = OriQuat.Norm();
}
// ======================/Orientation======================
void IRS::UpdatePositionSpeed(const Vector3& ShiftPosition, const Vector3& ShiftSpeed)
{
Shift.Pos += ShiftPosition;
Shift.Spd += ShiftSpeed;
}
void IRS::UpdateQuaternion(const Quaternion& ShiftQuaternion, float Alpha)
{
Shift.Pos = Shift.Qua * (1.0f - Alpha) + ShiftQuaternion * Alpha;
}
void IRS::RestoreAllShift(Vector3& ShiftPos)
{
Vector3 spd = Shift.Spd;
Shift.Spd = 0;
RecordSpeed.Mix(spd);
Inertial.Spd += spd;
Vector3 pos = Shift.Pos;
Shift.Pos = 0;
RecordPosit.Mix(pos);
Inertial.Pos += pos;
ShiftPos=pos;
Quaternion qua = Shift.Qua;
Shift.Qua = 0;
RecordQuat.Mix(qua);
OriQuat += qua;
OriQuat = OriQuat.Norm();
}
void IRS::SetAccelShift(float Pitch, float Roll, float Yaw)
{
float h_roll = (Roll * TO_RAD) / 2.0f;
float h_pitch = (Pitch * TO_RAD) / 2.0f;
float h_yaw = (Yaw * TO_RAD) / 2.0f;
Quaternion q_roll = {0.0f, sinf(h_roll), 0.0f, cosf(h_roll)};
Quaternion q_pitch = {sinf(h_pitch), 0.0f, 0.0f, cosf(h_pitch)};
// Как найти поворот: arcsin(ось/макс_ось) * TO_GRAD
Quaternion q_yaw = {0.0f, 0.0f, sinf(h_yaw), cosf(h_yaw)};
ShiftAccelPRY = q_pitch * q_roll * q_yaw;
}
void IRS::SetNorthDeclination(const float Yaw)
{
float dec = Yaw * TO_RAD; // Укажите ваше склонение
Vector3 mag_north_target;
NorthDeclination = { sinf(dec), cosf(dec) };
}
bool IRS::SetGroundHeight(float* Height, float MaxHeight, float Alpha)
{
if(Height)
{
float shift = Inertial.Pos.Z - *Height;
GroundShift = GroundShift*(1.0f-Alpha) + shift*Alpha;
}
else
{
float height = Inertial.Pos.Z - GroundShift;
if(height<MaxHeight)
{
float shift = Inertial.Pos.Z - MaxHeight;
GroundShift = GroundShift*(1.0f-Alpha) + shift*Alpha;
}
}
float height = Inertial.Pos.Z - GroundShift;
if (height < 0.0f) GroundShift+=height;
}
void IRS::GetSinXYCosZ(Vector3& SinXYCosZ)
{
SinXYCosZ = OriPRT;
}
void IRS::GetPitchRollYaw(Vector3& PitchRollYaw, bool& Reversed)
{
Reversed = OriPRT.Z < 0;
float yaw = atan2f(2.0f * (OriQuat.X * OriQuat.Y - OriQuat.W * OriQuat.Z), 1.0f - 2.0f * (OriQuat.Y * OriQuat.Y + OriQuat.Z * OriQuat.Z)) * TO_DEG;
if (yaw < 0.0f) yaw += 360.0f;
PitchRollYaw =
{
GetAngle(OriPRT.Y, OriPRT.X, OriPRT.Z), // Pitch
GetAngle(-OriPRT.X, OriPRT.Y, OriPRT.Z), // Roll
yaw // Yaw
};
}
void IRS::GetInertial(Vector3* Pos, Vector3* Spd, Vector3* Acc, float* Gnd)
{
if (Acc) *Acc = Inertial.Acc;
if (Spd) *Spd = Inertial.Spd;
if (Pos) *Pos = Inertial.Pos;
if (Gnd) *Gnd = GroundShift;
}
@@ -114,6 +267,3 @@ Vector3 IRS_getGravity(const Quaternion* q)