270 lines
6.2 KiB
C
270 lines
6.2 KiB
C
#include "IRS.h"
|
||
#include <math.h>
|
||
|
||
|
||
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)
|
||
{
|
||
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);
|
||
}
|
||
|
||
|
||
// ======================Orientation======================
|
||
void IRS::UpdateGyro(const Vector3& Gyr)
|
||
{
|
||
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 IRS::UpdateAccel(const Vector3& Acc)
|
||
{
|
||
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 IRS::RestoreQuat(const Vector3& Acc)
|
||
{
|
||
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();
|
||
}
|
||
|
||
void IRS::UpdateMagnet(const Vector3& Mag)
|
||
{
|
||
if(!Mag.IsFinite()) return;
|
||
|
||
IMU.Mag = Mag; // Сохраняем сырые данные (если нужно для отладки)
|
||
|
||
static int init_count = 100;
|
||
|
||
float alpha = 0.001f;
|
||
|
||
|
||
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;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|