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

Source/Control/Inc/attitude.h

@@ -5,15 +5,14 @@
 #include "vector.h"
 #include "pid.h"
 #include "radio_receiver.h"
+#include "imu_processing.h"
+#include "IRS.h"
 
 typedef struct
 {
     Vector3 gyro;
 } attitude_t;
 
-static uint8_t imu_update_flag = 0;
-static uint8_t pid_update_flag = 0;
-
 void attitude_init(attitude_t* att);
 
 void attitude_controller_update(control_channels_t* control,
@@ -23,6 +22,5 @@ void attitude_controller_update(control_channels_t* control,
 
 Quaternion rx_to_quaternion(const rc_channels* rx);
 float constrain(float x, float min, float max);
-void TIM6_DAC_IRQHandler();
 
 #endif

Source/Control/Src/attitude.c

@@ -66,14 +66,4 @@ float constrain(float x, float min, float max)
 {
 	if (x < min) x = min; else if (x > max) x = max;
 	return x;
-}
-
-void TIM6_DAC_IRQHandler()
-{
-	if (TIM6->SR & TIM_SR_UIF)
-	{
-		TIM6->SR &= ~TIM_SR_UIF;
-		imu_update_flag = 1;
-		pid_update_flag = 1;
-	}
 }

Source/INS/IRS.c

@@ -1,8 +1,11 @@
 #include "irs.h"
 #include <math.h>
 
-#define MAHONY_KP 2.5f
-#define MAHONY_KI 0.01f
+#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};
 
@@ -14,69 +17,191 @@ void IRS_init(IRS* irs)
     irs->q.z = 0.0f;
 }
 
-Vector3 IRS_getGravity(const IRS* irs)
-{
-    const Quaternion* q = &irs->q;
 
-    Vector3 g;
-    g.x = 2 * (q->x*q->z - q->w*q->y);
-    g.y = 2 * (q->w*q->x + q->y*q->z);
-    g.z = q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z;
+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,
+		gyro_in->y * dt * DEG2RAD,
+		gyro_in->z * dt * DEG2RAD
+	};
+	
+	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);
+	
+	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};
+	
+	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)
+{
+	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;
 }
 
-void IRS_update(IRS* irs, const Vector3* gyro_in, const Vector3* accel_in, float dt)
-{
-    Vector3 gyro = *gyro_in;
-    Vector3 accel = *accel_in;
 
-    // --- нормализация акселя
-    float norm = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
 
-    if (norm > 1e-6f)
-    {
-        accel.x /= norm;
-        accel.y /= norm;
-        accel.z /= norm;
 
-        Vector3 g = IRS_getGravity(irs);
 
-        Vector3 error =
-        {
-            accel.y*g.z - accel.z*g.y,
-            accel.z*g.x - accel.x*g.z,
-            accel.x*g.y - accel.y*g.x
-        };
 
-        gyro_bias.x += MAHONY_KI * error.x * dt;
-        gyro_bias.y += MAHONY_KI * error.y * dt;
-        gyro_bias.z += MAHONY_KI * error.z * dt;
 
-        gyro.x += MAHONY_KP * error.x + gyro_bias.x;
-        gyro.y += MAHONY_KP * error.y + gyro_bias.y;
-        gyro.z += MAHONY_KP * error.z + gyro_bias.z;
-    }
 
-    irs->gyro = gyro;
-    irs->accel = accel;
 
-    // --- интеграция кватерниона
-    Quaternion q = irs->q;
 
-    Quaternion omega =
-    {
-        gyro.x * DEG2RAD,
-        gyro.y * DEG2RAD,
-        gyro.z * DEG2RAD,
-        0
-    };
-
-    Quaternion dq = QuatProd(&q, &omega);
-    dq = QuatConstProd(&dq, 0.5f * dt);
-
-    q = QuatSum(&q, &dq);
-    q = QuatNormalize(&q, 1.0f);
-
-    irs->q = q;
-}

Source/INS/IRS.h

@@ -9,16 +9,22 @@
 
 typedef struct
 {
-    Quaternion q;   // ориентация
-    Vector3 gyro;   // deg/s
-    Vector3 accel;  // g
+    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);
 
-Vector3 IRS_getGravity(const IRS* irs);
+void setAccelShift(IRS* irs, const float pitch, const float roll, const float yaw);
+
+Vector3 IRS_getGravity(const Quaternion* q);
 
 #endif

Source/INS/geometry/quaternion.c

@@ -1,6 +1,8 @@
 #include "quaternion.h"
 #include <math.h>
 
+#define PI		3.14159265359f
+
 Quaternion QuatNormalize(const Quaternion* q, const float gain)
 {
   Quaternion res = {};
@@ -134,7 +136,24 @@ Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, boo
 	return error;
 }
 
