Включен low pass фильтр датчиков, уменьшен срез биквада

Source/BSP/Inc/imu.h

@@ -1,3 +1,5 @@
+#pragma once
+
 #ifndef IMU_H
 #define IMU_H
 
@@ -10,6 +12,16 @@
 #define REG_ACCEL_CONFIG		0x14
 #define IMU_DT					0.002f   // 2 ms
 
+#define GYRO_FS_SEL_2000		3 << 1
+#define GYRO_DLPFCFG_73			3 << 3
+#define GYRO_FCHOICE_ON			1
+#define GYRO_FCHOICE_OFF		0
+
+#define ACCEL_FS_SEL_8			2 << 1
+#define ACCEL_DLPFCFG_69		3 << 3
+#define ACCEL_FCHOICE_ON		1
+#define ACCEL_FCHOICE_OFF		0
+
 typedef struct
 {
 	int16_t ax, ay, az; // lsb

Source/BSP/Src/imu.c

@@ -54,10 +54,10 @@ void imu_init()
 	i2c_write(ICM_ADDR, REG_BANK_SEL, 2 << 4);
 	
 	// gyro ~2000 dps, FS_SEL = 3
-	i2c_write(ICM_ADDR, REG_GYRO_CONFIG_1, (3 << 1));
+	i2c_write(ICM_ADDR, REG_GYRO_CONFIG_1, GYRO_FS_SEL_2000 | GYRO_DLPFCFG_73 | GYRO_FCHOICE_ON);
 	
 	// accel ~4g, FS_SEL = 1
-	i2c_write(ICM_ADDR, REG_ACCEL_CONFIG, (1 << 1));
+	i2c_write(ICM_ADDR, REG_ACCEL_CONFIG, ACCEL_FS_SEL_8 | ACCEL_DLPFCFG_69 | ACCEL_FCHOICE_ON);
 	
 	// back to bank 0
 	i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));

Source/BSP/Src/imu_processing.c

@@ -20,13 +20,13 @@ static biquad_t gyro_z_lpf;
 
 void imu_processing_init()
 {
-	biquad_init_lpf(&accel_x_lpf, 30.0f, 500.0f);
+	biquad_init_lpf(&accel_x_lpf, 25.0f, 500.0f);
-	biquad_init_lpf(&accel_y_lpf, 30.0f, 500.0f);
+	biquad_init_lpf(&accel_y_lpf, 25.0f, 500.0f);
-	biquad_init_lpf(&accel_z_lpf, 30.0f, 500.0f);
+	biquad_init_lpf(&accel_z_lpf, 25.0f, 500.0f);
 	
-	biquad_init_lpf(&gyro_x_lpf, 120.0f, 500.0f);
+	biquad_init_lpf(&gyro_x_lpf, 25.0f, 500.0f);
-	biquad_init_lpf(&gyro_y_lpf, 120.0f, 500.0f);
+	biquad_init_lpf(&gyro_y_lpf, 25.0f, 500.0f);
-	biquad_init_lpf(&gyro_z_lpf, 120.0f, 500.0f);
+	biquad_init_lpf(&gyro_z_lpf, 25.0f, 500.0f);
 }
 
 void imu_read_scaled(imu_scaled_t* out)

Source/INS/IRS.h

@@ -6,33 +6,35 @@
 #include "quaternion.h"
 #include "vector.h"
 
-Quaternion orientationQuat;
-Vector3 tilts;					// local oriented sinX sinY cosZ
-
 typedef struct
 {
-	Vector3 Gyro, Accel;
+	Quaternion orientationQ;			// главный кватерион ориентации
-} IMU;							// последние значения датчиков
+	// Vector3 tilts;					// локально ориентированные наклоны sinX sinY cosZ
+	
+	Vector3 gyro, accel;				// последние значения датчиков
+	
+	float roll, pitch, yaw;
 
-typedef struct
+	// Vector3 accel, spd, pos;			// инерциальные значения движения					
-{
+} IRS;
-	Vector3 Accel, Spd, Pos;
+
-} Inertial;						// инерциальные значения движения
+void updateIRS(IRS* irs, const Vector3* gyro, const Vector3* accel, float dt);
+void IRSgetEuler(const IRS* irs);
 
 void updateGyro(Vector3* gyro, const Vector3* newGyro);
 void updateAccel(Vector3* accel, const Vector3* newAccel);
 
-void setShiftAlpha(const Vector3* pos, const Vector3* spd, const float* qua);	// коэффициент восстановления позиции и скорости в секунду
+// void setShiftAlpha(const Vector3* pos, const Vector3* spd, const float* qua);	// коэффициент восстановления позиции и скорости в секунду
 
-void updatePosSpeed();
+// void updatePosSpeed();
 void updateQuat();
 
-void restoreAllShift();
+// void restoreAllShift();
 
-void setAccelShift();
+// void setAccelShift();
 
-void getSinXYCosZ();
+// void getSinXYCosZ();
-void getRollPitchYaw();
+// void getRollPitchYaw();
-void getInertial();
+// void getInertial();
 
 #endif