Обновлена архитектура обработки IMU

Source/BSP/Src/imu_processing.c

@@ -24,9 +24,9 @@ void imu_processing_init()
 	biquad_init_lpf(&accel_y_lpf, 25.0f, 500.0f);
 	biquad_init_lpf(&accel_z_lpf, 25.0f, 500.0f);
 	
-	biquad_init_lpf(&gyro_x_lpf, 100.0f, 500.0f);
-	biquad_init_lpf(&gyro_y_lpf, 100.0f, 500.0f);
-	biquad_init_lpf(&gyro_z_lpf, 100.0f, 500.0f);
+	biquad_init_lpf(&gyro_x_lpf, 25.0f, 500.0f);
+	biquad_init_lpf(&gyro_y_lpf, 25.0f, 500.0f);
+	biquad_init_lpf(&gyro_z_lpf, 25.0f, 500.0f);
 }
 
 void imu_read_scaled(imu_scaled_t* out)

Source/Control/Inc/attitude.h

@@ -1,60 +1,28 @@
 #ifndef ATTITUDE_H
 #define ATTITUDE_H
 
-#include "imu_processing.h"
-#include "radio_receiver.h"
-#include "pid.h"
 #include "quaternion.h"
 #include "vector.h"
-
-#define CF_ALPHA		0.99f
-#define GYRO_BIAS_LIM	0.5f
-#define RATE_LIM		300.0f
-
-/*typedef struct
-{
-	float roll;			// deg
-	float pitch;		// deg
-	float yaw_rate;		// deg/s
-} attitude_t;*/
+#include "pid.h"
+#include "radio_receiver.h"
 
 typedef struct
 {
-	Quaternion orientation;	// current orientation
-	Vector3 gyro;			// current angular velocity
+    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_estimator_update(attitude_t* att, const imu_scaled_t* imu);
-void attitude_controller_update(control_channels_t* control, const rc_channels* rx, const attitude_t* att);
 
-void TIM6_DAC_IRQHandler();
-void attitude_update(attitude_t* attitude, imu_scaled_t* imu);
-void attitude_pid_update(control_channels_t* control, const rc_channels* rx, const attitude_t* att);
-
-Vector3 gravity_from_quat(const Quaternion* q);
-Quaternion rx_to_quaternion(const rc_channels* rx);
-
-float constrain(float x, float min, float max);
-
-///////////////////////////////////////////////////////////////////////////////
-
-void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu);
-void yaw_rate_update(attitude_t* attitude, imu_scaled_t* imu);
-/*void attitude_pid_update(control_channels_t* control, 
+void attitude_controller_update(control_channels_t* control,
                                 const rc_channels* rx,
-                         const attitude_t* att, 
-                         const imu_scaled_t* imu);*/
-
-
-float accel_roll_deg(const imu_scaled_t* imu);
-float accel_pitch_deg(const imu_scaled_t* imu);
-
-void integrate_gyro_roll_deg(float* roll, const imu_scaled_t* imu);
-void integrate_gyro_pitch_deg(float* pitch, const imu_scaled_t* imu);
-void integrate_gyro_yaw_deg(float* yaw, const imu_scaled_t* imu);
-
+                                const Quaternion* current_q,
+                                const Vector3* gyro);
 
+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

@@ -1,99 +1,32 @@
 #include "attitude.h"
 #include "pid.h"
+#include "imu.h"
+#include "imu_processing.h"
 #include <math.h>
 
-#define MAHONY_KP	2.5f
-#define MAHONY_KI	0.01f
+#define RATE_LIM 300.0f
 
-static Vector3 gyro_bias = {};
-
-volatile uint8_t imu_update_flag = 0;
-volatile uint8_t pid_update_flag = 0;
-
-float angle_kp_roll 	= 2.5f;
 float angle_kp_pitch = 2.5f;
+float angle_kp_roll  = 2.5f;
 float angle_kp_yaw   = 2.0f;
 
