68 lines
1.7 KiB
C++
68 lines
1.7 KiB
C++
#include "i2c.h"
|
|
|
|
#include "imu.h"
|
|
|
|
static const unsigned char IMU_Addr = 0x6A; // ACC GYR
|
|
|
|
static inline short Rev16(short v)
|
|
{
|
|
asm("REV16 %1, %0" : "=r" (v) : "r" (v)); // v = v<<8 | v>>8;
|
|
return v;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
static inline void IMU_SetReg(unsigned char Reg, unsigned char Value)
|
|
{
|
|
unsigned char reg[2];
|
|
reg[0]=Reg; reg[1]=Value;
|
|
I2C1_Write(IMU_Addr, reg, 2);
|
|
I2C1_Stop();
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
static inline unsigned char IMU_GetReg(unsigned char Reg)
|
|
{
|
|
I2C1_Write(IMU_Addr, Reg);
|
|
I2C1_Read(IMU_Addr, &Reg, 1);
|
|
I2C1_Stop();
|
|
return Reg;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
void IMU_Init() //
|
|
{
|
|
I2C1_Init();
|
|
|
|
for(int a=0; a<100000; a++) { asm volatile("NOP"); }
|
|
|
|
unsigned char wai=IMU_GetReg(0x0F);
|
|
|
|
IMU_SetReg(0x10, 0x38);
|
|
IMU_SetReg(0x11, 0x48);
|
|
IMU_SetReg(0x12, 0x06);
|
|
|
|
for(int a=0; a<100000; a++) { asm volatile("NOP"); }
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
void IMU_Get(IMU_Data& Data)
|
|
{
|
|
struct {short temp; short gx; short gy; short gz; short ax; short ay; short az; } data;
|
|
|
|
I2C1_Write(IMU_Addr, 0x20);
|
|
I2C1_Read(IMU_Addr, &data, sizeof(data));
|
|
I2C1_Stop();
|
|
|
|
Data.Temp = 21+(((long)Rev16(data.temp))*128)/42735; // 21+(temperature / 333.87)
|
|
|
|
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);
|
|
|
|
}
|
|
//------------------------------------------------------------------------------
|