Определение ориентации как в рабочей прошивке

Source/INS/IRS.c

@@ -1,14 +1,6 @@
 #include "irs.h"
 #include <math.h>
 
-#define MAHONY_KP 		2.5f
-#define MAHONY_KI 		0.01f
-
-#define ACC_ALPHA		0.01f
-#define ACC_MAX_ERROR	0.2f
-
-static Vector3 gyro_bias = {0};
-
 void IRS_init(IRS* irs)
 {
     irs->q.w = 1.0f;
@@ -17,103 +9,32 @@ void IRS_init(IRS* irs)
     irs->q.z = 0.0f;
 }
 
-
+void IRS_update(IRS* irs, float dt)
-void IRS_update(IRS* irs, const Vector3* gyro_in, const Vector3* accel_in, float dt)
 {
 	Quaternion qConjugate = QuatConjugate(&irs->q);
 	
 	// gyro update
-  
-	irs->gyro = *gyro_in;
-	
 	Vector3 gyro = 
 	{
-		gyro_in->x * dt * DEG2RAD,
+		irs->gyro.x * DEG2RAD,
-		gyro_in->y * dt * DEG2RAD,
+		irs->gyro.y * DEG2RAD,
-		gyro_in->z * dt * DEG2RAD
+		irs->gyro.z * DEG2RAD
 	};
 	
 	Quaternion g = {gyro.x, gyro.y, gyro.z, 0};
 	g = QuatProd(&irs->q, &g);
-	g = QuatConstProd(&g, 0.5f);
+	g = QuatConstProd(&g, dt * 0.5f);
 	irs->q = QuatSum(&irs->q, &g);
 	irs->q = QuatNormalize(&irs->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};
+	Vector3 accel = {irs->accel.x, irs->accel.y, -irs->accel.z};
-	
-	irs->accel = accel;
-	
 	restoreQuat(irs, &accel);
 	
-	
-	
-	
 	// /accel update
-	
-	
-	
-	/*Vector3 accel = {accel_in->x, accel_in->y, -accel_in->z};
-	
-	float acc_len = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
-	
-	if (acc_len > 1e-6f)
-	{
-		Vector3 acc_norm =
-		{
-			accel.x / acc_len,
-			accel.y / acc_len,
-			accel.z / acc_len
-		};
-		
-		float dyn = fabsf(acc_len - 1.0f);
-		
-		float trust;
-		if (dyn > ACC_MAX_ERROR)
-			trust = 0.0f;
-		else
-			trust = 1.0f - (dyn / ACC_MAX_ERROR);
-		
-		float gain = ACC_ALPHA * trust;
-		
-		if (gain > 1e-5f)
-		{
-			Vector3 est = IRS_getGravity(&q);
-			
-			Vector3 error = 
-			{
-				acc_norm.y*est.z - acc_norm.z*est.y,
-				acc_norm.z*est.x - acc_norm.x*est.z,
-				acc_norm.x*est.y - acc_norm.y*est.x
-			};
-			
-			Quaternion corretrion = 
-			{
-				error.x * gain * 0.5f,
-				error.y * gain * 0.5f,
-				error.z * gain * 0.5f,
-				1.0f
-			};
-			
-			q = QuatProd(&corretrion, &q);
-			q = QuatNormalize(&q, 1.0f);
-		}
-	}
-	
-	irs->q = q;
-	irs->gyro = gyro;
-	irs->accel = accel;*/
 }
 
 void restoreQuat(IRS* irs, const Vector3* accel)

Source/INS/IRS.h

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

Source/INS/geometry/quaternion.c

@@ -58,7 +58,7 @@ Quaternion QuatNegate(const Quaternion* q)
 
 Quaternion QuatSum(const Quaternion* q1, const Quaternion* q2)
 {
-	Quaternion res = {.x = q1->x + q2->x, .y = q1->y + q2->y, .z = q1->z + q2->z, .w = q1->w + q2->w};
+	Quaternion res = {q1->x + q2->x, q1->y + q2->y, q1->z + q2->z, q1->w + q2->w};
 	return res;
 }
 

Source/main.c

@@ -62,10 +62,15 @@ int main(void)
 			
 			imu_read_scaled(&imu);
 			
-		    Vector3 gyro = {imu.gx, imu.gy, imu.gz};
+			irs.gyro.x = imu.gx;
-		    Vector3 accel = {imu.ax, imu.ay, imu.az};
+			irs.gyro.y = imu.gy;
+			irs.gyro.z = imu.gz;
 			
-		    IRS_update(&irs, &gyro, &accel, IMU_DT);
+			irs.accel.x = imu.ax;
+			irs.accel.y = imu.ay;
+			irs.accel.z = imu.az;
+		
+		    IRS_update(&irs, IMU_DT);
 		}
 		
 		euler = QuatToEuler(&irs.q);