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 "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
#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

41
Source/Devices/IIMU.cpp
View File

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

57
Source/Devices/IIMU.h
View File

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