Логика обработки данных иму реализована как в рабочей прошивке. Дополнены функции вектора

Source/INS/IRS.c

@@ -17,41 +17,55 @@ void IRS_init(IRS* irs)
     irs->q.z = 0.0f;
 }
 
-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),
-		q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z
-	};
-
-    return g;
-}
 
 void IRS_update(IRS* irs, const Vector3* gyro_in, const Vector3* accel_in, float dt)
 {
-    Vector3 gyro = *gyro_in;
-	Vector3 accel = {accel_in->x, accel_in->y, -accel_in->z};
+	Quaternion qConjugate = QuatConjugate(&irs->q);
 	
-    // gyro intergate
-	Quaternion q = irs->q;
+	// gyro update
   
-	Quaternion omega = 
+	irs->gyro = *gyro_in;
+	
+	Vector3 gyro = 
 	{
-		gyro.x * DEG2RAD,
-		gyro.y * DEG2RAD,
-		gyro.z * DEG2RAD,
-		0
+		gyro_in->x * dt * DEG2RAD,
+		gyro_in->y * dt * DEG2RAD,
+		gyro_in->z * dt * DEG2RAD
 	};
 	
-	Quaternion dq = QuatProd(&q, &omega);
-	dq = QuatConstProd(&dq, 0.5f * dt);
+	Quaternion g = {gyro.x, gyro.y, gyro.z, 0};
+	g = QuatProd(&irs->q, &g);
+	g = QuatConstProd(&g, 0.5f);
+	irs->q = QuatSum(&irs->q, &g);
+	irs->q = QuatNormalize(&irs->q, 1.0f);
 	
-	q = QuatSum(&q, &dq);
-	q = QuatNormalize(&q, 1.0f);
+	Quaternion q0010 = {0.0f, 0.0f, 1.0f, 0.0f};
+	Quaternion conj0010prod = QuatProd(&qConjugate, &q0010);
+	Quaternion prtilts = QuatProd(&conj0010prod, &irs->q);
+	
+	irs->oriPRT.x = prtilts.x;
+	irs->oriPRT.y = prtilts.y;
+	irs->oriPRT.z = prtilts.z;
+	
+    // /gyro update
+	
+	// accel update
+	
+	Vector3 accel = {accel_in->x, accel_in->y, -accel_in->z};
+	
+	irs->accel = accel;
+	
+	restoreQuat(irs, &accel);
+	
+	
+	
+	
+	// /accel update
+	
+	
+	
+	/*Vector3 accel = {accel_in->x, accel_in->y, -accel_in->z};
 	
-	// accel correction
 	float acc_len = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
 	
 	if (acc_len > 1e-6f)
@@ -99,5 +113,95 @@ void IRS_update(IRS* irs, const Vector3* gyro_in, const Vector3* accel_in, float
 	
 	irs->q = q;
 	irs->gyro = gyro;
-	irs->accel = accel;
+	irs->accel = accel;*/
 }
+
+void restoreQuat(IRS* irs, const Vector3* accel)
+{
+	float len = lengthV3(accel);
+	
+	static float quat_acc_alpha = 0.03f;
+	static float quat_acc_max = 0.02f;
+	
+	float dyn = fabsf(len - 1.0f);
+	if (dyn > quat_acc_max) dyn = 1.0f; else dyn /= quat_acc_max;
+	
+	float gain = quat_acc_alpha * (1.0f - dyn);
+	
+	if (gain < 0.0001f) return;
+	
+	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 cross = Cross(&acc, &est);
+	float dot = DotV3(&acc, &est);
+	
+	if (dot < 0.0f)
+	{
+		float error_len = lengthV3(&cross);
+		if (error_len < 0.001f) {
+			cross.x = 1.0f;
+			cross.y = 0.0f;
+			cross.z = 0.0f;
+		}
+		else
+		{
+			cross = constProdV3(&cross, 1.0f / error_len);
+		}
+	}
+	
+	Vector3 axis = constProdV3(&cross, gain * 0.5f);
+	
+	Quaternion correction =
+	{
+		axis.x,
+		axis.y,
+		axis.z,
+		1.0f
+	};
+	
+	irs->q = QuatProd(&irs->q, &correction);
+	irs->q = QuatNormalize(&irs->q, 1.0f);
+}
+
+void setAccelShift(IRS* irs, const float roll, const float pitch, const float yaw)
+{
+	float h_roll = (roll * DEG2RAD) / 2.0f;
+	float h_pitch = (pitch * DEG2RAD) / 2.0f;
+	float h_yaw = (yaw * DEG2RAD) / 2.0f;
+	
+	Quaternion q_roll = {0.0f, sinf(h_roll), 0.0f, cosf(h_roll)};
+	Quaternion q_pitch = {sinf(h_pitch), 0.0f, 0.0f, cosf(h_pitch)};
+	Quaternion q_yaw = {0.0f, 0.0f, sinf(h_yaw), cosf(h_yaw)};
+	
+	Quaternion prProd = QuatProd(&q_pitch, &q_roll);
+	Quaternion pryProd = QuatProd(&prProd, &q_yaw);
+	
+	irs->shiftAccelPRY = pryProd;
+}
+
+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),
+		q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z
+	};
+
+    return g;
+}
+
+
+
+
+
+
+
+
+
+

