From 52922afeb13c98fdd53acf33061c806e1b3b61a9 Mon Sep 17 00:00:00 2001 From: Radzhab Bisultanov Date: Tue, 14 Apr 2026 17:36:05 +0300 Subject: [PATCH] =?UTF-8?q?=D0=A0=D0=B5=D0=B0=D0=BB=D0=B8=D0=B7=D0=BE?= =?UTF-8?q?=D0=B2=D0=B0=D0=BD=20=D1=83=D1=80=D0=BE=D0=B2=D0=B5=D0=BD=D1=8C?= =?UTF-8?q?=20=D0=BE=D0=B1=D1=80=D0=B0=D0=B1=D0=BE=D1=82=D0=BA=D0=B8=20?= =?UTF-8?q?=D0=B4=D0=B0=D0=BD=D0=BD=D1=8B=D1=85=20=D0=B4=D1=80=D0=B0=D0=B9?= =?UTF-8?q?=D0=B2=D0=B5=D1=80=D0=B0=20IMU?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Source/Devices/ICM20948.cpp | 164 ++++++++++++++++++++++++++++++++++++ Source/Devices/ICM20948.h | 53 ++++++++++++ Source/Devices/IIMU.cpp | 41 +++++++++ Source/Devices/IIMU.h | 57 +++++++++++++ Source/MathEnv/Biquad.cpp | 75 +++++++++++++++++ Source/MathEnv/Biquad.h | 21 +++++ Source/MathEnv/MathFunc.cpp | 14 +++ Source/MathEnv/MathFunc.h | 10 +++ drone.ewp | 16 ++++ drone.ewt | 15 ++++ 10 files changed, 466 insertions(+) create mode 100644 Source/MathEnv/Biquad.cpp create mode 100644 Source/MathEnv/Biquad.h create mode 100644 Source/MathEnv/MathFunc.cpp create mode 100644 Source/MathEnv/MathFunc.h diff --git a/Source/Devices/ICM20948.cpp b/Source/Devices/ICM20948.cpp index 0c1b1da..56cdd6a 100644 --- a/Source/Devices/ICM20948.cpp +++ b/Source/Devices/ICM20948.cpp @@ -1 +1,165 @@ #include "ICM20948.h" +#include "i2C.h" +#include "MathFunc.h" +#include "Biquad.h" +#include + +static BiquadFilter FiltAcc[3]; +static BiquadFilter FiltGyr[3]; + +static IMU_Proc IMU_DoneProc; + +static long GyroShift[3]{0, 0, 0}; + +struct IMU_DataBuffer +{ + short ax, ay, az, gx, gy, gz, temp, mx, my, mz; +}; + +static inline void IMU_Calibrate(IMU_DataBuffer& imu, long GyrLim, long AccZero, long AccMax); +static void IMU_CallbackProc(unsigned char Address, const unsigned char* Data, unsigned char Size); + +static unsigned char IMU_Buffer[32]; +static I2C_Request IMU_Device = {&IMU_CallbackProc, IMU_Buffer, sizeof(IMU_Buffer), 0 }; + +void ICM20948::Init() +{ + lpf2p_init(&FiltAcc[0], 500.0f, 25.0f); + lpf2p_init(&FiltAcc[1], 500.0f, 25.0f); + lpf2p_init(&FiltAcc[2], 500.0f, 25.0f); + + lpf2p_init(&FiltGyr[0], 500.0f, 25.0f); + lpf2p_init(&FiltGyr[1], 500.0f, 25.0f); + lpf2p_init(&FiltGyr[2], 500.0f, 25.0f); + + for (int a = 0; a < 100000; a++) { asm volatile("NOP"); } + + IMU_SetReg(IMU_Addr, 0x06, 0x80); + for (int a = 0; a < 100000; a++) { asm volatile("NOP"); } + + IMU_SetReg(IMU_Addr, 0x06, 0x01); + for (int a = 0; a < 100000; a++) { asm volatile("NOP"); } + + IMU_SetBank(2); + + IMU_SetReg(IMU_Addr, 0x01, GYRO_DLPFCFG_73 | GYRO_FS_SEL_2000 | GYRO_FCHOICE_ON); // GYRO_CONFIG_1 + IMU_SetReg(IMU_Addr, 0x14, ACCEL_DLPFCFG_69 | ACCEL_FS_SEL_8 | ACCEL_FCHOICE_ON); // ACCEL_CONFIG + + IMU_SetBank(0); + + for (int a = 0; a < 100000; a++) { asm volatile("NOP"); } +} + +void ICM20948::IMU_SetBank(unsigned char Bank) +{ + unsigned char reg[2]; + reg[0] = 0x7F; reg[1] = Bank << 4; + I2C1_Write(IMU_Addr, reg, 2); + I2C1_Stop(); +} + +void ICM20948::Get(IMU_Data& Data) +{ + struct { short ax; short ay; short az; short gx; short gy; short gz; short temp; short mx; short my; short mz; } data; + + I2C1_Write(IMU_Addr, IMU_Read_Reg); + I2C1_Read(IMU_Addr, &data, sizeof(data)); + I2C1_Stop(); + + Data.Temp = 21 + (((long)Rev16(data.temp)) * 128) / 42735; + + Data.Acc.X = Rev16(data.ax); + Data.Acc.Y = Rev16(data.ay); + Data.Acc.Z = Rev16(data.az); + + Data.Gyr.X = Rev16(data.gx); + Data.Gyr.Y = Rev16(data.gy); + Data.Gyr.Z = Rev16(data.gz); + + Data.Mag.X = data.mx; + Data.Mag.Y = data.my; + Data.Mag.Z = data.mz; +} + +void ICM20948::GetAsync(const IMU_Proc& DoneProc) +{ + IMU_DoneProc = DoneProc; + I2C1_Trans(&IMU_Device, IMU_Addr, &IMU_Read_Reg, 1, sizeof(IMU_DataBuffer)); +} + + +static void IMU_CallbackProc(unsigned char Address, const unsigned char* Data, unsigned char Size) +{ + IMU_DataBuffer& data = *(IMU_DataBuffer*)Data; + + data.ax = Rev16(data.ax); + data.ay = Rev16(data.ay); + data.az = Rev16(data.az); + + data.gx = Rev16(data.gx); + data.gy = Rev16(data.gy); + data.gz = Rev16(data.gz); + + data.ax = biquad_apply(&FiltAcc[0], data.ax); + data.ay = biquad_apply(&FiltAcc[1], data.ay); + data.az = biquad_apply(&FiltAcc[2], data.az); + + data.gx = biquad_apply(&FiltGyr[0], data.gx); + data.gy = biquad_apply(&FiltGyr[1], data.gy); + data.gz = biquad_apply(&FiltGyr[2], data.gz); + + static IMU_DataBuffer test = *(IMU_DataBuffer*)Data; + + if (CalibDataIMU.AllowedCalib) IMU_Calibrate(data, 50, 4096, 500); + + static IMU_Data result; + + static float alpha = 0.01f; + + result.RawAcc.X = (result.RawAcc.X * (1.0f - alpha)) + test.ax * alpha; + result.RawAcc.Y = (result.RawAcc.Y * (1.0f - alpha)) + test.ay * alpha; + result.RawAcc.Z = (result.RawAcc.Z * (1.0f - alpha)) + test.az * alpha; + + result.RawGyr.X = (result.RawGyr.X * (1.0f - alpha)) + test.gx * alpha; + result.RawGyr.Y = (result.RawGyr.Y * (1.0f - alpha)) + test.gy * alpha; + result.RawGyr.Z = (result.RawGyr.Z * (1.0f - alpha)) + test.gz * alpha; + + result.Temp = 21 + (((long)Rev16(data.temp)) * 128) / 42735; // 21+(temperature / 333.87) + + result.Acc.X = Normalize(data.ax, 1.0f, CalibDataIMU.Data.Acc.X[0], CalibDataIMU.Data.Acc.X[1]); + result.Acc.Y = Normalize(data.ay, 1.0f, CalibDataIMU.Data.Acc.Y[0], CalibDataIMU.Data.Acc.Y[1]); + result.Acc.Z = Normalize(data.az, 1.0f, CalibDataIMU.Data.Acc.Z[0], CalibDataIMU.Data.Acc.Z[1]); + + result.Gyr.X = ((float)(data.gx - GyroShift[0])) / 16.384f; + result.Gyr.Y = ((float)(data.gy - GyroShift[1])) / 16.384f; + result.Gyr.Z = ((float)(data.gz - GyroShift[2])) / 16.384f; + + IMU_DoneProc(result); +} + +static inline void IMU_Calibrate(IMU_DataBuffer& imu, long GyrLim, long AccZero, long AccMax) +{ + long len = imu.gx * imu.gx + imu.gy * imu.gy + imu.gz * imu.gz; + if (len > GyrLim * GyrLim) return; + + static float x = imu.gx, y = imu.gy, z = imu.gz; + + const float alpha = 0.001f; + + x = x * (1.0f - alpha) + alpha * imu.gx; + y = y * (1.0f - alpha) + alpha * imu.gy; + z = z * (1.0f - alpha) + alpha * imu.gz; + + GyroShift[0] = x; + GyroShift[1] = y; + GyroShift[2] = z; + + len = imu.ax * imu.ax + imu.ay * imu.ay + imu.az * imu.az; + if (abs(len - AccZero * AccZero) > (AccMax * AccMax)) return; + + len = sqrtf(len); + + static float acc = len; + + acc = acc * (1.0f - alpha) + alpha * len; +} diff --git a/Source/Devices/ICM20948.h b/Source/Devices/ICM20948.h index 72c94de..82a1063 100644 --- a/Source/Devices/ICM20948.h +++ b/Source/Devices/ICM20948.h @@ -3,6 +3,59 @@ #ifndef ICM20948_H #define ICM20948_H +#include "IIMU.h" +#define ACCEL_FCHOICE_OFF 0 +#define ACCEL_FCHOICE_ON 1 + +#define ACCEL_FS_SEL_2 0<<1 +#define ACCEL_FS_SEL_4 1<<1 +#define ACCEL_FS_SEL_8 2<<1 +#define ACCEL_FS_SEL_16 3<<1 + +#define ACCEL_DLPFCFG_265 0<<3 +#define ACCEL_DLPFCFG_136 2<<3 +#define ACCEL_DLPFCFG_69 3<<3 +#define ACCEL_DLPFCFG_34 4<<3 +#define ACCEL_DLPFCFG_17 5<<3 +#define ACCEL_DLPFCFG_8 6<<3 +#define ACCEL_DLPFCFG_500 7<<3 + +#define GYRO_FCHOICE_OFF 0 +#define GYRO_FCHOICE_ON 1 + +#define GYRO_FS_SEL_250 0<<1 +#define GYRO_FS_SEL_500 1<<1 +#define GYRO_FS_SEL_1000 2<<1 +#define GYRO_FS_SEL_2000 3<<1 + +#define GYRO_DLPFCFG_230 0<<3 +#define GYRO_DLPFCFG_188 1<<3 +#define GYRO_DLPFCFG_154 2<<3 +#define GYRO_DLPFCFG_73 3<<3 +#define GYRO_DLPFCFG_36 4<<3 +#define GYRO_DLPFCFG_18 5<<3 +#define GYRO_DLPFCFG_9 6<<3 +#define GYRO_DLPFCFG_377 7<<3 + +#define MAG_DATARATE_10_HZ 0x02 +#define MAG_DATARATE_20_HZ 0x04 +#define MAG_DATARATE_50_HZ 0x06 +#define MAG_DATARATE_100_HZ 0x08 + +class ICM20948 : IIMU +{ +public: + ICM20948(){}; + ~ICM20948(){}; + + unsigned char IMU_Addr = 0x68; + unsigned char IMU_Read_Reg = 0x2D; + + void Init(); + void IMU_SetBank(unsigned char Bank); + void Get(IMU_Data& Data); + void GetAsync(const IMU_Proc& DoneProc); +}; #endif \ No newline at end of file diff --git a/Source/Devices/IIMU.cpp b/Source/Devices/IIMU.cpp index 8ef2cf3..52b3ddd 100644 --- a/Source/Devices/IIMU.cpp +++ b/Source/Devices/IIMU.cpp @@ -1,2 +1,43 @@ #include "IIMU.h" +#include "I2C.h" +#include "ICM20948.h" + +IMU_Info IMUInfo; + +IMU_Calib_Data CalibDataIMU; + +void IMU_SetReg(unsigned char Addr, unsigned char Reg, unsigned char Value) +{ + unsigned char reg[2]; + reg[0] = Reg; reg[1] = Value; + I2C1_Write(Addr, reg, 2); + I2C1_Stop(); +} + +unsigned char IMU_GetReg(unsigned char Addr, unsigned char Reg) +{ + I2C1_Write(Addr, Reg); + I2C1_Read(Addr, &Reg, 1); + I2C1_Stop(); + return Reg; +} + +IIMU* TryFindIMU(bool& find) +{ + I2C1_Init(); + IIMU* Imu = nullptr; + for (int i = 0; i < sizeof(IMUInfo.Addrs); ++i) + { + find = I2C1_CheckDeviceWhoAmI(IMUInfo.Addrs[i], IMUInfo.WhoIAMReg[i], IMUInfo.ExpectedIDs[i]); + if (find && i == 0) + { + static ICM20948 imuICM20948; + Imu = (IIMU*)&imuICM20948; + break; + } + for (int a = 0; a < 10000000; ++a) { asm volatile("NOP"); } + } + if (find) Imu->Init(); + return Imu; +} diff --git a/Source/Devices/IIMU.h b/Source/Devices/IIMU.h index b5ea7db..bc714da 100644 --- a/Source/Devices/IIMU.h +++ b/Source/Devices/IIMU.h @@ -3,7 +3,64 @@ #ifndef IIMU_H #define IIMU_H +#pragma pack(push, 1) +struct IMU_Data +{ + long Temp; + struct { float X, Y, Z; } Acc; + struct { float X, Y, Z; } RawAcc; + struct { float X, Y, Z; } Gyr; + struct { float X, Y, Z; } RawGyr; + struct { float X, Y, Z; } Mag; + struct { float X, Y, Z; } RawMag; +}; +struct XYZ_Calib +{ + short X[2] = {-4096, 4096}; + short Y[2] = {-4096, 4096}; + short Z[2] = {-4096, 4096}; +}; +struct XYZ_IMU_DATA +{ + char Writed = sizeof(XYZ_IMU_DATA); + XYZ_Calib Acc; +}; + +struct IMU_Calib_Data +{ + bool AllowedCalib = false; + bool AccCalibNeed = true; + XYZ_IMU_DATA Data; +}; + +struct IMU_Info +{ + unsigned char Addrs[1] = {0x68}; + unsigned char WhoIAMReg[1] = {0x00}; + unsigned char ExpectedIDs[1] = {0xEA}; +}; +#pragma pack(pop) + +typedef void (*IMU_Proc)(IMU_Data&); + +extern IMU_Calib_Data CalibDataIMU; + +void IMU_SetReg(unsigned char Addr, unsigned char Reg, unsigned char Value); + +unsigned char IMU_GetReg(unsigned char Addr, unsigned char Reg); + +class IIMU +{ +protected: + virtual ~IIMU() = default; +public: + virtual void Init() = 0; + virtual void Get(IMU_Data& Data) = 0; + virtual void GetAsync(const IMU_Proc& DoneProc) = 0; +}; + +IIMU* TryFindIMU(bool& find); #endif \ No newline at end of file diff --git a/Source/MathEnv/Biquad.cpp b/Source/MathEnv/Biquad.cpp new file mode 100644 index 0000000..dd6747a --- /dev/null +++ b/Source/MathEnv/Biquad.cpp @@ -0,0 +1,75 @@ +#include "Biquad.h" +#include + +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; +} diff --git a/Source/MathEnv/Biquad.h b/Source/MathEnv/Biquad.h new file mode 100644 index 0000000..2cc8b97 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Source/MathEnv/MathFunc.cpp b/Source/MathEnv/MathFunc.cpp new file mode 100644 index 0000000..be3b330 --- /dev/null +++ b/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; +} \ No newline at end of file diff --git a/Source/MathEnv/MathFunc.h b/Source/MathEnv/MathFunc.h new file mode 100644 index 0000000..6046ccb --- /dev/null +++ b/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 \ No newline at end of file diff --git a/drone.ewp b/drone.ewp index 2d2fca6..c8b1597 100644 --- a/drone.ewp +++ b/drone.ewp @@ -362,6 +362,7 @@ $PROJ_DIR$\Source\Control\Inc $PROJ_DIR$\Source\INS\geometry $PROJ_DIR$\Source\INS + $PROJ_DIR$\Source\MathEnv