Реализовано асинхронное чтение 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;
 }