-pid_t pid_roll 	= {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f};
-pid_t pid_pitch = {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f};
-pid_t pid_yaw 	= {.kp = 1.8f, .ki = 0.6f, .kd = 0.0f};
-
-/*int16_t desired_roll = 0;
-int16_t desired_pitch = 0;
-
-float desired_roll_rate = 0.0f;
-float desired_pitch_rate = 0.0f;
-
-float roll_rate_error = 0.0f;
-float pitch_rate_error = 0.0f;
-float yaw_rate_error = 0.0f;
-
-float error_roll = 0.0f;
-float error_pitch = 0.0f;
-float error_yaw = 0.0f;*/
+pid_t pid_pitch = {.kp = 0.6f, .kd = 0.025f};
+pid_t pid_roll  = {.kp = 0.6f, .kd = 0.025f};
+pid_t pid_yaw   = {.kp = 0.6f};
 
 void attitude_init(attitude_t* att)
 {
-	att->orientation.w = 1.0f;
-	att->orientation.x = 0.0f;
-	att->orientation.y = 0.0f;
-	att->orientation.z = 0.0f;
-	
-	att->gyro.x = 0.0f;
-	att->gyro.y = 0.0f;
-	att->gyro.z = 0.0f;
+    att->gyro = (Vector3){0};
 }
 
-void attitude_estimator_update(attitude_t* att, const imu_scaled_t* imu)
-{
-	Vector3 gyro 	= {imu->gx, imu->gy, imu->gz};
-	Vector3 accel 	= {imu->ax, imu->ay, imu->az};
-	
-	float norm = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
-	
-	if (norm > 0.0001f)
-	{
-		accel.x /= norm;
-		accel.y /= norm;
-		accel.z /= norm;
-		
-		Vector3 g = gravity_from_quat(&att->orientation);
-		
-		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 * IMU_DT;
-		gyro_bias.y += MAHONY_KI * error.y * IMU_DT;
-		gyro_bias.z += MAHONY_KI * error.z * IMU_DT;
-		
-		gyro_bias.x = constrain(gyro_bias.x, -GYRO_BIAS_LIM, GYRO_BIAS_LIM);
-		gyro_bias.y = constrain(gyro_bias.y, -GYRO_BIAS_LIM, GYRO_BIAS_LIM);
-		gyro_bias.z = constrain(gyro_bias.z, -GYRO_BIAS_LIM, GYRO_BIAS_LIM);
-		
-		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;
-	}
-	
-	att->gyro = gyro;
-	
-	Quaternion h = {gyro.x * DEG2RAD, gyro.y * DEG2RAD, gyro.z * DEG2RAD, 0};
-	
-	Quaternion dq = QuatProd(&att->orientation, &h);
-	dq = QuatConstProd(&dq, 0.5f * IMU_DT);
-	att->orientation = QuatSum(&att->orientation, &dq);
-	
-	att->orientation = QuatNormalize(&att->orientation, 1.0f);
-}
-
-void attitude_controller_update(control_channels_t* control, const rc_channels* rx, const attitude_t* att)
+void attitude_controller_update(control_channels_t* control,
+                                const rc_channels* rx,
+                                const Quaternion* current_q,
+                                const Vector3* gyro)
 {
     Quaternion q_target = rx_to_quaternion(rx);
-	Quaternion q_error 	= QuatGetError(&att->orientation, &q_target, true);
+
+    Quaternion q_error = QuatGetError(current_q, &q_target, true);
 
     Vector3 angle_error =
     {
@@ -104,8 +37,8 @@ void attitude_controller_update(control_channels_t* control, const rc_channels*
 
     Vector3 desired_rate =
     {
-		angle_error.x * angle_kp_roll,
-		angle_error.y * angle_kp_pitch,
+        angle_error.x * angle_kp_pitch,
+        angle_error.y * angle_kp_roll,
         angle_error.z * angle_kp_yaw
     };
 
@@ -113,35 +46,26 @@ void attitude_controller_update(control_channels_t* control, const rc_channels*
     desired_rate.y = constrain(desired_rate.y, -RATE_LIM, RATE_LIM);
     desired_rate.z = constrain(desired_rate.z, -RATE_LIM, RATE_LIM);
 
-	float roll_error 	= desired_rate.x - att->gyro.x;
-	float pitch_error 	= desired_rate.y - att->gyro.y;
-	float yaw_error 	= desired_rate.z - att->gyro.z;
-	
-	control->roll 	= pid_update(&pid_roll, roll_error, att->gyro.x, IMU_DT);
-	control->pitch 	= pid_update(&pid_pitch, pitch_error, att->gyro.y, IMU_DT);
-	control->yaw 	= pid_update(&pid_yaw, yaw_error, att->gyro.z, IMU_DT);
+    control->pitch = pid_update(&pid_pitch, desired_rate.x - gyro->x, gyro->x, IMU_DT);
+    control->roll  = pid_update(&pid_roll,  desired_rate.y - gyro->y, gyro->y, IMU_DT);
+    control->yaw   = pid_update(&pid_yaw,   desired_rate.z - gyro->z, gyro->z, IMU_DT);
 }
 
 Quaternion rx_to_quaternion(const rc_channels* rx)
 {
-	float roll 	= int_mapping(rx->rc_roll, -500, 500, -45, 45) * DEG2RAD;
 	float pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45) * DEG2RAD;
+	float roll 	= int_mapping(rx->rc_roll, -500, 500, -45, 45) * DEG2RAD;
 	float yaw 	= 0;
 	
-	Vector3 rpy = {roll, pitch, yaw};
+	Vector3 pry = {pitch, roll, yaw};
 	
-	return QuatCreateFromEuler(&rpy);
+	return QuatCreateFromEuler(&pry);
 }
 
-Vector3 gravity_from_quat(const Quaternion* q)
+float constrain(float x, float min, float max)
 {
-	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;
-	
-	return g;
+	if (x < min) x = min; else if (x > max) x = max;
+	return x;
 }
 
 void TIM6_DAC_IRQHandler()
@@ -153,111 +77,3 @@ void TIM6_DAC_IRQHandler()
 		pid_update_flag = 1;
 	}
 }
