From 469ec64d45c8cf3a23d182aa91c2290c74db1722 Mon Sep 17 00:00:00 2001
From: Radzhab Bisultanov <bisultanov@sfedu.ru>
Date: Thu, 12 Mar 2026 13:54:48 +0300
Subject: [PATCH] =?UTF-8?q?=D0=92=D0=BA=D0=BB=D1=8E=D1=87=D0=B5=D0=BD=20lo?=
 =?UTF-8?q?w=20pass=20=D1=84=D0=B8=D0=BB=D1=8C=D1=82=D1=80=20=D0=B4=D0=B0?=
 =?UTF-8?q?=D1=82=D1=87=D0=B8=D0=BA=D0=BE=D0=B2,=20=D1=83=D0=BC=D0=B5?=
 =?UTF-8?q?=D0=BD=D1=8C=D1=88=D0=B5=D0=BD=20=D1=81=D1=80=D0=B5=D0=B7=20?=
 =?UTF-8?q?=D0=B1=D0=B8=D0=BA=D0=B2=D0=B0=D0=B4=D0=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Source/BSP/Inc/imu.h            | 12 ++++++++++++
 Source/BSP/Src/imu.c            |  4 ++--
 Source/BSP/Src/imu_processing.c | 12 ++++++------
 3 files changed, 20 insertions(+), 8 deletions(-)

diff --git a/Source/BSP/Inc/imu.h b/Source/BSP/Inc/imu.h
index c16dafa..3182ae7 100644
--- a/Source/BSP/Inc/imu.h
+++ b/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
diff --git a/Source/BSP/Src/imu.c b/Source/BSP/Src/imu.c
index 7c91e88..fa7cf6a 100644
--- a/Source/BSP/Src/imu.c
+++ b/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));
diff --git a/Source/BSP/Src/imu_processing.c b/Source/BSP/Src/imu_processing.c
index 7628ffa..876673c 100644
--- a/Source/BSP/Src/imu_processing.c
+++ b/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_y_lpf, 30.0f, 500.0f);
-	biquad_init_lpf(&accel_z_lpf, 30.0f, 500.0f);
+	biquad_init_lpf(&accel_x_lpf, 25.0f, 500.0f);
+	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, 120.0f, 500.0f);
-	biquad_init_lpf(&gyro_y_lpf, 120.0f, 500.0f);
-	biquad_init_lpf(&gyro_z_lpf, 120.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)