Реализован уровень обработки данных драйвера IMU

Source/MathEnv/Biquad.cpp

@@ -0,0 +1,75 @@
+#include "Biquad.h"
+#include <math.h>
+
+static constexpr float PI = 3.14159265359f;
+
+void lpf2p_init(BiquadFilter *filter, float sample_freq, float cutoff_freq)
+{
+  // Некорректные параметры, фильтр не будет работать
+  if (cutoff_freq <= 0.0f || sample_freq <= 0.0f) return;
+  
+  float fr = sample_freq / cutoff_freq;
+  float ohm = tanf(PI / fr);
+  float c = 1.0f + 2.0f * cosf(PI / 4.0f) * ohm + ohm * ohm;
+  
+  filter->b0 = ohm * ohm / c;
+  filter->b1 = 2.0f * filter->b0;
+  filter->b2 = filter->b0;
+
+  filter->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
+  filter->a2 = (1.0f - 2.0f * cosf(PI / 4.0f) * ohm + ohm * ohm) / c;
+
+  // Сбрасываем историю
+  filter->x1 = filter->x2 = 0.0f;
+  filter->y1 = filter->y2 = 0.0f;
+}
+
+void notch_init(BiquadFilter *filter, float sample_freq, float center_freq, float bandwidth)
+{
+  // Некорректные параметры
+  if (center_freq <= 0.0f || sample_freq <= 0.0f || bandwidth <= 0.0f) return;
+  
+  // Промежуточные расчеты по стандартным формулам для biquad-фильтра
+  float w0 = 2.0f * PI * center_freq / sample_freq;
+  float alpha = sinf(w0) * sinhf(logf(2.0f) / 2.0f * bandwidth * w0 / sinf(w0));
+  
+  // Рассчитываем коэффициенты b (для входа x) и a (для выхода y)
+  float b0_temp = 1.0f;
+  float b1_temp = -2.0f * cosf(w0);
+  float b2_temp = 1.0f;
+  
+  float a0_temp = 1.0f + alpha;
+  float a1_temp = -2.0f * cosf(w0);
+  float a2_temp = 1.0f - alpha;
+  
+  // Нормализуем коэффициенты (делим на a0), чтобы использовать в уравнении
+  filter->b0 = b0_temp / a0_temp;
+  filter->b1 = b1_temp / a0_temp;
+  filter->b2 = b2_temp / a0_temp;
+  
+  filter->a1 = a1_temp / a0_temp;
+  filter->a2 = a2_temp / a0_temp;
+  
+  // Сбрасываем историю
+  filter->x1 = filter->x2 = 0.0f;
+  filter->y1 = filter->y2 = 0.0f;
+}
+
+float biquad_apply(BiquadFilter *filter, float input)
+{
+  // Применяем разностное уравнение: y[n] = b0*x[n] + b1*x[n-1] + b2*x[n-2] - a1*y[n-1] - a2*y[n-2]
+  float output = filter->b0 * input +
+                filter->b1 * filter->x1 +
+                filter->b2 * filter->x2 -
+                filter->a1 * filter->y1 -
+                filter->a2 * filter->y2;
+  
+  // Обновляем историю для следующего шага
+  filter->x2 = filter->x1;
+  filter->x1 = input;
+  
+  filter->y2 = filter->y1;
+  filter->y1 = output;
+  
+  return output;
+}

Source/MathEnv/Biquad.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#ifndef BIQUAD_H
+#define BIQUAD_H
+
+typedef struct
+{
+  // Коэффициенты фильтра
+  float a1, a2;
+  float b0, b1, b2;
+  
+  float x1, x2; // Предыдущие входы
+  float y1, y2; // Предыдущие выходы
+} BiquadFilter;
+
+void lpf2p_init(BiquadFilter *filter, float sample_freq, float cutoff_freq);
+void notch_init(BiquadFilter *filter, float sample_freq, float center_freq, float bandwidth);
+float biquad_apply(BiquadFilter* filter, float input);
+
+
+#endif

Source/MathEnv/MathFunc.cpp

@@ -0,0 +1,14 @@
+#include "MathFunc.h"
+
+float Normalize(float Value, float Scale, float Min, float Max)
+{
+  const float len = (Max - Min) / 2.0f;
+  const float shift = (Max + Min) / 2.0f;
+  return (Value - shift) * Scale / len;
+}
+
+short Rev16(short v)
+{
+  asm("REV16 %1, %0" : "=r" (v) : "r" (v)); // v = v<<8 | v>>8;
+  return v;
+}

Source/MathEnv/MathFunc.h

@@ -0,0 +1,10 @@
+#pragma once
+
+#ifndef MATHFUNC_H
+#define MATHFUNC_H
+
+float Normalize(float Value, float Scale, float Min, float Max);
+short Rev16(short v);
+
+
+#endif