Исправлена ошибка в реализации функции Cross, из-за которой коррекция кватерниона проходила неправильно

Source/BSP/Src/imu_processing.c

@@ -37,7 +37,7 @@ void imu_read_scaled(imu_scaled_t* out)
 	
 	out->ax = raw.ax / ACCEL_SENS_SCALE_FACTOR - accel_bias_x;
 	out->ay = raw.ay / ACCEL_SENS_SCALE_FACTOR - accel_bias_y;
-	out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_bias_z + 1;
+	out->az = raw.az / ACCEL_SENS_SCALE_FACTOR - accel_bias_z;
 	
 	out->ax = biquad_apply(&accel_x_lpf, out->ax);
 	out->ay = biquad_apply(&accel_y_lpf, out->ay);

Source/INS/IRS.c

@@ -53,10 +53,7 @@ void restoreQuat(IRS* irs, const Vector3* accel)
 	
 	Vector3 acc = normalizeV3(accel, 1.0f);
 	
-	Vector3 est;
-	est.x = 2.0f * (irs->q.x * irs->q.z - irs->q.w * irs->q.y);
-	est.y = 2.0f * (irs->q.w * irs->q.x - irs->q.y * irs->q.z);
-	est.z = irs->q.w * irs->q.w - irs->q.x * irs->q.x - irs->q.y * irs->q.y + irs->q.z * irs->q.z;
+	Vector3 est = IRS_getGravity(&irs->q);
 	
 	Vector3 cross = Cross(&acc, &est);
 	float dot = DotV3(&acc, &est);
@@ -70,9 +67,7 @@ void restoreQuat(IRS* irs, const Vector3* accel)
 			cross.z = 0.0f;
 		}
 		else
-		{
 			cross = constProdV3(&cross, 1.0f / error_len);
-		}
 	}
 	
 	Vector3 axis = constProdV3(&cross, gain * 0.5f);
@@ -109,8 +104,8 @@ Vector3 IRS_getGravity(const Quaternion* q)
 {
     Vector3 g = 
 	{
-		2 * (q->x*q->z - q->w*q->y),
-		2 * (q->w*q->x + q->y*q->z),
+		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
 	};
 

Source/INS/geometry/vector.c

@@ -154,7 +154,7 @@ Vector3 Cross(const Vector3* v1, const Vector3* v2)
 {
 	Vector3 res = 
 	{
-		v1->x * v2->z - v1->z * v2->y,
+		v1->y * v2->z - v1->z * v2->y,
 		v1->z * v2->x - v1->x * v2->z,
 		v1->x * v2->y - v1->y * v2->x
 	};