-
-void attitude_update(attitude_t* attitude, imu_scaled_t* imu)
-{
-	if (!imu_update_flag)
-		return;
-	
-	imu_update_flag = 0;
-	
-	imu_read_scaled(imu);
-	attitude_estimator_update(attitude, imu);
-	
-	// complementary_filter_update(attitude, imu);
-	// yaw_rate_update(attitude, imu);
-}
-
-void attitude_pid_update(control_channels_t* control, const rc_channels* rx, const attitude_t* att)
-{
-	if (!pid_update_flag)
-		return;
-	pid_update_flag = 0;
-	attitude_controller_update(control, rx, att);
-}
-
-float constrain(float x, float min, float max)
-{
-	if (x < min) x = min; else if (x > max) x = max;
-	return x;
-}
-
-/*void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu)
-{
-	static float roll_acc;
-	static float pitch_acc;
-	
-	roll_acc = accel_roll_deg(imu);
-	pitch_acc = accel_pitch_deg(imu);
-	
-	integrate_gyro_roll_deg(&att->roll, imu);
-	integrate_gyro_pitch_deg(&att->pitch, imu);
-	
-	att->roll = CF_ALPHA * att->roll + (1 - CF_ALPHA) * roll_acc;
-	att->pitch = CF_ALPHA * att->pitch + (1 - CF_ALPHA) * pitch_acc;
-}*/
-
-/*void yaw_rate_update(attitude_t* attitude, imu_scaled_t* imu)
-{
-	attitude->yaw_rate = imu->gz;
-}
-
-void attitude_pid_update(control_channels_t* control, 
-                         const rc_channels* rx, 
-                         const attitude_t* att,
-                         const imu_scaled_t* imu)
-{
-	if (pid_update_flag)
-	{
-		pid_update_flag = 0;
-		
-		desired_roll  = int_mapping(rx->rc_roll, -500, 500, -45, 45);
-		desired_pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45);
-		
-		desired_roll_rate  = angle_kp_roll  * (desired_roll - att->roll);
-		desired_pitch_rate = angle_kp_pitch * (desired_pitch - att->pitch);
-		
-		if (desired_roll_rate > 200) desired_roll_rate = 200;
-		if (desired_roll_rate < -200) desired_roll_rate = -200;
-		
-		if (desired_pitch_rate > 200)  desired_pitch_rate = 200;
-		if (desired_pitch_rate < -200) desired_pitch_rate = -200;
-		
-		roll_rate_error = desired_roll_rate - imu->gy;
-		pitch_rate_error = desired_pitch_rate - imu->gx;
-		yaw_rate_error = - imu->gz;
-		
-		control->roll  = pid_update(&pid_roll, roll_rate_error, imu->gy, IMU_DT);
-		control->pitch = pid_update(&pid_pitch, pitch_rate_error, imu->gx, IMU_DT);
-		control->yaw   = pid_update(&pid_yaw, yaw_rate_error, imu->gz, IMU_DT);
-	}
-}*/
-
-float accel_roll_deg(const imu_scaled_t* imu) {
-	// right-left
-	return atan2f(imu->ax, sqrtf(imu->ay * imu->ay + imu->az * imu->az)) * 180.0f / PI;
-}
-
-float accel_pitch_deg(const imu_scaled_t* imu)
-{
-	// forward-backward
-	return atan2f(-imu->ay, sqrtf(imu->ax * imu->ax + imu->az * imu->az)) * 180.0f / PI;
-}
-
-void integrate_gyro_roll_deg(float* roll, const imu_scaled_t* imu)
-{
-	// right-left
-	*roll += imu->gy * IMU_DT;
-}
-
-void integrate_gyro_pitch_deg(float* pitch, const imu_scaled_t* imu)
-{
-	// forward-backward
-	*pitch += imu->gx * IMU_DT;
-}
-
-void integrate_gyro_yaw_deg(float* yaw, const imu_scaled_t* imu)
-{
-	// forward-backward
-	*yaw += imu->gz * IMU_DT;
-}

