107 lines
2.8 KiB
C++
107 lines
2.8 KiB
C++
#include "i2c.h"
|
|
|
|
#include "mag.h"
|
|
|
|
static const unsigned char MAG_Addr = 0x0E; // IST8310
|
|
|
|
static void (*MAG_DoneProc)(MAG_Data& Data);
|
|
|
|
#pragma pack(push,1)
|
|
struct MAG_DataBuffer {char s1; short mx; short my; short mz;};
|
|
#pragma pack(pop)
|
|
|
|
static MAG_Data MagResult;
|
|
|
|
static bool Wait=false;
|
|
|
|
static void MAG_CallbackStart(unsigned char Address, const unsigned char* Data, unsigned char Size)
|
|
{
|
|
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
static void MAG_CallbackProc(unsigned char Address, const unsigned char* Data, unsigned char Size)
|
|
{
|
|
MAG_DataBuffer* data=(MAG_DataBuffer*)Data;
|
|
|
|
if(data->s1 & 1)
|
|
{
|
|
MagResult.X = data->mx;
|
|
MagResult.Y = data->my;
|
|
MagResult.Z = data->mz;
|
|
Wait=false;
|
|
}
|
|
|
|
MAG_DoneProc(MagResult);
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
static unsigned char MAG_BufferStart[4], MAG_BufferDevice[8];
|
|
static I2C_Request MAG_Start = {&MAG_CallbackStart, MAG_BufferStart, sizeof(MAG_BufferStart), 0};
|
|
static I2C_Request MAG_Request = {&MAG_CallbackProc, MAG_BufferDevice, sizeof(MAG_BufferDevice), 0};
|
|
|
|
static inline void SetReg(unsigned char Reg, unsigned char Value)
|
|
{
|
|
unsigned char reg[2] = {Reg, Value}; // I2C_SLV4_ADDR - MAG_ADDR, Reg, I2C_SLVx_EN, Value
|
|
I2C2_Write(MAG_Addr, reg, 2);
|
|
I2C2_Stop();
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
void MAG_Init() // IST8310
|
|
{
|
|
I2C2_Init();
|
|
I2C2_Init();
|
|
|
|
for(int a=0; a<100000; a++) { asm volatile("NOP"); } // Подождем готовности устройства к работе
|
|
|
|
// Reset
|
|
SetReg(11U, 1U); // CNTL2 <- SRST
|
|
|
|
for(int a=0; a<100000; a++) { asm volatile("NOP"); }
|
|
|
|
// Config
|
|
SetReg(11U, 8U); // CNTL2 <- DREN
|
|
SetReg(65U, 8U | 1U); // AVGCNTL <- AVG_Y_AvgBy2 | AVG_XZ_AvgBy2
|
|
SetReg(66U, 192U); // PDCNTL <- PULSE_DUR_Normal
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
bool MAG_Get(MAG_Data& Data)
|
|
{
|
|
if(!Wait) SetReg(10U, 1U); // CNTL1 <- MODE_SingleMeasurement
|
|
Wait=true;
|
|
|
|
MAG_DataBuffer data;
|
|
|
|
I2C2_Write(MAG_Addr, 2U);
|
|
I2C2_Read(MAG_Addr, &data, sizeof(MAG_DataBuffer));
|
|
I2C2_Stop();
|
|
|
|
if(!(data.s1 & 1)) return false;
|
|
|
|
Data.X = data.mx;
|
|
Data.Y = data.my;
|
|
Data.Z = data.mz;
|
|
|
|
Wait=false;
|
|
|
|
return true;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|
|
void MAG_GetAsunc(void (*DoneProc)(MAG_Data& Data))
|
|
{
|
|
if(!Wait)
|
|
{
|
|
unsigned char reg[]={10U, 1U};
|
|
I2C2_Trans(&MAG_Start, MAG_Addr, reg, 2, 0);
|
|
Wait=true;
|
|
}
|
|
|
|
MAG_DoneProc=DoneProc;
|
|
unsigned char reg=2U;
|
|
I2C2_Trans(&MAG_Request, MAG_Addr, ®, 1, sizeof(MAG_DataBuffer));
|
|
}
|
|
//------------------------------------------------------------------------------
|