-
+Vector3 QuatToEuler(const Quaternion* q)
+{
+	Vector3 e;
+
+    e.x = atan2f(2*(q->w*q->x + q->y*q->z),
+                 1 - 2*(q->x*q->x + q->y*q->y));
+
+    e.y = asinf(2*(q->w*q->y - q->z*q->x));
+
+    e.z = atan2f(2*(q->w*q->z + q->x*q->y),
+                 1 - 2*(q->y*q->y + q->z*q->z));
+
+    e.x *= 180.0f / PI;
+    e.y *= 180.0f / PI;
+    e.z *= 180.0f / PI;
+
+    return e;
+}
 
 
 

Source/INS/geometry/quaternion.h

@@ -25,6 +25,7 @@ Vector3 QuatRotateAroundZ(const Quaternion* q, const Vector3* vec, bool CCW);
 Quaternion QuatCreateFromEuler(const Vector3* eulerAngels);
 Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, bool fastWay);
 
+Vector3 QuatToEuler(const Quaternion* q);
 
 #endif
 

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

Source/main.c

@@ -8,6 +8,10 @@
 #include "pid.h"
 #include "lidar.h"
 
+void TIM6_DAC_IRQHandler();
+
+static uint8_t irs_update_flag = 0;
+static uint8_t control_update_flag = 0;
 
 imu_scaled_t imu;
 IRS irs;
@@ -17,6 +21,8 @@ rc_channels rx_chs_normalized;
 control_channels_t ctrl_chs;
 lidar_data lidar;
 
+Vector3 euler;
+
 void delay_ms(uint32_t ms);
 
 int main(void)
@@ -31,11 +37,12 @@ int main(void)
 	i2c_gpio_init();
 	i2c1_init();
 	imu_init();
-	imu_tim6_init();
-	imu_processing_init();
 	
+	imu_processing_init();
 	imu_calibrate();
 	
+	imu_tim6_init();
+	
 	IRS_init(&irs);
 	
 	attitude_init(&attitude);
@@ -45,47 +52,59 @@ int main(void)
 	motors_init();
 	
 	while (1)
-{
-    receiver_update(&rx_chs_raw);
-    rx_chs_normalized = normalize_channels(rx_chs_raw);
-
-    if (imu_update_flag)
-    {
-        imu_update_flag = 0;
-
-        imu_read_scaled(&imu);
-
-        Vector3 gyro = {imu.gx, imu.gy, imu.gz};
-        Vector3 accel = {imu.ax, imu.ay, imu.az};
-
-        IRS_update(&irs, &gyro, &accel, IMU_DT);
-    }
-
-    if (pid_update_flag)
-    {
-        pid_update_flag = 0;
-
-        attitude_controller_update(
-            &ctrl_chs,
-            &rx_chs_normalized,
-            &irs.q,
-            &irs.gyro
-        );
-
-        if (rx_chs_normalized.rc_armed)
-        {
-            motors_set_throttle_mix(
-                rx_chs_normalized.rc_throttle,
-                &ctrl_chs,
-                rx_chs_normalized.rc_armed
-            );
-        }
-        else
-        {
-            motors_turn_off();
-        }
-    }
+	{
+	    receiver_update(&rx_chs_raw);
+	    rx_chs_normalized = normalize_channels(rx_chs_raw);
+		
+		if (irs_update_flag)
+		{
+		    irs_update_flag = 0;
+			
+			imu_read_scaled(&imu);
+		
+		    Vector3 gyro = {imu.gx, imu.gy, imu.gz};
+		    Vector3 accel = {imu.ax, imu.ay, imu.az};
+		
+		    IRS_update(&irs, &gyro, &accel, IMU_DT);
+		}
+		
+		euler = QuatToEuler(&irs.q);
+		
+		if (control_update_flag)
+		{
+		    control_update_flag = 0;
+		
+		    attitude_controller_update(
+		        &ctrl_chs,
+		        &rx_chs_normalized,
+		        &irs.q,
+		        &irs.gyro
+		    );
+		}
+		
+		if (rx_chs_normalized.rc_armed)
+	    {
+	        motors_set_throttle_mix(
+	            rx_chs_normalized.rc_throttle,
+	            &ctrl_chs,
+	            rx_chs_normalized.rc_armed
+	        );
+	    }
+	    else
+		{
+			motors_turn_off();
+		}
+	}
 }
+
+void TIM6_DAC_IRQHandler()
+{
+	if (TIM6->SR & TIM_SR_UIF)
+	{
+		TIM6->SR &= ~TIM_SR_UIF;
+		irs_update_flag = 1;
+		control_update_flag = 1;
+	}
 }
 
 void delay_ms(uint32_t ms)