Refactor code structure for improved readability and maintainability

imu.c

@@ -1,4 +1,5 @@
 #include "imu.h"
+#include "arm_math.h"
 #include <math.h>
 
 volatile int16_t raw_ax, raw_ay, raw_az;
@@ -16,17 +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) {
+        f->b0 = 1.0f; f->b1 = 0; f->b2 = 0;
+        f->a1 = 0; f->a2 = 0;
+        return;
+    }
+    
     float w0 = 2.0f * 3.14159265f * center_freq / fs;
-    float alpha = sinf(w0) / (2.0f * Q);
-    float cosw0 = cosf(w0);
-    float a0 = 1.0f + alpha;
-    f->b0 = 1.0f / a0;
-    f->b1 = -2.0f * cosw0 / a0;
-    f->b2 = 1.0f / a0;
-    f->a1 = -2.0f * cosw0 / a0;
-    f->a2 = (1.0f - alpha) / a0;
-    f->d1 = 0; f->d2 = 0;
+    float alpha = fast_sin_approx(w0) / (2.0f * Q);
+    float cosw0 = fast_cos_approx(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;
 }
 
 void I2C1_Init(void) {
@@ -77,9 +95,9 @@ void IMU_Init(void) {
     IMU_SetBank(0);
 
     // Начальная инициализация ( на 0 Гц dsp_manager сам их включит)
-    biquad_init_notch(&notch1, 0, 1.0f, 1000.0f);
-    biquad_init_notch(&notch2, 0, 1.0f, 1000.0f);
-    biquad_init_notch(&notch3, 0, 1.0f, 1000.0f);
+    biquad_init_notch(&notch1, 0, 1.0f, 1500.0f);
+    biquad_init_notch(&notch2, 0, 1.0f, 1500.0f);
+    biquad_init_notch(&notch3, 0, 1.0f, 1500.0f);
 }
 
 void IMU_Calibrate(void) {