319 lines
10 KiB
C
319 lines
10 KiB
C
#include "LSM303.h"
|
||
#include <math.h>
|
||
|
||
#define PI 3.1415926
|
||
#define Delayms(X) HAL_Delay(X)
|
||
|
||
#define LSM303_CLK_ENABLE __HAL_RCC_GPIOB_CLK_ENABLE()
|
||
iic_bus_t LSM303_bus =
|
||
{
|
||
.IIC_SDA_PORT = GPIOB,
|
||
.IIC_SCL_PORT = GPIOB,
|
||
.IIC_SDA_PIN = GPIO_PIN_13,
|
||
.IIC_SCL_PIN = GPIO_PIN_14,
|
||
};
|
||
|
||
/************************************************************************************************************************
|
||
NOTICE: the LSM303 can not use mutiple read!
|
||
|
||
*************************************************************************************************************************/
|
||
|
||
|
||
/*************************************************************************************************************************
|
||
*function : LSM303_ReadOneReg()
|
||
*paraments : RegAddr
|
||
*return : data readed
|
||
*detail :
|
||
*************************************************************************************************************************/
|
||
unsigned char LSM303_ReadOneReg(unsigned char RegAddr)
|
||
{
|
||
unsigned char dat;
|
||
unsigned char SlaveAddr = (RegAddr>0x19)?LSM303_SlaveAddr_A:LSM303_SlaveAddr_M;
|
||
dat = IIC_Read_One_Byte(&LSM303_bus, SlaveAddr,RegAddr);
|
||
return dat;
|
||
}
|
||
|
||
/*************************************************************************************************************************
|
||
*function : LSM303_ReadMultiReg()
|
||
*paraments : RegAddr, Regnumber, Databuff
|
||
*return : multiple datas readed
|
||
*detail : can't use real continue read, we tested the LSM303 is not allowed, so we have to use multiple ReadOneReg
|
||
*************************************************************************************************************************/
|
||
void LSM303_ReadMultiReg(unsigned char RegAddr, unsigned char RegNum, unsigned char DataBuff[])
|
||
{
|
||
unsigned char i;
|
||
unsigned char SlaveAddr = (RegAddr>0x19)?LSM303_SlaveAddr_A:LSM303_SlaveAddr_M;
|
||
//IIC_Read_Multi_Byte(SlaveAddr,RegAddr,RegNum,DataBuff);
|
||
for(i=0;i<RegNum;i++)
|
||
{
|
||
DataBuff[i] = IIC_Read_One_Byte(&LSM303_bus, SlaveAddr,RegAddr+i);
|
||
}
|
||
}
|
||
|
||
/*************************************************************************************************************************
|
||
*function : LSM303_Temp_ReadOneReg()
|
||
*paraments : RegAddr
|
||
*return : data readed
|
||
*detail : it's for temperature data read
|
||
*************************************************************************************************************************/
|
||
unsigned char LSM303_Temp_ReadOneReg(unsigned char RegAddr)
|
||
{
|
||
unsigned char dat;
|
||
dat = IIC_Read_One_Byte(&LSM303_bus, LSM303_SlaveAddr_M,RegAddr);
|
||
return dat;
|
||
}
|
||
|
||
/*************************************************************************************************************************
|
||
*function : LSM303_WriteOneReg()
|
||
*paraments : RegAddr, data
|
||
*return : None
|
||
*detail :
|
||
*************************************************************************************************************************/
|
||
void LSM303_WriteOneReg(unsigned char RegAddr, unsigned char dat)
|
||
{
|
||
unsigned char SlaveAddr = (RegAddr>0x19)?LSM303_SlaveAddr_A:LSM303_SlaveAddr_M;
|
||
IIC_Write_One_Byte(&LSM303_bus, SlaveAddr,RegAddr,dat);
|
||
}
|
||
|
||
/*************************************************************************************************************************
|
||
*function : LSM303DLH_Init()
|
||
*paraments : None
|
||
*return : succeed or not
|
||
*detail : the defalt CTRL_REG4 value is 0x00. FS[5:4], 00: ±2 g, 01: ±4 g, 10: ±8 g, 11: ±16 g
|
||
*************************************************************************************************************************/
|
||
unsigned char LSM303DLH_Init()
|
||
{
|
||
unsigned char temp;
|
||
unsigned char retry = 0;
|
||
|
||
LSM303_CLK_ENABLE;
|
||
IICInit(&LSM303_bus);
|
||
|
||
for(retry = 0;retry < 3;retry ++)
|
||
{
|
||
LSM303_WriteOneReg(LSM303_CTRL_REG4_A, 0x10); //full scale: +-4g
|
||
Delayms(1);
|
||
LSM303_WriteOneReg(LSM303_CTRL_REG1_A, 0x2F); //low power mode, 10Hz速度
|
||
Delayms(1);
|
||
temp = LSM303_ReadOneReg(LSM303_CTRL_REG1_A); //读取0x20寄存器，默认值为0x07
|
||
if(temp != 0x2F)
|
||
{
|
||
Delayms(10);
|
||
}
|
||
else break;
|
||
}
|
||
if(temp != 0x2F)
|
||
{
|
||
return 1;
|
||
}
|
||
Delayms(1);
|
||
for(retry = 0;retry < 3;retry ++)
|
||
{
|
||
LSM303_WriteOneReg(LSM303_CRA_REG_M, 0x10); //磁场传感器15Hz,正常测量;关闭温度计
|
||
Delayms(1);
|
||
LSM303_WriteOneReg(LSM303_CRB_REG_M, 0x80); //磁场传感器gain默认为0x80: +-4.0Gauss, 450LBS/Gauss for XY, 400LBS/Gauss for Z
|
||
Delayms(1);
|
||
LSM303_WriteOneReg(LSM303_MR_REG_M, 0x00); //磁场传感器连续转换模式
|
||
Delayms(1);
|
||
temp = LSM303_ReadOneReg(LSM303_MR_REG_M);
|
||
if(temp != 0)
|
||
{
|
||
Delayms(10);
|
||
}
|
||
else break;
|
||
}
|
||
if(temp != 0)
|
||
{
|
||
return 1;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
/*************************************************************************************************************************
|
||
*function : LSM303DLH_Sleep()
|
||
*paraments : None
|
||
*return : None
|
||
*detail :
|
||
*************************************************************************************************************************/
|
||
void LSM303DLH_Sleep()
|
||
{
|
||
LSM303_WriteOneReg(LSM303_MR_REG_M,0x03);
|
||
LSM303_WriteOneReg(LSM303_CTRL_REG1_A,0x0f);
|
||
}
|
||
|
||
/*************************************************************************************************************************
|
||
*function : LSM303DLH_Wakeup()
|
||
*paraments : None
|
||
*return : None
|
||
*detail :
|
||
*************************************************************************************************************************/
|
||
void LSM303DLH_Wakeup()
|
||
{
|
||
LSM303_WriteOneReg(LSM303_MR_REG_M,0x00);//磁场传感器连续转换模式
|
||
LSM303_WriteOneReg(LSM303_CTRL_REG1_A,0x2f);//low power mode, 10Hz速度
|
||
}
|
||
|
||
/*************************************************************************************************************************
|
||
*function : LSM303_ReadAcceleration()
|
||
*paraments : int16_t *Xa, int16_t *Ya, int16_t *Z
|
||
*return : None
|
||
*detail : to read the Acceleration data, but the real Acceleration should be divided by the unit(n LSB/A)
|
||
*************************************************************************************************************************/
|
||
void LSM303_ReadAcceleration(int16_t *Xa, int16_t *Ya, int16_t *Za)
|
||
{
|
||
uint8_t buff[6];
|
||
int16_t temp;
|
||
LSM303_ReadMultiReg(LSM303_OUT_X_L_A,6,buff);
|
||
|
||
temp = buff[1];
|
||
temp <<= 8;
|
||
temp |= buff[0];
|
||
*Xa = temp;
|
||
|
||
temp = buff[3];
|
||
temp <<= 8;
|
||
temp |= buff[2];
|
||
*Ya = temp;
|
||
|
||
temp = buff[5];
|
||
temp <<= 8;
|
||
temp |= buff[4];
|
||
*Za = temp;
|
||
}
|
||
|
||
/*************************************************************************************************************************
|
||
*function : LSM303_ReadMagnetic()
|
||
*paraments : int16_t *Xm, int16_t *Ym, int16_t *Zm
|
||
*return : None
|
||
*detail : to read the Magnetic data, but the real Magnetic should be divided by the unit(n LSB/M)
|
||
*************************************************************************************************************************/
|
||
void LSM303_ReadMagnetic(int16_t *Xm, int16_t *Ym, int16_t *Zm)
|
||
{
|
||
uint8_t buff[6];
|
||
int16_t temp;
|
||
LSM303_ReadMultiReg(LSM303_OUT_X_H_M,6, buff);
|
||
|
||
temp = buff[0];
|
||
temp <<= 8;
|
||
temp |= buff[1];
|
||
*Xm = temp;
|
||
|
||
temp = buff[2];
|
||
temp <<= 8;
|
||
temp |= buff[3];
|
||
*Zm = temp;
|
||
|
||
temp = buff[4];
|
||
temp <<= 8;
|
||
temp |= buff[5];
|
||
*Ym = temp;
|
||
}
|
||
|
||
/*************************************************************************************************************************
|
||
*function : LSM303_ReadTemperature()
|
||
*paraments : int16_t *Temp
|
||
*return : None
|
||
*detail : to read the Temperature data,
|
||
the real Temperature already be divided by 8(8 LSB/deg)(not guaranteed, as the datasheet said)
|
||
*************************************************************************************************************************/
|
||
void LSM303_ReadTemperature(int16_t *Temp)
|
||
{
|
||
uint8_t buff[2];
|
||
int16_t temp;
|
||
IIC_Read_Multi_Byte(&LSM303_bus, LSM303_SlaveAddr_M, TEMP_OUT_H_M, 2, buff);
|
||
temp = buff[0];
|
||
temp <<= 8;
|
||
temp |= buff[1];
|
||
temp >>= 4;
|
||
*Temp = temp / 3 + 0.5;//8 LSB/deg
|
||
}
|
||
|
||
/*************************************************************************************************************************
|
||
*function : LSM303DLH_CalculationZAxisAngle()
|
||
*paraments : int16_t Xa, int16_t Ya, int16_t Za
|
||
*return : ZAxisAngle
|
||
*detail :
|
||
*************************************************************************************************************************/
|
||
int LSM303DLH_CalculationZAxisAngle(int16_t Xa, int16_t Ya, int16_t Za)
|
||
{
|
||
double A;
|
||
float fx,fy,fz;
|
||
|
||
A = sqrt((int)Xa*Xa + (int)Ya*Ya + (int)Za*Za); //计算角加速度的矢量模长 |A|=根号下(X*X+Y*Y+Z*Z)
|
||
fx = Xa/A;
|
||
fy = Ya/A;
|
||
fz = Za/A;
|
||
|
||
|
||
//Z方向
|
||
A = fx*fx+fy*fy;
|
||
A = sqrt(A);
|
||
A = (double)A/fz;
|
||
A = atan(A);
|
||
A = A*180/PI;
|
||
if(A < 0)
|
||
{
|
||
A += 90;
|
||
A = 0-A;
|
||
}
|
||
else
|
||
{
|
||
A = 90-A;
|
||
}
|
||
return A*100;
|
||
}
|
||
|
||
|
||
/*************************************************************************************************************************
|
||
*function : LSM303DLH_CalculationZAxisAngle()
|
||
*paraments : int16_t Xa, int16_t Ya, int16_t Za
|
||
*return : XAxisAngle
|
||
*detail :
|
||
*************************************************************************************************************************/
|
||
int LSM303DLH_CalculationXAxisAngle(int16_t Xa, int16_t Ya, int16_t Za)
|
||
{
|
||
double A;
|
||
float fx,fy,fz;
|
||
|
||
A = sqrt((int)Xa*Xa + (int)Ya*Ya + (int)Za*Za); //计算角加速度的矢量模长 |A|=根号下(X*X+Y*Y+Z*Z)
|
||
fx = Xa/A;
|
||
fy = Ya/A;
|
||
fz = Za/A;
|
||
|
||
|
||
//X方向
|
||
A = fz*fz+fy*fy;
|
||
A = sqrt(A);
|
||
|
||
A = (double)A/fx;
|
||
A = atan(A);
|
||
A = A*180/PI;
|
||
if(A < 0)
|
||
{
|
||
A += 90; //向上为正
|
||
}
|
||
else
|
||
{
|
||
A = 90-A;
|
||
A = 0-A; //向下为负
|
||
}
|
||
return A*100;
|
||
}
|
||
|
||
/*************************************************************************************************************************
|
||
*function : Azimuth_Calculate()
|
||
*paraments : int16_t Xa, int16_t Ya, int16_t Za, int16_t Xm, int16_t Ym, int16_t Zm
|
||
*return : Azimuth
|
||
*detail : to Calculate the Azimuth to find the direction
|
||
*************************************************************************************************************************/
|
||
float Azimuth_Calculate(int16_t Xa, int16_t Ya, int16_t Za, int16_t Xm, int16_t Ym, int16_t Zm)
|
||
{
|
||
float pitch, roll, Hy, Hx, Azimuth;
|
||
pitch = atan2f(Xa, sqrtf(Ya * Ya + Za * Za));
|
||
roll = atan2f(Ya, sqrtf(Xa * Xa + Za * Za));
|
||
Hy = Ym * cosf(roll) + Xm * sinf(roll) * sinf(pitch) - Zm * cosf(pitch) * sinf(roll);
|
||
Hx = Xm * cosf(pitch) + Zm * sinf(pitch);
|
||
Azimuth = atan2f(Hy,Hx)*180.0/PI;
|
||
return Azimuth;
|
||
}
|