last_fft

imu.c

@@ -17,34 +17,34 @@ float biquad_apply(biquad_t *f, float x) {
     return out;
 }
 
-static float fast_sin_approx(float x) {
-    float x2 = x * x;
-    return x * (1.0f - (x2 * 0.16666667f) + (x2 * x2 * 0.0083333338f));
-}
-
-static float fast_cos_approx(float x) {
-    float x2 = x * x;
-    return 1.0f - (x2 * 0.5f) + (x2 * x2 * 0.041666668f) - (x2 * x2 * x2 * 0.0013888889f);
-}
-
 void biquad_init_notch(biquad_t *f, float center_freq, float Q, float fs) {
-    if (center_freq < 1.0f) {
+    // Защита: при слишком низкой/высокой частоте уводим фильтр в bypass.
+    if (center_freq < 1.0f || center_freq > (0.45f * fs) || Q <= 0.05f) {
         f->b0 = 1.0f; f->b1 = 0; f->b2 = 0;
         f->a1 = 0; f->a2 = 0;
+        f->d1 = 0; f->d2 = 0;
         return;
     }
-    
+
     float w0 = 2.0f * 3.14159265f * center_freq / fs;
-    float alpha = fast_sin_approx(w0) / (2.0f * Q);
-    float cosw0 = fast_cos_approx(w0);
-    
+    float alpha = sinf(w0) / (2.0f * Q);
+    float cosw0 = cosf(w0);
+
     float inv_a0 = 1.0f / (1.0f + alpha); // Считаем один раз инверсию
-    
+
     f->b0 = inv_a0;
     f->b1 = -2.0f * cosw0 * inv_a0;
     f->b2 = inv_a0;
     f->a1 = -2.0f * cosw0 * inv_a0;
     f->a2 = (1.0f - alpha) * inv_a0;
+
+    // Защита от NaN/Inf, чтобы не обнулялся выход при некорректной перенастройке.
+    if (!isfinite(f->b0) || !isfinite(f->b1) || !isfinite(f->b2) ||
+        !isfinite(f->a1) || !isfinite(f->a2)) {
+        f->b0 = 1.0f; f->b1 = 0; f->b2 = 0;
+        f->a1 = 0; f->a2 = 0;
+        f->d1 = 0; f->d2 = 0;
+    }
 }
 
 void I2C1_Init(void) {