Source/INS/IRS.h

@@ -10,14 +10,20 @@
 typedef struct
 {
     Quaternion q;   			// ориентация
+	Vector3 oriPRT; 			// orientation pitch roll tilts
     Vector3 gyro;   			// deg/s
     Vector3 accel;  			// g
 	
+	Quaternion shiftAccelPRY; 	// смещение акселерометра
+
 } IRS;
 
 void IRS_init(IRS* irs);
 
 void IRS_update(IRS* irs, const Vector3* gyro, const Vector3* accel, float dt);
+void restoreQuat(IRS* irs, const Vector3* accel);
+
+void setAccelShift(IRS* irs, const float pitch, const float roll, const float yaw);
 
 Vector3 IRS_getGravity(const Quaternion* q);
 

Source/INS/geometry/vector.c

@@ -10,8 +10,29 @@ Vector2 normalizeV2(const Vector2* v, float gain)
 
 Vector3 normalizeV3(const Vector3* v, float gain)
 {
-	float len = lengthV3(v);
-	Vector3 res = {.x = v->x / len, .y = v->y / len, .z = v->z};
+	Vector3 res = {0};
+	float n = lengthV3(v);
+	
+	if (n > 1e-12f)
+	{
+		n = gain / n;
+		res.x = v->x * n;
+		res.y = v->y * n;
+		res.z = v->z * n;
+	}
+	
+	return res;
+}
+
+float DotV2(const Vector2* v1, const Vector2* v2)
+{
+	float res = v1->x * v2->x + v1->y * v2->y;
+	return res;
+}
+
+float DotV3(const Vector3* v1, const Vector3* v2)
+{
+	float res = v1->x * v2->x + v1->y * v2->y + v1->z * v2->z;
 	return res;
 }
 
@@ -129,6 +150,21 @@ float scalarProdV3(const Vector3* v1, const Vector3* v2)
 }
 
 
+Vector3 Cross(const Vector3* v1, const Vector3* v2)
+{
+	Vector3 res = 
+	{
+		v1->x * v2->z - v1->z * v2->y,
+		v1->z * v2->x - v1->x * v2->z,
+		v1->x * v2->y - v1->y * v2->x
+	};
+	
+	return res;
+}
+
+
+
+
 
 
 

Source/INS/geometry/vector.h

@@ -17,6 +17,9 @@ typedef struct
 Vector2 normalizeV2(const Vector2* v, float gain);
 Vector3 normalizeV3(const Vector3* v, float gain);
 
+float DotV2(const Vector2* v1, const Vector2* v2);
+float DotV3(const Vector3* v1, const Vector3* v2);
+
 Vector2 absV2(const Vector2* v);
 Vector3 absV3(const Vector3* v);
 
@@ -45,6 +48,6 @@ float scalarProdV2(const Vector2* v1, const Vector2* v2);
 float scalarProdV3(const Vector3* v1, const Vector3* v2);
 
 Vector2 vectorProdV2(const Vector2* v1, const Vector2* v2);
-Vector3 vectorProdV3(const Vector3* v1, const Vector3* v2);
+Vector3 Cross(const Vector3* v1, const Vector3* v2);
 
 #endif