Реализовано асинхронное чтение IMU

Source/INS/IRS.c

@@ -11,26 +11,24 @@ void IRS_init(IRS* irs)
 
 void IRS_update(IRS* irs, float dt)
 {
-	Quaternion qConjugate = QuatConjugate(&irs->q);
-	
 	// gyro update
 	Vector3 gyro = 
 	{
-		irs->gyro.x * DEG2RAD,
-		irs->gyro.y * DEG2RAD,
-		irs->gyro.z * DEG2RAD
+		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, dt * 0.5f);
+	g = QuatConstProd(&g, 0.5f);
 	irs->q = QuatSum(&irs->q, &g);
 	irs->q = QuatNormalize(&irs->q, 1.0f);
 	
   // /gyro update
 	
 	// accel update
-	restoreQuat(irs);
+	//restoreQuat(irs);
 	
 	// /accel update
 }
@@ -100,14 +98,14 @@ void setAccelShift(IRS* irs, const float roll, const float pitch, const float ya
 
 Vector3 IRS_getGravity(const Quaternion* q)
 {
-    Vector3 g = 
+  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
 	};
 
-    return g;
+  return g;
 }
 
 

Source/INS/IRS.h

@@ -9,10 +9,10 @@
 
 typedef struct
 {
-    Quaternion q;   			// ориентация
+  Quaternion q;   			// ориентация
 	Vector3 oriPRT; 			// orientation pitch roll tilts
-    Vector3 gyro;   			// deg/s
-    Vector3 accel;  			// g
+  Vector3 gyro;   			// deg/s
+  Vector3 accel;  			// g
 	
 	Quaternion shiftAccelPRY; 	// смещение акселерометра
 

Source/INS/geometry/vector.c

@@ -10,7 +10,7 @@ Vector2 normalizeV2(const Vector2* v, float gain)
 
 Vector3 normalizeV3(const Vector3* v, float gain)
 {
-	Vector3 res = {0};
+	Vector3 res = {0.0f, 0.0f, 0.0f};
 	float n = lengthV3(v);
 	
 	if (n > 1e-12f)
@@ -133,7 +133,7 @@ Vector2 constProdV2(const Vector2* v, float value)
 
 Vector3 constProdV3(const Vector3* v, float value)
 {
-	Vector3 res = {.x = v->x * value, .y = v->y * value, .z = v->z * value};
+	Vector3 res = {v->x * value, v->y * value, v->z * value};
 	return res;
 }