166 lines
4.9 KiB
C++
166 lines
4.9 KiB
C++
#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;
|
|
}
|