Обновлена IRS

Source/INS/IRS.h

@@ -1,30 +1,93 @@
+#pragma once
+
 #ifndef IRS_H
 #define IRS_H
 
 #include "Quaternion.h"
 #include "Vector.h"
+#include "util/Record.h"
 
-#define PI			3.14159265359f
-#define DEG2RAD		PI / 180.0f
-
-typedef struct
+class IRS // Inertial Reference System
 {
-  Quaternion q;   			// ориентация
-	Vector3 oriPRT; 			// orientation pitch roll tilts
-  Vector3 gyro;   			// deg/s
-  Vector3 accel;  			// g
-	
-	Quaternion shiftAccelPRY; 	// смещение акселерометра
+public:
+  IRS();
+  
+  static constexpr float Freq = 100.0f;
+  static constexpr float Period = 1.0f / Freq;
+  
+  Quaternion OriQuat; // Главный кватернион ориентации
+  Vector3 OriPRT;     // Локальные наклоны
+  
+  struct { Vector3 Gyr, Acc, Mag; } IMU;      // Последние значения датчиков
+  struct { Vector3 Acc, Spd, Pos; } Inertial; // Инерциальные значения движения
+  
+  float GroundShift = 0.0f; // Разница между высотой и поверхностью
+  
+  Quaternion ShiftAccelPRY; // Смещение акселерометра
+  Vector2 NorthDeclination; // Смещение магнитометра
+  
+private:
+  // Запись значений для расчётов в прошлом
+  struct StructRecGyr
+  {
+    static constexpr float Past = 0.1f; // Максимальная запись прошлых данных
+    static constexpr unsigned long Count = Freq * Past + 1;
+    Vector3 Buffer[Count];
+    Record<Vector3> Rec;
+  } RecGyr;
 
-} IRS;
+  struct StructRecAccSpdPos
+  {
+    static constexpr float Past = 0.6f; // Максимальная запись прошлых данных
+    static constexpr unsigned long Count = Freq * Past + 1;
+    Vector3 Buffer[Count];
+    Record<Vector3> 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<Quaternion> Rec;
+  } RecQua;
+  
+  //---
+  // Сдвиг данных
+  struct { Vector3 Pos, Spd; Quaternion Qua = 0.0f; } Shift; // Значение смещения позиции, скорости и кватерниона
+  //---
+  
+public:
+  Record<Vector3>& RecordGyro;
+  Record<Vector3>& RecordSpeed;
+  Record<Vector3>& RecordPosit;
+  Record<Quaternion>& 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