Выполняется корректное чтение данных иму. Оси и наклоны соответствуют рабочим прошивке и дрону

Source/INS/IRS.c

@@ -27,19 +27,17 @@ void IRS_update(IRS* irs, float dt)
 	irs->q = QuatSum(&irs->q, &g);
 	irs->q = QuatNormalize(&irs->q, 1.0f);
 	
-    // /gyro update
+  // /gyro update
 	
 	// accel update
-	
-	Vector3 accel = {irs->accel.x, irs->accel.y, irs->accel.z};
-	restoreQuat(irs, &accel);
+	restoreQuat(irs);
 	
 	// /accel update
 }
 
-void restoreQuat(IRS* irs, const Vector3* accel)
+void restoreQuat(IRS* irs)
 {
-	float len = lengthV3(accel);
+	float len = lengthV3(&irs->accel);
 	
 	static float quat_acc_alpha = 0.03f;
 	static float quat_acc_max = 0.02f;
@@ -51,7 +49,7 @@ void restoreQuat(IRS* irs, const Vector3* accel)
 	
 	if (gain < 0.0001f) return;
 	
-	Vector3 acc = normalizeV3(accel, 1.0f);
+	Vector3 acc = normalizeV3(&irs->accel, 1.0f);
 	
 	Vector3 est = IRS_getGravity(&irs->q);
 	

Source/INS/IRS.h

@@ -21,7 +21,7 @@ typedef struct
 void IRS_init(IRS* irs);
 
 void IRS_update(IRS* irs, float dt);
-void restoreQuat(IRS* irs, const Vector3* accel);
+void restoreQuat(IRS* irs);
 
 void setAccelShift(IRS* irs, const float pitch, const float roll, const float yaw);
 

Source/INS/geometry/quaternion.c

@@ -5,7 +5,7 @@
 
 Quaternion QuatNormalize(const Quaternion* q, const float gain)
 {
-  Quaternion res = {};
+  Quaternion res = {0.0f, 0.0f, 0.0f, 0.0f};
 	
 	float norm = sqrtf(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
 	
@@ -70,7 +70,7 @@ Quaternion QuatDiff(const Quaternion* q1, const Quaternion* q2)
 
 Quaternion QuatConstProd(const Quaternion* q, const float value)
 {
-	Quaternion res = {.x = q->x * value, .y = q->y * value, .z = q->z * value, .w = q->w * value};
+	Quaternion res = {q->x * value, q->y * value, q->z * value, q->w * value};
 	return res;
 }
 
@@ -83,8 +83,6 @@ Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2)
 		q1->w * q2->z + q1->x * q2->y + q1->y * q2->x - q1->z * q2->w,
 		q1->w * q2->w + q1->x * q2->x + q1->y * q2->y - q1->z * q2->z
 	};
-	
-	
 	return res;
 }