last_rab_alpha

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

imu.c

@@ -30,6 +30,22 @@ float gyro_bias_x = 0;
 // notch1, notch2, notch3 удалены (заменены на notch_fmac_coeffs[3])
 // biquad_apply и biquad_init_notch удалены (больше не нужны с FMAC)
 
+#define M_PI 3.14159265358979323846f
+
+void biquad_init_notch(biquad_t *f, float freq, float q, float fs) {
+    float omega = 2.0f * M_PI * freq / fs;
+    float alpha = sinf(omega) / (2.0f * q);
+    
+    float a0 = 1.0f + alpha;
+    f->b0 = 1.0f / a0;
+    f->b1 = -2.0f * cosf(omega) / a0;
+    f->b2 = 1.0f / a0;
+    f->a1 = -2.0f * cosf(omega) / a0;
+    f->a2 = (1.0f - alpha) / a0;
+    f->d1 = 0;
+    f->d2 = 0;
+}
+
 fmac_coeffs_t notch_fmac_coeffs[3];
 fmac_state_t  notch_fmac_state[3];
 
@@ -87,6 +103,18 @@ static void IMU_WriteReg(uint8_t reg, uint8_t val) {
 
 void IMU_SetBank(uint8_t bank) { IMU_WriteReg(0x7F, (bank & 0x03) << 4); }
 
+float biquad_apply(biquad_t *f, float x) {
+    // Реализация Direct Form II (правильная математика для IIR)
+    float w = x - f->a1 * f->d1 - f->a2 * f->d2;
+    float y = f->b0 * w + f->b1 * f->d1 + f->b2 * f->d2;
+    f->d2 = f->d1;
+    f->d1 = w;
+    return y;
+}
+
+// 3 динамических каскада
+biquad_t dyn_notch_filters[3];
+
 void IMU_Init(void) {
     // Пробуждение...
     IMU_SetBank(0);
@@ -95,22 +123,25 @@ void IMU_Init(void) {
     IMU_WriteReg(0x07, 0x00);
     
     IMU_SetBank(2);
-    IMU_WriteReg(0x01, 0x01); // Bypass (отключаем встроенный фильтр для анализа)
+    // ICM-20948: GYRO_CONFIG_1 находится в нулевом регистре второго банка. 
+    // Значение 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)
+    IMU_WriteReg(0x01, (7 << 3) | (3 << 1) | 1); 
+    
     IMU_SetBank(0);
 
-    // b0 = 1.0 (в Q14 это 16384), остальные 0
+    // Инициализируем фильтры в режиме Bypass (пропускание)
     for (int i = 0; i < 3; i++) {
-    notch_fmac_coeffs[i].b0 = 0; 
-    notch_fmac_coeffs[i].b1 = 0;
-    notch_fmac_coeffs[i].b2 = 0;
-    notch_fmac_coeffs[i].a1 = 0;
-    notch_fmac_coeffs[i].a2 = 0;
-    
-    notch_fmac_state[i].x1 = 0; 
-    notch_fmac_state[i].x2 = 0;
-    notch_fmac_state[i].y1 = 0; 
-    notch_fmac_state[i].y2 = 0;
-}
+        dyn_notch_filters[i].b0 = 1.0f;
+        dyn_notch_filters[i].b1 = 0;
+        dyn_notch_filters[i].b2 = 0;
+        dyn_notch_filters[i].a1 = 0;
+        dyn_notch_filters[i].a2 = 0;
+        dyn_notch_filters[i].d1 = 0;
+        dyn_notch_filters[i].d2 = 0;
+    }
 }
 
 void IMU_Calibrate(void) {
@@ -127,30 +158,34 @@ void IMU_ReadRawData(void) {
     uint8_t buf[14];
     I2C_ReadMulti(IMU_ADDR, 0x2D, buf, 14);
     
-    raw_gx = (int16_t)(buf[6] << 8 | buf[7]);
-    float x = (float)raw_gx - gyro_bias_x;
+    // 1. Читаем сырое
+    int16_t gyro_x_raw = (int16_t)(buf[6] << 8 | buf[7]);
+    
+    // 2. Центрируем относительно нуля (убираем дрейф)
+    float x = (float)gyro_x_raw - gyro_bias_x;
+    
+    // 3. СОХРАНЯЕМ это в raw_gx (это будет красная линия в Python)
+    raw_gx = (int16_t)x; 
 
-    // ВМЕСТО ЭТОГО:
-    // x = biquad_apply(&notch1, x);
-    // x = biquad_apply(&notch2, x);
-    // x = biquad_apply(&notch3, x);
+    // 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);
 
-    // ТЕПЕРЬ:
-    x = FMAC_Process_Sample(x);
-
-    filt_gx = x;
+    // 5. Сохраняем в filt_gx (это будет синяя линия)
+    filt_gx = x_filtered;
 }
 
 void Update_FMAC_Coeffs(int stage, float b0, float b1, float b2, float a1, float a2) {
     if (stage < 0 || stage > 2) return;
-    const float scale = 16384.0f; // Q14
     
-    notch_fmac_coeffs[stage].b0 = (int16_t)(b0 * scale);
-    notch_fmac_coeffs[stage].b1 = (int16_t)(b1 * scale);
-    notch_fmac_coeffs[stage].b2 = (int16_t)(b2 * scale);
-    // Для FMAC знаки a1 и a2 инвертируем!
-    notch_fmac_coeffs[stage].a1 = (int16_t)(-a1 * scale);
-    notch_fmac_coeffs[stage].a2 = (int16_t)(-a2 * scale);
+    // Обновляем софтверные фильтры вместо FMAC
+    dyn_notch_filters[stage].b0 = b0;
+    dyn_notch_filters[stage].b1 = b1;
+    dyn_notch_filters[stage].b2 = b2;
+    dyn_notch_filters[stage].a1 = a1;
+    dyn_notch_filters[stage].a2 = a2;
 }
 
 // Внутренняя функция для обработки одного каскада через FMAC
@@ -178,7 +213,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);
     
     if (timeout == 0) return input;