From 273398ba16d4df05e90a955d3eb6a8192d512acb Mon Sep 17 00:00:00 2001 From: Radzhab Bisultanov Date: Thu, 16 Apr 2026 14:06:03 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9E=D0=B1=D0=BD=D0=BE=D0=B2=D0=BB=D0=B5?= =?UTF-8?q?=D0=BD=D0=B0=20IRS?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Source/Devices/IIMU.cpp | 11 ++ Source/Devices/IIMU.h | 2 + Source/INS/IRS.c | 340 ++++++++++++++++++++++++++----------- Source/INS/IRS.h | 95 +++++++++-- Source/INS/util/Record.cpp | 47 +++++ Source/INS/util/Record.h | 45 +++++ Source/main.cpp | 6 + 7 files changed, 435 insertions(+), 111 deletions(-) create mode 100644 Source/INS/util/Record.cpp create mode 100644 Source/INS/util/Record.h diff --git a/Source/Devices/IIMU.cpp b/Source/Devices/IIMU.cpp index 52b3ddd..b088ab6 100644 --- a/Source/Devices/IIMU.cpp +++ b/Source/Devices/IIMU.cpp @@ -1,11 +1,22 @@ #include "IIMU.h" +#include "stm32g4xx.h" #include "I2C.h" #include "ICM20948.h" +#include "GPIO.h" + IMU_Info IMUInfo; IMU_Calib_Data CalibDataIMU; +void IMU_InitPower() +{ + GPIO_InitPin(GPIO_PIN_13 | GPIO_PORT_C | GPIO_OUTPUT | GPIO_SET); // POWER ON 3V3 (nPC13) + for (volatile int i = 0; i < 1000000; i ++) {}; + GPIOC->BSRR = GPIO_BSRR_BR13; + for (volatile int i = 0; i < 1000000; i ++) {}; +} + void IMU_SetReg(unsigned char Addr, unsigned char Reg, unsigned char Value) { unsigned char reg[2]; diff --git a/Source/Devices/IIMU.h b/Source/Devices/IIMU.h index bc714da..6ee602c 100644 --- a/Source/Devices/IIMU.h +++ b/Source/Devices/IIMU.h @@ -47,6 +47,8 @@ typedef void (*IMU_Proc)(IMU_Data&); extern IMU_Calib_Data CalibDataIMU; +void IMU_InitPower(); + void IMU_SetReg(unsigned char Addr, unsigned char Reg, unsigned char Value); unsigned char IMU_GetReg(unsigned char Addr, unsigned char Reg); diff --git a/Source/INS/IRS.c b/Source/INS/IRS.c index 78538bb..aeeee0d 100644 --- a/Source/INS/IRS.c +++ b/Source/INS/IRS.c @@ -1,111 +1,264 @@ -#include "irs.h" +#include "IRS.h" #include -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 Rec; + } RecGyr; -} IRS; + struct StructRecAccSpdPos + { + static constexpr float Past = 0.6f; // Максимальная запись прошлых данных + static constexpr unsigned long Count = Freq * Past + 1; + Vector3 Buffer[Count]; + Record Rec; + } RecAcc, RecSpd, RecPos; -void IRS_init(IRS* irs); + struct StructRecQua + { + static constexpr float Past = 0.6f; // Максимальная запись прошлых данных + static constexpr unsigned long Count = Freq * Past + 1; + Quaternion Buffer[Count]; + Record Rec; + } RecQua; + + //--- + // Сдвиг данных + struct { Vector3 Pos, Spd; Quaternion Qua = 0.0f; } Shift; // Значение смещения позиции, скорости и кватерниона + //--- + +public: + Record& RecordGyro; + Record& RecordSpeed; + Record& RecordPosit; + Record& RecordQuat; -void IRS_update(IRS* irs, float dt); -void restoreQuat(IRS* irs); +private: + void RestoreQuat(const Vector3& Acc); + static float GetAngle(float X, float Y, float Z); + +public: + void UpdateGyro(const Vector3& Gyr); // deg/s + void UpdateAccel(const Vector3& Acc); // g + void UpdateMagnet(const Vector3& Mag); // gaus + + // Коэффициенты восстановления позиции и скорости в секунду + void SetShiftAlpha(const Vector3* Pos, const Vector3* Spd, const float* Qua); -void setAccelShift(IRS* irs, const float pitch, const float roll, const float yaw); + void UpdatePositionSpeed(const Vector3& ShiftPosition, const Vector3& ShiftSpeed); + void UpdateQuaternion(const Quaternion& ShiftQuaternion, float Alpha); -Vector3 IRS_getGravity(const Quaternion* q); + void RestoreAllShift(Vector3& ShiftPos); + + void SetAccelShift(float Pitch, float Roll, float Yaw); + void SetNorthDeclination(const float Yaw); + + bool SetGroundHeight(float* Height, float MaxHeight, float Alpha); + + void GetSinXYCosZ(Vector3& SinXYCos); + void GetPitchRollYaw(Vector3& PitchRollYaw, bool& Reversed); + void GetInertial(Vector3* Pos, Vector3* Spd, Vector3* Acc, float* Gnd); +}; #endif \ No newline at end of file diff --git a/Source/INS/util/Record.cpp b/Source/INS/util/Record.cpp new file mode 100644 index 0000000..9ec6628 --- /dev/null +++ b/Source/INS/util/Record.cpp @@ -0,0 +1,47 @@ +#include "Record.h" +#include + +template class Record; +template class Record; +template class Record; +template class Record; + +template +Record::Record(T* RecordBuffer, float Frequency, float Period) +{ + Init(RecordBuffer, Frequency, Period); +} + +template +void Record::Init(T* RecordBuffer, float Frequency, float Period) +{ + Freq = Frequency; + Buffer = RecordBuffer; + Count = (float)(Frequency * Period + 1.0f); +} + +template +T Record::Past(float Time) const +{ + unsigned long move = (float)(Time * Freq); + if (move >= Count) move = Count - 1; + + if (Index >= move) move = Index - move; + else move = Index + (Count - move); + + return Buffer[move]; +} + +template +void Record::Add(const T& Value) +{ + Index++; + if (Index >= Count) Index = 0; + Buffer[Index] = Value; +} + +template +void Record::Mix(const T& Shift) +{ + for (unsigned int a = 0; a < Count; a++) Buffer[a] += Shift; +} diff --git a/Source/INS/util/Record.h b/Source/INS/util/Record.h new file mode 100644 index 0000000..626b914 --- /dev/null +++ b/Source/INS/util/Record.h @@ -0,0 +1,45 @@ +#pragma once + +#ifndef RECORD_H +#define RECORD_H + +#include "Vector.h" +#include "Quaternion.h" + +template +class Record +{ +public: + Record() {}; + Record(T* RecordBuffer, float Frequency, float Period); // BufferSize = (Frequency * Period) + 1; + +private: + float Freq; + unsigned long Count; + T* Buffer; + T Shift; + unsigned long Index = 0; + +public: + void Init(T* RecordBuffer, float Frequency, float Period); // BufferSize = (Frequency * Period) + 1; + + T Past(float Time) const; + + void Add(const T& Value); + + void Mix(const T& Shift); +}; + +template +class Average +{ +private: + T Value; + float Count = 0; + +public: + void Set(const T& Val) { Value += Val; Count++; } + T Get() { if (Count == 0.0f) return T(); T ave = Value / Count; Count = 0; Value = 0.0f; return ave; } +}; + +#endif \ No newline at end of file diff --git a/Source/main.cpp b/Source/main.cpp index 4a41551..9aa7411 100644 --- a/Source/main.cpp +++ b/Source/main.cpp @@ -1,6 +1,12 @@ #include "stm32g4xx.h" +#include "ICM20948.h" + +extern "C" void SystemClock_Config(); int main() { + SystemClock_Config(); // 170MHz + IMU_InitPower(); + return 0; } \ No newline at end of file