bisultanov/RaDrone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Раджаб Бисултанов
2026-04-14 17:36:05 +03:00
parent d59cf7cd55
commit 52922afeb1
10 changed files with 466 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

164
Source/Devices/ICM20948.cpp
View File

@@ -1 +1,165 @@
#include "ICM20948.h" #include "ICM20948.h"
#include "i2C.h"
#include "MathFunc.h"
#include "Biquad.h"
#include <cmath>
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;
}

53
Source/Devices/ICM20948.h
View File

@@ -3,6 +3,59 @@
#ifndef ICM20948_H #ifndef ICM20948_H
#define 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 #endif

41
Source/Devices/IIMU.cpp
View File

@@ -1,2 +1,43 @@
#include "IIMU.h" #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;
}

57
Source/Devices/IIMU.h
View File

@@ -3,7 +3,64 @@
#ifndef IIMU_H #ifndef IIMU_H
#define 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 #endif

75
Source/MathEnv/Biquad.cpp Normal file
View File

@@ -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;
}

21
Source/MathEnv/Biquad.h Normal file
View File

@@ -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

14
Source/MathEnv/MathFunc.cpp Normal file
View File

@@ -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;
}

10
Source/MathEnv/MathFunc.h Normal file
View File

@@ -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

16
drone.ewp
View File

@@ -362,6 +362,7 @@
<state>$PROJ_DIR$\Source\Control\Inc</state> <state>$PROJ_DIR$\Source\Control\Inc</state>
<state>$PROJ_DIR$\Source\INS\geometry</state> <state>$PROJ_DIR$\Source\INS\geometry</state>
<state>$PROJ_DIR$\Source\INS</state> <state>$PROJ_DIR$\Source\INS</state>
<state>$PROJ_DIR$\Source\MathEnv</state>
</option> </option>
<option> <option>
<name>CCStdIncCheck</name> <name>CCStdIncCheck</name>
@@ -2373,6 +2374,21 @@
<name>$PROJ_DIR$\Source\INS\IRS.h</name> <name>$PROJ_DIR$\Source\INS\IRS.h</name>
</file> </file>
</group> </group>
<group>
<name>MathEnv</name>
<file>
<name>$PROJ_DIR$\Source\MathEnv\Biquad.cpp</name>
</file>
<file>
<name>$PROJ_DIR$\Source\MathEnv\Biquad.h</name>
</file>
<file>
<name>$PROJ_DIR$\Source\MathEnv\MathFunc.cpp</name>
</file>
<file>
<name>$PROJ_DIR$\Source\MathEnv\MathFunc.h</name>
</file>
</group>
<file> <file>
<name>$PROJ_DIR$\Source\main.c</name> <name>$PROJ_DIR$\Source\main.c</name>
</file> </file>

15
drone.ewt
View File

@@ -3558,6 +3558,21 @@
<name>$PROJ_DIR$\Source\INS\IRS.h</name> <name>$PROJ_DIR$\Source\INS\IRS.h</name>
</file> </file>
</group> </group>
<group>
<name>MathEnv</name>
<file>
<name>$PROJ_DIR$\Source\MathEnv\Biquad.cpp</name>
</file>
<file>
<name>$PROJ_DIR$\Source\MathEnv\Biquad.h</name>
</file>
<file>
<name>$PROJ_DIR$\Source\MathEnv\MathFunc.cpp</name>
</file>
<file>
<name>$PROJ_DIR$\Source\MathEnv\MathFunc.h</name>
</file>
</group>
<file> <file>
<name>$PROJ_DIR$\Source\main.c</name> <name>$PROJ_DIR$\Source\main.c</name>
</file> </file>