55 lines
1.2 KiB
C++
55 lines
1.2 KiB
C++
#include "IIMU.h"
|
|
#include "stm32g4xx.h"
|
|
#include "I2C.h"
|
|
#include "ICM20948.h"
|
|
#include "GPIO.h"
|
|
|
|
|
|
IMU_Info IMUInfo;
|
|
|
|
IMU_Calib_Data CalibDataIMU;
|
|
|
|
void IMU_InitPower()
|
|
{
|
|
GPIO_InitPin(GPIO_PIN_13 | GPIO_PORT_C | GPIO_OUTPUT | GPIO_SET); // POWER ON 3V3 (nPC13)
|
|
for (volatile int i = 0; i < 1000000; i ++) {};
|
|
GPIOC->BSRR = GPIO_BSRR_BR13;
|
|
for (volatile int i = 0; i < 1000000; i ++) {};
|
|
}
|
|
|
|
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;
|
|
}
|
|
|