가속, 자이로, 온도, 압감지, 마그넷 센서
LSM6DSL, LIS2MDL, LPS22HB
소스와 통신하는 부분을 ST에서 제공하는 프로그램에서 발췌했더니 비효율적으로 되어있습니다.
데이터시트를 보고 최적화와 함께 함수들을 만들어 써야합니다.
void main(void)
{
ACCGyroAxis_TypeDef ACCAxis, GyroAxis;
MagnetAxis_TypeDef MagnetAxis;
float fPress, ftemperature;
float fBatt;
uint16_t Batt_Percent;
LSM6DSL_ACCGyroInit();
LIS2MDL_MagnetInit();
LPS22HB_TempPreInit();
LSM6DSL_ACCGyroEnable();
LIS2MDL_MagnetEnable();
LPS22HB_TempPreEnable();
LSM6DSL_ACCGyroSetting();
LIS2MDL_MagnetSetting();
while(1)
{
MeasurementSensorData(5, &ACCAxis, &GyroAxis, &MagnetAxis, &fPress, &ftemperature, &fBatt, &Batt_Percent);
}
}
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
#define LSM6DSL_ACC_SENSITIVITY_FOR_FS_2G 0.061 /**< Sensitivity value for 2 g full scale [mg/LSB] */
#define LSM6DSL_ACC_SENSITIVITY_FOR_FS_4G 0.122 /**< Sensitivity value for 4 g full scale [mg/LSB] */
#define LSM6DSL_ACC_SENSITIVITY_FOR_FS_8G 0.244 /**< Sensitivity value for 8 g full scale [mg/LSB] */
#define LSM6DSL_ACC_SENSITIVITY_FOR_FS_16G 0.488 /**< Sensitivity value for 16 g full scale [mg/LSB] */
#define LSM6DSL_GYRO_SENSITIVITY_FOR_FS_125DPS 04.375 /**< Sensitivity value for 125 dps full scale [mdps/LSB] */
#define LSM6DSL_GYRO_SENSITIVITY_FOR_FS_245DPS 08.750 /**< Sensitivity value for 245 dps full scale [mdps/LSB] */
#define LSM6DSL_GYRO_SENSITIVITY_FOR_FS_500DPS 17.500 /**< Sensitivity value for 500 dps full scale [mdps/LSB] */
#define LSM6DSL_GYRO_SENSITIVITY_FOR_FS_1000DPS 35.000 /**< Sensitivity value for 1000 dps full scale [mdps/LSB] */
#define LSM6DSL_GYRO_SENSITIVITY_FOR_FS_2000DPS 70.000 /**< Sensitivity value for 2000 dps full scale [mdps/LSB] */
#define LSM6DSL_ACC_SENSITIVITY LSM6DSL_ACC_SENSITIVITY_FOR_FS_2G
#define LSM6DSL_GYRO_SENSITIVITY LSM6DSL_GYRO_SENSITIVITY_FOR_FS_2000DPS
typedef enum {
eModeACC = 0,
eModeGyro,
} eModeACCGyro_TypeDef;
typedef struct {
int32_t ACCGyro_X;
int32_t ACCGyro_Y;
int32_t ACCGyro_Z;
}ACCGyroAxis_TypeDef;
//#define LSM6DSL_RESERVED 0x00
#define LSM6DSL_FUNC_CFG_ACCESS 0x01 //Embedded functions
//#define LSM6DSL_RESERVED 0x02
//#define LSM6DSL_RESERVED 0x03
#define LSM6DSL_SENSOR_SYNC_TIME_FRAME 0x04 //r/w
#define LSM6DSL_SENSOR_SYNC_RES_RATIO 0x05 //r/w
#define LSM6DSL_FIFO_CTRL1 0x06 //r/w
#define LSM6DSL_FIFO_CTRL2 0x07 //r/w
#define LSM6DSL_FIFO_CTRL3 0x08 //r/w
#define LSM6DSL_FIFO_CTRL4 0x09 //r/w
#define LSM6DSL_FIFO_CTRL5 0x0A //r/w
#define LSM6DSL_DRDY_PULSE_CFG_G 0x0B //r/w
//#define LSM6DSL_RESERVED 0x0C
#define LSM6DSL_INT1_CTRL 0x0D //r/w INT1 pin control
#define LSM6DSL_INT2_CTRL 0x0E //r/w INT2 pin control
#define LSM6DSL_WHO_AM_I 0x0F //r Who I am ID
#define LSM6DSL_CTRL1_XL 0x10 //r/w
#define LSM6DSL_CTRL2_G 0x11 //r/w
#define LSM6DSL_CTRL3_C 0x12 //r/w
#define LSM6DSL_CTRL4_C 0x13 //r/w
#define LSM6DSL_CTRL5_C 0x14 //r/w
#define LSM6DSL_CTRL6_C 0x15 //r/w
#define LSM6DSL_CTRL7_G 0x16 //r/w
#define LSM6DSL_CTRL8_XL 0x17 //r/w
#define LSM6DSL_CTRL9_XL 0x18 //r/w
#define LSM6DSL_CTRL10_C 0x19 //r/w
#define LSM6DSL_MASTER_CONFIG 0x1A //r/w
#define LSM6DSL_WAKE_UP_SRC 0x1B //r
#define LSM6DSL_TAP_SRC 0x1C //r Interrupt registers
#define LSM6DSL_D6D_SRC 0x1D //r
#define LSM6DSL_STATUS_REG 0x1E //r
//#define LSM6DSL_RESERVED 0x1F
#define LSM6DSL_OUT_TEMP_L 0x20 //r Temperature output
#define LSM6DSL_OUT_TEMP_H 0x21 //r data registers
#define LSM6DSL_OUTX_L_G 0x22 //r GYRO
#define LSM6DSL_OUTX_H_G 0x23 //r
#define LSM6DSL_OUTY_L_G 0x24 //r
#define LSM6DSL_OUTY_H_G 0x25 //r
#define LSM6DSL_OUTZ_L_G 0x26 //r
#define LSM6DSL_OUTZ_H_G 0x27 //r
#define LSM6DSL_OUTX_L_XL 0x28 //r ACC
#define LSM6DSL_OUTX_H_XL 0x29 //r
#define LSM6DSL_OUTY_L_XL 0x2A //r
#define LSM6DSL_OUTY_H_XL 0x2B //r
#define LSM6DSL_OUTZ_L_XL 0x2C //r
#define LSM6DSL_OUTZ_H_XL 0x2D //r
#define LSM6DSL_SENSORHUB1_REG 0x2E //r
#define LSM6DSL_SENSORHUB2_REG 0x2F //r
#define LSM6DSL_SENSORHUB3_REG 0x30 //r
#define LSM6DSL_SENSORHUB4_REG 0x31 //r
#define LSM6DSL_SENSORHUB5_REG 0x32 //r
#define LSM6DSL_SENSORHUB6_REG 0x33 //r
#define LSM6DSL_SENSORHUB7_REG 0x34 //r
#define LSM6DSL_SENSORHUB8_REG 0x35 //r
#define LSM6DSL_SENSORHUB9_REG 0x36 //r
#define LSM6DSL_SENSORHUB10_REG 0x37 //r
#define LSM6DSL_SENSORHUB11_REG 0x38 //r
#define LSM6DSL_SENSORHUB12_REG 0x39 //r
#define LSM6DSL_FIFO_STATUS1 0x3A //r
#define LSM6DSL_FIFO_STATUS2 0x3B //r
#define LSM6DSL_FIFO_STATUS3 0x3C //r
#define LSM6DSL_FIFO_STATUS4 0x3D //r
#define LSM6DSL_FIFO_DATA_OUT_L 0x3E //r FIFO data output
#define LSM6DSL_FIFO_DATA_OUT_H 0x3F //r
#define LSM6DSL_TIMESTAMP0_REG 0x40 //r
#define LSM6DSL_TIMESTAMP1_REG 0x41 //r
#define LSM6DSL_TIMESTAMP2_REG 0x42 //r/w
//#define LSM6DSL_RESERVED 0x43 ~ 0x48
#define LSM6DSL_STEP_TIMESTAMP_L 0x49 //r Step counter
#define LSM6DSL_STEP_TIMESTAMP_H 0x4A //r timestamp registers
#define LSM6DSL_STEP_COUNTER_L 0x4B //r Step counte0xoutput
#define LSM6DSL_STEP_COUNTER_H 0x4C //r registers
#define LSM6DSL_SENSORHUB13_REG 0x4D //r
#define LSM6DSL_SENSORHUB14_REG 0x4E //r
#define LSM6DSL_SENSORHUB15_REG 0x4F //r
#define LSM6DSL_SENSORHUB16_REG 0x50 //r
#define LSM6DSL_SENSORHUB17_REG 0x51 //r
#define LSM6DSL_SENSORHUB18_REG 0x52 //r
#define LSM6DSL_FUNC_SRC1 0x53 //r
#define LSM6DSL_FUNC_SRC2 0x54 //r
#define LSM6DSL_WRIST_TILT_IA 0x55 //r Interrupt register
//#define LSM6DSL_RESERVED - 56-57 -
#define LSM6DSL_TAP_CFG 0x58 //r/w
#define LSM6DSL_TAP_THS_6D 0x59 //r/w
#define LSM6DSL_INT_DUR2 0x5A //r/w
#define LSM6DSL_WAKE_UP_THS 0x5B //r/w
#define LSM6DSL_WAKE_UP_DU0x0x5C //r/w
#define LSM6DSL_FREE_FALL 0x5D //r/w
#define LSM6DSL_MD1_CFG 0x5E //r/w
#define LSM6DSL_MD2_CFG 0x5F //r/w
#define LSM6DSL_MASTER_CMD_CODE 0x60 //r/w
#define LSM6DSL_SENS_SYNC_SPI_ERROR_CODE 0x61 //r/w
//#define LSM6DSL_RESERVED - 62-65 - Reserved
#define LSM6DSL_OUT_MAG_RAW_X_L 0x66 //r
#define LSM6DSL_OUT_MAG_RAW_X_H 0x67 //r
#define LSM6DSL_OUT_MAG_RAW_Y_L 0x68 //r
#define LSM6DSL_OUT_MAG_RAW_Y_H 0x69 //r
#define LSM6DSL_OUT_MAG_RAW_Z_L 0x6A //r
#define LSM6DSL_OUT_MAG_RAW_Z_H 0x6B //r
//#define LSM6DSL_RESERVED - 6C-72 -
#define LSM6DSL_X_OFS_US0x0x73 //r/w
#define LSM6DSL_Y_OFS_US0x0x74 //r/w
#define LSM6DSL_Z_OFS_US0x0x75 //r/w
//#define LSM6DSL_RESERVED - 76-7F -
/* Private macro ------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes ------------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
extern void UART1_printf(const char *fmt,...);
ACCGyroAxis_TypeDef gstACCAxis;
ACCGyroAxis_TypeDef gstGyroAxis;
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
*******************************************************************************/
void LSM6DSL_ACCGyroInit(void)
{
uint8_t Data[10];
Data[0] = 0x0C;
SPI2_1Wire_Write(CS_LSM6DSL, LSM6DSL_CTRL3_C, Data, 1);
Data[0] = 0x00;
SPI2_1Wire_Read(CS_LSM6DSL, LSM6DSL_WHO_AM_I, Data, 1);
Data[0] = 0x6A;
SPI2_1Wire_Read(CS_LSM6DSL, LSM6DSL_CTRL3_C, Data, 1);
Data[0] = 0x0C;
SPI2_1Wire_Write(CS_LSM6DSL, LSM6DSL_CTRL3_C, Data, 1);
SPI2_1Wire_Read(CS_LSM6DSL, LSM6DSL_CTRL3_C, Data, 1);
Data[0] = 0x4C;
SPI2_1Wire_Write(CS_LSM6DSL, LSM6DSL_CTRL3_C, Data, 1);
SPI2_1Wire_Read(CS_LSM6DSL, LSM6DSL_FIFO_CTRL5, Data, 1);
Data[0] = 0x00;
SPI2_1Wire_Write(CS_LSM6DSL, LSM6DSL_FIFO_CTRL5, Data, 1);
SPI2_1Wire_Read(CS_LSM6DSL, LSM6DSL_CTRL1_XL, Data, 1);
Data[0] = 0x00;
SPI2_1Wire_Write(CS_LSM6DSL, LSM6DSL_CTRL1_XL, Data, 1);
SPI2_1Wire_Read(CS_LSM6DSL, LSM6DSL_CTRL1_XL, Data, 1);
Data[0] = 0x00;
SPI2_1Wire_Write(CS_LSM6DSL, LSM6DSL_CTRL1_XL, Data, 1);
SPI2_1Wire_Read(CS_LSM6DSL, LSM6DSL_CTRL4_C, Data, 1);
Data[0] = 0x04;
SPI2_1Wire_Write(CS_LSM6DSL,LSM6DSL_CTRL4_C, Data, 1);
Data[0] = 0x00;
SPI2_1Wire_Read(CS_LSM6DSL, LSM6DSL_WHO_AM_I, Data, 1);
Data[0] = 0x6A;
SPI2_1Wire_Read(CS_LSM6DSL, LSM6DSL_CTRL3_C, Data, 1);
Data[0] = 0x4C;
SPI2_1Wire_Write(CS_LSM6DSL, LSM6DSL_CTRL3_C, Data, 1);
SPI2_1Wire_Read(CS_LSM6DSL, LSM6DSL_CTRL3_C, Data, 1);
Data[0] = 0x4C;
SPI2_1Wire_Write(CS_LSM6DSL, LSM6DSL_CTRL3_C, Data, 1);
SPI2_1Wire_Read(CS_LSM6DSL, LSM6DSL_FIFO_CTRL5, Data, 1);
Data[0] = 0x00;
SPI2_1Wire_Write(CS_LSM6DSL, LSM6DSL_FIFO_CTRL5, Data, 1);
SPI2_1Wire_Read(CS_LSM6DSL, LSM6DSL_CTRL2_G, Data, 1);
Data[0] = 0x0C;
SPI2_1Wire_Write(CS_LSM6DSL, LSM6DSL_CTRL2_G, Data, 1);
SPI2_1Wire_Read(CS_LSM6DSL, LSM6DSL_CTRL2_G, Data, 1);
Data[0] = 0x0C;
SPI2_1Wire_Write(CS_LSM6DSL, LSM6DSL_CTRL2_G, Data, 1);
SPI2_1Wire_Read(CS_LSM6DSL, LSM6DSL_CTRL4_C, Data, 1);
Data[0] = 0x04;
SPI2_1Wire_Write(CS_LSM6DSL, LSM6DSL_CTRL4_C, Data, 1);
}
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
<- \ ACC + / ACC-
Side
Right - Left +
*******************************************************************************/
void LSM6DSL_ACCGyroEnable(void)
{
uint8_t Data[10], ACCGyroLoop;
Data[0] = 0x01;
SPI2_1Wire_Read(CS_LSM6DSL, LSM6DSL_CTRL1_XL, Data, 1);
Data[0] = 0x40;
SPI2_1Wire_Write(CS_LSM6DSL, LSM6DSL_CTRL1_XL, Data, 1);
Data[0] = 0x0A;
SPI2_1Wire_Read(CS_LSM6DSL, LSM6DSL_CTRL2_G, Data, 1);
Data[0] = 0x4C;
SPI2_1Wire_Write(CS_LSM6DSL, LSM6DSL_CTRL2_G, Data, 1);
delay_ms(100);
ACCGyroAxis_TypeDef ACCAxis, GyroAxis;
LSM6DSL_ACCGyroGet(&gstACCAxis, &gstGyroAxis);
for(ACCGyroLoop = 0; ACCGyroLoop < 10; ACCGyroLoop++){
LSM6DSL_ACCGyroGet(&ACCAxis, &GyroAxis);
gstACCAxis.ACCGyro_X = (gstACCAxis.ACCGyro_X + ACCAxis.ACCGyro_X)/2;
gstACCAxis.ACCGyro_Y = (gstACCAxis.ACCGyro_Y + ACCAxis.ACCGyro_Y)/2;
gstACCAxis.ACCGyro_Z = (gstACCAxis.ACCGyro_Z + ACCAxis.ACCGyro_Z)/2;
gstGyroAxis.ACCGyro_X = (gstGyroAxis.ACCGyro_X + GyroAxis.ACCGyro_X)/2;
gstGyroAxis.ACCGyro_Y = (gstGyroAxis.ACCGyro_Y + GyroAxis.ACCGyro_Y)/2;
gstGyroAxis.ACCGyro_Z = (gstGyroAxis.ACCGyro_Z + GyroAxis.ACCGyro_Z)/2;
delay_ms(1);
}
}
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
*******************************************************************************/
void LSM6DSL_ACCGyroSetting(void)
{
uint8_t Data[10];
//ODR 6.6kHz
Data[0] = 0x0A;
SPI2_1Wire_Read(CS_LSM6DSL, 0x10, Data, 1);
Data[0] = 0xA0;
SPI2_1Wire_Write(CS_LSM6DSL, 0x10, Data, 1);
//FS 4g
Data[0] = 0xA0;
SPI2_1Wire_Read(CS_LSM6DSL, 0x10, Data, 1);
Data[0] = 0xA8;
SPI2_1Wire_Write(CS_LSM6DSL, 0x10, Data, 1);
//ODR/2 low pass filtered sent to composite filter
Data[0] = 0xA8;
SPI2_1Wire_Read(CS_LSM6DSL, 0x17, Data, 1);
Data[0] &= ~0x80;//LSM6DSL_ACC_GYRO_IN_COMP_MASK;
Data[0] |= 0x00;//LSM6DSL_ACC_GYRO_IN_ODR_DIV_2;
SPI2_1Wire_Write(CS_LSM6DSL, 0x17, Data, 1);
//Enable LPF2 filter in composite filter block
Data[0] = 0x60;
SPI2_1Wire_Read(CS_LSM6DSL, 0x17, Data, 1);
Data[0] &= ~0x80;//LSM6DSL_ACC_GYRO_LPF2_XL_MASK;
Data[0] |= 0x80;//LSM6DSL_ACC_GYRO_LPF2_XL_ENABLE;
SPI2_1Wire_Write(CS_LSM6DSL, 0x17, Data, 1);
//Low pass filter @ ODR/400
Data[0] = 0xE0;
SPI2_1Wire_Read(CS_LSM6DSL, 0x17, Data, 1);
Data[0] &= ~0x60;//LSM6DSL_ACC_GYRO_HPCF_XL_MASK;
Data[0] |= 0x60;//LSM6DSL_ACC_GYRO_HPCF_XL_DIV400;
SPI2_1Wire_Write(CS_LSM6DSL, 0x17, Data, 1);
//ACC Read
Data[0] = 0x00;
SPI2_1Wire_Read(CS_LSM6DSL, 0x10, Data, 1);
//Set LSB to 0 >> Analog filter 1500Hz
Data[0] &= 0xFE;
SPI2_1Wire_Write(CS_LSM6DSL, 0x10, Data, 1);
// Initialize settings for 6-axis MEMS Gyroscope
// FS 2000dps */
// ODR 416Hz */
// LPF1 FTYPE set to 10b
Data[0] = 0xA8;
SPI2_1Wire_Read(CS_LSM6DSL, 0x15, Data, 1);
Data[0] &= ~0x03;//LSM6DSL_ACC_GYRO_FTYPE_MASK
Data[0] |= 0x01;//LSM6DSL_ACC_GYRO_LP_G_NARROW
SPI2_1Wire_Write(CS_LSM6DSL, 0x15, Data, 1);
//Gyroscope settings: full scale 2000dps, ODR 416Hz
Data[0] = 0x6C;
SPI2_1Wire_Write(CS_LSM6DSL, 0x11, Data, 1);
}
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
*******************************************************************************/
void LSM6DSL_ACCGyro(uint8_t *pRawdata, ACCGyroAxis_TypeDef *ACCGyroAxis, eModeACCGyro_TypeDef chMode)
{
short int ACCGyroTemp;
if(chMode == eModeACC){
ACCGyroTemp = ((((uint16_t)pRawdata[1]) << 8) + (uint16_t)pRawdata[0]);
ACCGyroAxis->ACCGyro_X = (ACCGyroTemp * LSM6DSL_ACC_SENSITIVITY);
ACCGyroTemp = ((((uint16_t)pRawdata[3]) << 8) + (uint16_t)pRawdata[2]);
ACCGyroAxis->ACCGyro_Y = (ACCGyroTemp * LSM6DSL_ACC_SENSITIVITY);
ACCGyroTemp = ((((uint16_t)pRawdata[5]) << 8) + (uint16_t)pRawdata[4]);
ACCGyroAxis->ACCGyro_Z = (ACCGyroTemp * LSM6DSL_ACC_SENSITIVITY);
}
else if(chMode == eModeGyro){
ACCGyroTemp = ((((uint16_t)pRawdata[1]) << 8) + (uint16_t)pRawdata[0]);
ACCGyroAxis->ACCGyro_X = (ACCGyroTemp * LSM6DSL_GYRO_SENSITIVITY);
ACCGyroTemp = ((((uint16_t)pRawdata[3]) << 8) + (uint16_t)pRawdata[2]);
ACCGyroAxis->ACCGyro_Y = (ACCGyroTemp * LSM6DSL_GYRO_SENSITIVITY);
ACCGyroTemp = ((((uint16_t)pRawdata[5]) << 8) + (uint16_t)pRawdata[4]);
ACCGyroAxis->ACCGyro_Z = (ACCGyroTemp * LSM6DSL_GYRO_SENSITIVITY);
}
}
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
*******************************************************************************/
void LSM6DSL_RawData(uint8_t *pAddr, uint8_t *pRawData, uint8_t chLen)
{
uint8_t SpiLoop;
for(SpiLoop = 0; SpiLoop < chLen; SpiLoop++){
// UART1_printf("%02X, %02X, %02X \r\n", CS_LSM6DSL, pAddr[SpiLoop], pRawData[SpiLoop]);
SPI2_1Wire_Read(CS_LSM6DSL, pAddr[SpiLoop], &pRawData[SpiLoop], 1 );
}
}
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
*******************************************************************************/
void LSM6DSL_ACCGyroGet(ACCGyroAxis_TypeDef *ACCAxis, ACCGyroAxis_TypeDef *GyroAxis)
{
const uint8_t Data_ACCGyro[15] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0A, 0xFE};
uint8_t Addr_ACCGyro[15] = \
{LSM6DSL_OUTX_L_XL, LSM6DSL_OUTX_H_XL, LSM6DSL_OUTY_L_XL, LSM6DSL_OUTY_H_XL, LSM6DSL_OUTZ_L_XL, LSM6DSL_OUTZ_H_XL\
, LSM6DSL_CTRL1_XL,\
LSM6DSL_OUTX_L_G, LSM6DSL_OUTX_H_G, LSM6DSL_OUTY_L_G, LSM6DSL_OUTY_H_G, LSM6DSL_OUTZ_L_G, LSM6DSL_OUTZ_H_G\
, LSM6DSL_CTRL2_G, LSM6DSL_CTRL2_G};
uint8_t Data[15] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0A, 0xFE};
uint8_t pData[6];
memcpy(Data, Data_ACCGyro, 15);
LSM6DSL_RawData(Addr_ACCGyro, Data, 15);//0~5, 7~12
memcpy(pData, Data, 6);
LSM6DSL_ACCGyro(pData, ACCAxis, eModeACC);
memcpy(pData, Data+7, 6);
LSM6DSL_ACCGyro(pData, GyroAxis, eModeGyro);
}
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
typedef struct {
int32_t Mag_X;
int32_t Mag_Y;
int32_t Mag_Z;
}MagnetAxis_TypeDef;
//#define LIS2MDL_Reserved 00 - 44 Reserved
#define LIS2MDL_OFFSET_X_REG_L 0x45 //r/w Hard-iron registers
#define LIS2MDL_OFFSET_X_REG_H 0x46 //r/w
#define LIS2MDL_OFFSET_Y_REG_L 0x47 //r/w
#define LIS2MDL_OFFSET_Y_REG_H 0x48 //r/w
#define LIS2MDL_OFFSET_Z_REG_L 0x49 //r/w
#define LIS2MDL_OFFSET_Z_REG_H 0x4A //r/w
//#define LIS2MDL_RESERVED 4B-4C Reserved
#define LIS2MDL_WHO_AM_I 0x4F //r
//#define LIS2MDL_RESERVED 50-5F Reserved
#define LIS2MDL_CFG_REG_A 0x60 //r/w Configuration
#define LIS2MDL_CFG_REG_B 0x61 //r/w
#define LIS2MDL_CFG_REG_C 0x62 //r/w
#define LIS2MDL_INT_CRTL_REG 0x63 //r/w configuration registers
#define LIS2MDL_INT_SOURCE_REG 0x64 //r
#define LIS2MDL_INT_THS_L_REG 0x65 //r/w
#define LIS2MDL_INT_THS_H_REG 0x66 //r/w
#define LIS2MDL_STATUS_REG 0x67 //r
#define LIS2MDL_OUTX_L_REG 0x68 //r Output registers
#define LIS2MDL_OUTX_H_REG 0x69 //r
#define LIS2MDL_OUTY_L_REG 0x6A //r
#define LIS2MDL_OUTY_H_REG 0x6B //r
#define LIS2MDL_OUTZ_L_REG 0x6C //r
#define LIS2MDL_OUTZ_H_REG 0x6D //r
#define LIS2MDL_TEMP_OUT_L_REG 0x6E //r Temperature sensor
#define LIS2MDL_TEMP_OUT_H_REG 0x6F //r
/* Private macro ------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes ------------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
extern void UART1_printf(const char *fmt,...);
MagnetAxis_TypeDef gStMagnetAxis;
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
*******************************************************************************/
void LIS2MDL_MagnetInit(void)
{
uint8_t Data[10];
Data[0] = 0x00;
SPI2_1Wire_Read(CS_LIS2MDL, LIS2MDL_WHO_AM_I, Data, 1);
Data[0] = 0x40;
SPI2_1Wire_Read(CS_LIS2MDL, LIS2MDL_CFG_REG_A, Data, 1);//reboot memory content
Data[0] = 0x0F;
SPI2_1Wire_Write(CS_LIS2MDL, LIS2MDL_CFG_REG_A, Data, 1);//Output data rate configuration - 50Hz
SPI2_1Wire_Read(CS_LIS2MDL, LIS2MDL_CFG_REG_C, Data, 1);
Data[0] = 0x10;
SPI2_1Wire_Write(CS_LIS2MDL, LIS2MDL_CFG_REG_C, Data, 1);// reading of incorrect data is avoided when the user reads asynchronously
SPI2_1Wire_Read(CS_LIS2MDL, LIS2MDL_CFG_REG_A, Data, 1);
Data[0] = 0x0F;
SPI2_1Wire_Write(CS_LIS2MDL, LIS2MDL_CFG_REG_A, Data, 1);
SPI2_1Wire_Read(CS_LIS2MDL, LIS2MDL_CFG_REG_C, Data, 1);
Data[0] = 0x10;
SPI2_1Wire_Write(CS_LIS2MDL, LIS2MDL_CFG_REG_C, Data, 1);
}
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
1. Write CFG_REG_A = 80h // Enable temperature compensation
//Mag = 10 Hz (high-resolution and continuous mode)
2. Write CFG_REG_C = 01h // Mag data-ready interrupt enable
*******************************************************************************/
void LIS2MDL_MagnetEnable(void)
{
uint8_t Data[10];
Data[0] = 0x0A;
SPI2_1Wire_Read(CS_LIS2MDL, LIS2MDL_CFG_REG_A, Data, 1);
Data[0] = 0x0C;
SPI2_1Wire_Write(CS_LIS2MDL, LIS2MDL_CFG_REG_A, Data, 1);//100Hz
}
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
Initialize settings for Magnetometer settings
(By default after reset is in in idle mode)
*******************************************************************************/
void LIS2MDL_MagnetSetting(void)
{
uint8_t Data[10];
Data[0] = 0x8C;
SPI2_1Wire_Write(CS_LIS2MDL, LIS2MDL_CFG_REG_A, Data, 1);
Data[0] = 0x02;
SPI2_1Wire_Write(CS_LIS2MDL, LIS2MDL_CFG_REG_B, Data, 1);
}
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
*******************************************************************************/
void LIS2MDL_Magnet(uint8_t *pRawdata, MagnetAxis_TypeDef *MagnetAxis, float fSensitivity)
{
short int MagnetTemp;
MagnetTemp = (((uint16_t)pRawdata[1]) << 8) + (uint16_t)pRawdata[0];
MagnetAxis->Mag_X = (MagnetTemp * fSensitivity);
MagnetTemp = (((uint16_t)pRawdata[3]) << 8) + (uint16_t)pRawdata[2];
MagnetAxis->Mag_Y = (MagnetTemp * fSensitivity);
MagnetTemp = (((uint16_t)pRawdata[5]) << 8) + (uint16_t)pRawdata[4];
MagnetAxis->Mag_Z = (MagnetTemp * fSensitivity);
}
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
*******************************************************************************/
void LIS2MDL_RawData(uint8_t *pAddr, uint8_t *pRawData, uint8_t chLen)
{
uint8_t SpiLoop;
for(SpiLoop = 0; SpiLoop < chLen; SpiLoop++){
// UART1_printf("%02X, %02X, %02X \r\n", CS_LIS2MDL, pAddr[SpiLoop], pRawData[SpiLoop]);
SPI2_1Wire_Read(CS_LIS2MDL, pAddr[SpiLoop], &pRawData[SpiLoop], 1 );
}
}
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
*******************************************************************************/
void LIS2MDL_MagnetGet(MagnetAxis_TypeDef *MagnetAxis)
{
uint8_t Addr_Magnet[6] = {0x68, 0x69, 0x6A, 0x6B, 0x6C, 0x6D};
uint8_t Data[6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
int16_t *pRawData;
float Sensitivity = 1.5f;
LIS2MDL_RawData(Addr_Magnet, Data, 6);
LIS2MDL_Magnet(Data, MagnetAxis, Sensitivity);
}
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
//#define LPS22HB_Reserved 0x00 - 0A - Reserved
#define LPS22HB_INTERRUPT_CFG 0x0B //r/w Interrupt register
#define LPS22HB_THS_P_L 0x0C //r/w threshold registers
#define LPS22HB_THS_P_H 0x0D //r/w
//#define LPS22HB_Reserved 0E - Reserved
#define LPS22HB_WHO_AM_I 0x0F //r Who am I
#define LPS22HB_CTRL_REG1 0x10 //r/w
#define LPS22HB_CTRL_REG2 0x11 //r/w Control registers
#define LPS22HB_CTRL_REG3 0x12 //r/w
//#define LPS22HB_Reserved 13 - Reserved
#define LPS22HB_FIFO_CTRL 0x14 //r/w FIFO configuration register
#define LPS22HB_REF_P_XL 0x15 //r/w
#define LPS22HB_REF_P_L 0x16 //r/w Reference pressure registers
#define LPS22HB_REF_P_H 0x17 //r/w
#define LPS22HB_RPDS_L 0x18 //r/w offset registers
#define LPS22HB_RPDS_H 0x19 //r/w
#define LPS22HB_RES_CONF 0x1A //r/w Resolution register
//#define LPS22HB_Reserved 1B - 24 - Reserved
#define LPS22HB_INT_SOURCE 0x25 //r Interrupt register
#define LPS22HB_FIFO_STATUS 0x26 //r FIFO status register
#define LPS22HB_STATUS 0x27 //r Status register
#define LPS22HB_PRESS_OUT_XL 0x28 //r
#define LPS22HB_PRESS_OUT_L 0x29 //r Pressure output registers
#define LPS22HB_PRESS_OUT_H 0x2A //r
#define LPS22HB_TEMP_OUT_L 0x2B //r Temperature output registers
#define LPS22HB_TEMP_OUT_H 0x2C //r
//#define LPS22HB_Reserved 2D - 32 - Reserved
#define LPS22HB_LPFP_RES 0x33 //r Filte0xreset register
/* Private macro ------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes ------------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
*******************************************************************************/
void LPS22HB_TempPreInit(void)
{
uint8_t Data[10];
Data[0] = 0x01;
SPI2_1Wire_Write(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
SPI2_1Wire_Read(CS_LPS22HB, LPS22HB_CTRL_REG2, Data, 1);
Data[0] = 0xFC;
SPI2_1Wire_Write(CS_LPS22HB, LPS22HB_CTRL_REG2, Data, 1);
Data[0] = 0x01;
SPI2_1Wire_Write(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
SPI2_1Wire_Read(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
Data[0] = 0x01;
SPI2_1Wire_Write(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
Data[0] = 0x00;
SPI2_1Wire_Read(CS_LPS22HB, LPS22HB_WHO_AM_I, Data, 1);
Data[0] = 0xB1;
SPI2_1Wire_Read(CS_LPS22HB, LPS22HB_RES_CONF, Data, 1);
Data[0] = 0x01;
SPI2_1Wire_Write(CS_LPS22HB, LPS22HB_RES_CONF, Data, 1);
SPI2_1Wire_Read(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
Data[0] = 0x01;
SPI2_1Wire_Write(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
SPI2_1Wire_Read(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
Data[0] = 0x01;
SPI2_1Wire_Write(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
SPI2_1Wire_Read(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
Data[0] = 0x01;
SPI2_1Wire_Write(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
SPI2_1Wire_Read(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
Data[0] = 0x03;
SPI2_1Wire_Write(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
SPI2_1Wire_Read(CS_LPS22HB, LPS22HB_CTRL_REG2, Data, 1);
Data[0] = 0x00;
SPI2_1Wire_Write(CS_LPS22HB, LPS22HB_CTRL_REG2, Data, 1);
SPI2_1Wire_Read(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
Data[0] = 0x03;
SPI2_1Wire_Write(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
SPI2_1Wire_Read(CS_LPS22HB, LPS22HB_CTRL_REG2, Data, 1);
Data[0] = 0xF8;
SPI2_1Wire_Write(CS_LPS22HB, LPS22HB_CTRL_REG2, Data, 1);
}
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
*******************************************************************************/
void LPS22HB_TempPreEnable(void)
{
uint8_t Data[10];
Data[0] = 0x0A;
SPI2_1Wire_Read(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
Data[0] = 0x33;
SPI2_1Wire_Write(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
SPI2_1Wire_Read(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
Data[0] = 0x0A;
SPI2_1Wire_Read(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
Data[0] = 0x33;
SPI2_1Wire_Write(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
SPI2_1Wire_Read(CS_LPS22HB, LPS22HB_CTRL_REG1, Data, 1);
}
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
*******************************************************************************/
void LPS22HB_Press(uint8_t *pRawdata, float *fPress)
{
int32_t RawPresse, Pout;
uint32_t Temp = 0;
uint8_t PressLoop;
for(PressLoop = 0; PressLoop < 3; PressLoop++){
Temp |= (((uint32_t)pRawdata[PressLoop]) << (8 * PressLoop));
}
/* convert the 2's complement 24 bit to 2's complement 32 bit */
if(Temp & 0x00800000){
Temp |= 0xFF000000;
}
RawPresse = ((int32_t)Temp);
Pout = (RawPresse * 100) / 4096;
*fPress = ( float )Pout / 100.0f;
}
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
*******************************************************************************/
void LPS22HB_Temperature(uint8_t *pRawdata, float *fTemperature)
{
short int TemperatureTemp = 0, Tout = 0;
TemperatureTemp = (((uint16_t)pRawdata[1]) << 8) + (uint16_t)pRawdata[0];
Tout = (TemperatureTemp * 10) / 100;
*fTemperature = (float)Tout / 10.0f;
}
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
*******************************************************************************/
void LPS22HB_RawData(uint8_t *pAddr, uint8_t *pRawData, uint8_t chLen)
{
uint8_t SpiLoop;
for(SpiLoop = 0; SpiLoop < chLen; SpiLoop++){
// UART1_printf("%02X, %02X, %02X \r\n", CS_LPS22HB, pAddr[SpiLoop], pRawData[SpiLoop]);
SPI2_1Wire_Read(CS_LPS22HB, pAddr[SpiLoop], &pRawData[SpiLoop], 1 );
}
}
/*******************************************************************************
* Function Name :
* Parameters : None
* Return : None
* Description :
*******************************************************************************/
void LPS22HB_TempPressGet(float *fPress, float *ftemperature)
{
uint8_t Addr_Temperature[2] = {0x2B, 0x2C};
uint8_t Addr_Pressure[3] = {0x28, 0x29, 0x2A};
uint8_t data[3] = {0x4F, 0xFF, 0x00};
LPS22HB_RawData(Addr_Temperature, data, 2);
LPS22HB_Temperature(data, ftemperature);
LPS22HB_RawData(Addr_Pressure, data, 3);
LPS22HB_Press(data, fPress);
}
/*******************************************************************************
* Function Name :
* Parameters : None
* Return :
* Description :
*******************************************************************************/
void MeasurementSensorData(const uint8_t ConvertType,ACCGyroAxis_TypeDef *pACCAxis, ACCGyroAxis_TypeDef *pGyroAxis, MagnetAxis_TypeDef *pMagnetAxis, float *pfPress, float *pftemperature, float *pfBatt, uint16_t *pBatt_Percent)
{
int32_t Tmp;
ACCGyroAxis_TypeDef ACCAxis, GyroAxis;
MagnetAxis_TypeDef MagnetAxis;
float fPress, ftemperature;
/* Measurement */
LSM6DSL_ACCGyroGet(&ACCAxis, &GyroAxis);
LIS2MDL_MagnetGet(&MagnetAxis);
LPS22HB_TempPressGet(pfPress, pftemperature);
*pfBatt = Calculation_VBAT(uhADCxConvertedValue[0], 0);
*pBatt_Percent = Calculation_VBAT_Percent(*pfBatt, MINVOLTAGE, MAXVOLTAGE, TARGETPERCENT);
pMagnetAxis->Mag_X = (int32_t) MagnetAxis.Mag_X;
pMagnetAxis->Mag_Y = (int32_t) MagnetAxis.Mag_Y;
pMagnetAxis->Mag_Z = (int32_t) MagnetAxis.Mag_Z;
if(ConvertType == 1){
Tmp = pACCAxis->ACCGyro_X;
pACCAxis->ACCGyro_X = pACCAxis->ACCGyro_Y;
pACCAxis->ACCGyro_Y = -Tmp;
Tmp = pGyroAxis->ACCGyro_X;
pGyroAxis->ACCGyro_X = pGyroAxis->ACCGyro_Y;
pGyroAxis->ACCGyro_Y = -Tmp;
Tmp = pMagnetAxis->Mag_X;
pMagnetAxis->Mag_X = pMagnetAxis->Mag_Y;
pMagnetAxis->Mag_Y = -Tmp;
}
else if (ConvertType == 2){
}
else if (ConvertType == 3){
pACCAxis->ACCGyro_X = -ACCAxis.ACCGyro_Y;
pACCAxis->ACCGyro_Y = ACCAxis.ACCGyro_X;
pACCAxis->ACCGyro_Z = ACCAxis.ACCGyro_Z;
pGyroAxis->ACCGyro_X = -GyroAxis.ACCGyro_Y;
pGyroAxis->ACCGyro_Y = GyroAxis.ACCGyro_X;
pGyroAxis->ACCGyro_Z = GyroAxis.ACCGyro_Z;
pMagnetAxis->Mag_X = -MagnetAxis.Mag_Y;
pMagnetAxis->Mag_Y = MagnetAxis.Mag_X;
}
else if (ConvertType == 4){
pACCAxis->ACCGyro_X = -ACCAxis.ACCGyro_X;
pACCAxis->ACCGyro_Y = -ACCAxis.ACCGyro_Y;
pGyroAxis->ACCGyro_X = -GyroAxis.ACCGyro_X;
pGyroAxis->ACCGyro_Y = -GyroAxis.ACCGyro_Y;
pMagnetAxis->Mag_X = -MagnetAxis.Mag_X;
pMagnetAxis->Mag_Y = -MagnetAxis.Mag_Y;
}
else if (ConvertType == 5){
pACCAxis->ACCGyro_X = ACCAxis.ACCGyro_X;
pACCAxis->ACCGyro_Y = ACCAxis.ACCGyro_Y;
pACCAxis->ACCGyro_Z = ACCAxis.ACCGyro_Z;
pGyroAxis->ACCGyro_X = GyroAxis.ACCGyro_X;
pGyroAxis->ACCGyro_Y = GyroAxis.ACCGyro_Y;
pGyroAxis->ACCGyro_Z = GyroAxis.ACCGyro_Z;
}
}
'초보의 아웅다웅 설계하기 > STM32' 카테고리의 다른 글
| Keil 자동완성 기능 끄기 (0) | 2019.03.21 |
|---|---|
| PID 제어 (0) | 2019.03.02 |
| Base Timer 인터럽트 사용 안하고 설정하기 (1) | 2018.12.20 |
| STM32F Sleep Mode에서 Uart로 Wakeup하기 (0) | 2018.12.06 |
| SoftUart Only TX (2) | 2018.10.22 |