Source/INS/IRS.c

@@ -1 +1,82 @@
-#include "IRS.h"
+#include "irs.h"
+#include <math.h>
+
+#define MAHONY_KP 2.5f
+#define MAHONY_KI 0.01f
+
+static Vector3 gyro_bias = {0};
+
+void IRS_init(IRS* irs)
+{
+    irs->q.w = 1.0f;
+    irs->q.x = 0.0f;
+    irs->q.y = 0.0f;
+    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;
+
+    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

@@ -1,40 +1,24 @@
-#pragma once
-
 #ifndef IRS_H
 #define IRS_H
 
 #include "quaternion.h"
 #include "vector.h"
 
+#define PI			3.14159265359f
+#define DEG2RAD		PI / 180.0f
+
 typedef struct
 {
-	Quaternion orientationQ;			// главный кватерион ориентации
-	// Vector3 tilts;					// локально ориентированные наклоны sinX sinY cosZ
+    Quaternion q;   // ориентация
+    Vector3 gyro;   // deg/s
+    Vector3 accel;  // g
 
-	Vector3 gyro, accel;				// последние значения датчиков
-	
-	float roll, pitch, yaw;
-
-	// Vector3 accel, spd, pos;			// инерциальные значения движения					
 } IRS;
 
-void updateIRS(IRS* irs, const Vector3* gyro, const Vector3* accel, float dt);
-void IRSgetEuler(const IRS* irs);
+void IRS_init(IRS* irs);
 
-void updateGyro(Vector3* gyro, const Vector3* newGyro);
-void updateAccel(Vector3* accel, const Vector3* newAccel);
+void IRS_update(IRS* irs, const Vector3* gyro, const Vector3* accel, float dt);
 
-// void setShiftAlpha(const Vector3* pos, const Vector3* spd, const float* qua);	// коэффициент восстановления позиции и скорости в секунду
-
-// void updatePosSpeed();
-void updateQuat();
-
-// void restoreAllShift();
-
-// void setAccelShift();
-
-// void getSinXYCosZ();
-// void getRollPitchYaw();
-// void getInertial();
+Vector3 IRS_getGravity(const IRS* irs);
 
 #endif

Source/main.c

@@ -1,6 +1,7 @@
 #include "stm32g431xx.h"
 #include "imu.h"
 #include "imu_processing.h"
+#include "IRS.h"
 #include "attitude.h"
 #include "radio_receiver.h"
 #include "motors.h"
@@ -9,6 +10,7 @@
 
 
 imu_scaled_t imu;
+IRS irs;
 attitude_t attitude;
 rc_channels rx_chs_raw;
 rc_channels rx_chs_normalized;
@@ -34,6 +36,8 @@ int main(void)
 	
 	imu_calibrate();
 	
+	IRS_init(&irs);
+	
 	attitude_init(&attitude);
 	
 	receiver_init();
@@ -41,16 +45,40 @@ int main(void)
 	motors_init();
 	
 	while (1)
-	{
+{
     receiver_update(&rx_chs_raw);
     rx_chs_normalized = normalize_channels(rx_chs_raw);
 
-		attitude_update(&attitude, &imu);
-		attitude_pid_update(&ctrl_chs, &rx_chs_normalized, &attitude);
+    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);
+            motors_set_throttle_mix(
+                rx_chs_normalized.rc_throttle,
+                &ctrl_chs,
+                rx_chs_normalized.rc_armed
+            );
         }
         else
         {
@@ -58,6 +86,7 @@ int main(void)
         }
     }
 }
+}
 
 void delay_ms(uint32_t ms)
 {