last_rab_alpha+hysteresis

Co-authored-by: Copilot <copilot@github.com>

imu.c

@@ -22,8 +22,7 @@
 #define FMAC_MEM_SIZE      256
 #define STAGE_SIZE         10  // Резервируем с запасом под каждый каскад
 
-// raw_ax, raw_ay, raw_az удалены (не используются)
-volatile int16_t raw_gx; // Нужен только для гироскопа X
+volatile int16_t raw_gx; // только  гироскопа X
 float filt_gx;
 float gyro_bias_x = 0;
 
@@ -127,7 +126,7 @@ void IMU_Init(void) {
     // Значение GYRO_DLPFCFG = 7 для макс. ширины без полного байпаса: ~361.4 Hz LPF.
     // (0 - это 196Hz, 1 - 151Hz, 2 - 119Hz, 3 - 51Hz, 7 - 361.4Hz)
     // GYRO_FS_SEL = 3 (2000 dps)
-    // ВНИМАНИЕ: На ICM-20948 бит GYRO_FCHOICE (бит 0) включает LPF, если равен 1! (а 0 = Bypass)
+    // ВНИМАНИЕ: На ICM-20948 бит GYRO_FCHOICE (бит 0) включает LPF, если равен 1. (а 0 = Bypass)
     IMU_WriteReg(0x01, (7 << 3) | (3 << 1) | 1); 
     
     IMU_SetBank(0);
@@ -164,16 +163,16 @@ void IMU_ReadRawData(void) {
     // 2. Центрируем относительно нуля (убираем дрейф)
     float x = (float)gyro_x_raw - gyro_bias_x;
     
-    // 3. СОХРАНЯЕМ это в raw_gx (это будет красная линия в Python)
+    // 3. Сохраняем это в raw_gx
     raw_gx = (int16_t)x; 
 
-    // 4. Прогоняем через 3 каскада программных фильтров (режекторные)
+    // 4. Прогоняем через 3 каскада программных фильтров (режектор)
     float x_filtered = x;
     x_filtered = biquad_apply(&dyn_notch_filters[0], x_filtered);
     x_filtered = biquad_apply(&dyn_notch_filters[1], x_filtered);
     x_filtered = biquad_apply(&dyn_notch_filters[2], x_filtered);
 
-    // 5. Сохраняем в filt_gx (это будет синяя линия)
+    // 5. Сохраняем в filt_gx
     filt_gx = x_filtered;
 }
 
@@ -186,6 +185,13 @@ void Update_FMAC_Coeffs(int stage, float b0, float b1, float b2, float a1, float
     dyn_notch_filters[stage].b2 = b2;
     dyn_notch_filters[stage].a1 = a1;
     dyn_notch_filters[stage].a2 = a2;
+    
+    // Если переходим из Bypass (b0≈1) в активный фильтр (b0<1),
+    // обнуляем состояние, чтобы избежать скачков
+    if (b0 < 0.9f) {
+        dyn_notch_filters[stage].d1 = 0;
+        dyn_notch_filters[stage].d2 = 0;
+    }
 }
 
 // Внутренняя функция для обработки одного каскада через FMAC
@@ -213,7 +219,7 @@ static int16_t FMAC_Step(fmac_coeffs_t *c, fmac_state_t *s, int16_t input) {
     FMAC->WDATA = input;
     
     uint32_t timeout = 1000;
-    while ((FMAC->SR & 0x01) && --timeout);
+    while (!(FMAC->SR & 0x01) && --timeout);  // Ждем флаг VLD (Valid Data)
     
     if (timeout == 0) return input;