ST陀螺仪LSM6DSR的驱动
由于最近MPU的价格疯涨，不得已只能更换主控上的陀螺仪芯片，刚好VL53L1X这颗激光芯片的厂家ST有几颗陀螺仪可以用，就拿了几颗试一下，感觉效果还不错，好像可以用于手机做单击双击的判断，于是突发奇想是不是也可以用作我们小车的对抗系统用来检测是否被炮弹击中呢？不过目前还没有尝试这个功能，只是先用来给无人机做姿态解算。直接贴代码，驱动还是蛮简单的，对着说明书就能搞出来。
#define LSM6DSR_IIC_ADDR 0XD4
#define LSM6DSR_INT1_CTRL 0X0D
#define LSM6DSR_INT2_CTRL 0X0E
#define LSM6DSR_WHO_AM_I 0X0F
#define LSM6DSR_ID 0X6B
#define LSM6DSR_CTRL1_XL 0X10
#define LSM6DSR_CTRL2_G 0X11
#define LSM6DSR_CTRL3_C 0X12
#define LSM6DSR_CTRL4_C 0X13
#define LSM6DSR_CTRL5_C 0X14
#define LSM6DSR_CTRL6_C 0X15
#define LSM6DSR_CTRL7_G 0X16
#define LSM6DSR_CTRL8_XL 0X17
#define LSM6DSR_CTRL9_XL 0X18
#define LSM6DSR_CTRL10_C 0X19
#define LSM6DSR_STATUS_REG 0X1E
#define LSM6DSR_OUT_TEMP_L 0X20
#define LSM6DSR_OUT_TEMP_H 0X21
#define LSM6DSR_OUTX_L_GYRO 0X22
#define LSM6DSR_OUTX_H_GYRO 0X23
#define LSM6DSR_OUTY_L_GYRO 0X24
#define LSM6DSR_OUTY_H_GYRO 0X25
#define LSM6DSR_OUTZ_L_GYRO 0X26
#define LSM6DSR_OUTZ_H_GYRO 0X27
#define LSM6DSR_OUTX_L_ACC 0X28
#define LSM6DSR_OUTX_H_ACC 0X29
#define LSM6DSR_OUTY_L_ACC 0X2A
#define LSM6DSR_OUTY_H_ACC 0X2B
#define LSM6DSR_OUTZ_L_ACC 0X2C
#define LSM6DSR_OUTZ_H_ACC 0X2D
#define LSM6DSR_I3C_BUS_AVB 0x62
#define PROPERTY_ENABLE (1U)
#define PROPERTY_DISABLE (0U)
typedef enum ACC_LPF_1
{
ACC_USE_LPF1 = 0<<1,
ACC_USE_LPF2 = 1<<1,
}ACC_LPF_USE_SET_E;
typedef enum ACC_ODR_SET
{
ACC_ODR_1_6HZ = 11 << 4,
ACC_ODR_12_5HZ = 1 << 4,
ACC_ODR_26HZ = 2 << 4,
ACC_ODR_52HZ = 3 << 4,
ACC_ODR_104HZ = 4 << 4,
ACC_ODR_208HZ = 5 << 4,
ACC_ODR_417HZ = 6 << 4,
ACC_ODR_833HZ = 7 << 4,
ACC_ODR_1660HZ = 8 << 4,
ACC_ODR_3330HZ = 9 << 4,
ACC_ODR_6660HZ = 10 << 4
} ACC_ODR_SET_E;
typedef enum GYRO_ODR_SET
{
GYRO_ODR_12_5HZ = 1 << 4,
GYRO_ODR_26HZ = 2 << 4,
GYRO_ODR_52HZ = 3 << 4,
GYRO_ODR_104HZ = 4 << 4,
GYRO_ODR_208HZ = 5 << 4,
GYRO_ODR_417HZ = 6 << 4,
GYRO_ODR_833HZ = 7 << 4,
GYRO_ODR_1660HZ = 8 << 4,
GYRO_ODR_3330HZ = 9 << 4,
GYRO_ODR_6660HZ = 10 << 4
} GYRO_ODR_SET_E;
typedef enum XL_HM_MODE_SET
{
XL_HM_MODE_ENABLE = 0 << 4, //启用高性能模式
XL_HM_MODE_DISABLE = 1 << 4
} XL_HM_MODE_SET_E;
typedef enum G_HM_MODE_SET
{
G_HM_MODE_ENABLE = 0 << 7, //启用高性能模式
G_HM_MODE_DISABLE = 1 << 7,
} G_HM_MODE_SET_E;
typedef enum
{
LSM6DSR_2g = 0<<2,
LSM6DSR_16g = 1<<2,
LSM6DSR_4g = 2<<2,
LSM6DSR_8g = 3<<2,
} ACC_G_SET_E;
typedef enum
{
LSM6DSR_125dps = 2,
LSM6DSR_250dps = 0,
LSM6DSR_500dps = 4,
LSM6DSR_1000dps = 8,
LSM6DSR_2000dps = 12,
LSM6DSR_4000dps = 1,
} GYRO_DPS_SET_E;
typedef enum
{
LSM6DSR_I3C_DISABLE = 0x80,
LSM6DSR_I3C_ENABLE_T_50us = 0x00,
LSM6DSR_I3C_ENABLE_T_2us = 0x01,
LSM6DSR_I3C_ENABLE_T_1ms = 0x02,
LSM6DSR_I3C_ENABLE_T_25ms = 0x03,
} lsm6dsr_i3c_disable_t;
uint16_t LSM6DSR_i2c_error_num = 0;
static uint8_t LSM6DSR_i2c_rx_buf[24];
static int16_t LSM6DSR_gyroBuffer[3];
static int16_t LSM6DSR_accBuffer[3];
bool LSM6DSR_gyroUpdated;
bool LSM6DSR_accUpdated;
static uint16_t LSM6DSR_acc_lsb_div = 0;
static uint16_t LSM6DSR_gyro_lsb_div = 0;
void LSM6DSR_Write_Reg(uint8_t reg, uint8_t val)
{
uint8_t i2c_error = soft_i2c_write(LSM6DSR_IIC_ADDR, reg, &val, 1);
if (i2c_error)
LSM6DSR_i2c_error_num++;
// usartPrintf("i2c_error:0x%x\n", i2c_error);
}
void LSM6DSR_Write_Reg_Bytes(uint8_t reg, uint8_t *val, uint8_t len)
{
uint8_t i2c_error = soft_i2c_write(LSM6DSR_IIC_ADDR, reg, val, len);
if (i2c_error)
LSM6DSR_i2c_error_num++;
// usartPrintf("i2c_error:0x%x\n", i2c_error);
}
/**
* @brief read one byte from register
* @note
* @param reg: register address
* @retval
*/
uint8_t LSM6DSR_Read_Reg(uint8_t reg)
{
uint8_t i2c_error = soft_i2c_read(LSM6DSR_IIC_ADDR, reg, LSM6DSR_i2c_rx_buf, 1);
if (i2c_error)
LSM6DSR_i2c_error_num++;
// usartPrintf("i2c_error:0x%x\n", i2c_error);
return LSM6DSR_i2c_rx_buf[0];
}
/**
* @brief read multiple bytes form Register
* @note
* @param reg: Register address
* @param pBuffer: data out buffer
* @param size: size of read bytes
* @retval None
*/
void LSM6DSR_Read_Reg_Bytes(uint8_t reg, uint8_t *pBuffer, uint8_t size)
{
for (u8 i = 0; i < (size); i++)
{
soft_i2c_read(LSM6DSR_IIC_ADDR, reg, pBuffer, 1);
reg++;
pBuffer++;
}
}
bool Check_LSM6DSR_ID()
{
return (LSM6DSR_Read_Reg(LSM6DSR_WHO_AM_I) == LSM6DSR_ID);
}
void LSM6DSR_RESET()
{
LSM6DSR_Write_Reg(LSM6DSR_CTRL3_C, 0X80);
}
bool LSM6DSR_init()
{
// LSM6DSR_RESET();
// delay_ms(100);
LSM6DSR_Write_Reg(LSM6DSR_I3C_BUS_AVB, LSM6DSR_I3C_DISABLE & 0X03); //禁用I3C
LSM6DSR_Write_Reg(LSM6DSR_CTRL3_C, 0X40);
LSM6DSR_Write_Reg(LSM6DSR_CTRL1_XL, ACC_ODR_208HZ | LSM6DSR_8g | ACC_USE_LPF1); //设置加速度参数，量程选自己合适的就好
LSM6DSR_Write_Reg(LSM6DSR_CTRL2_G, GYRO_ODR_208HZ | LSM6DSR_2000dps); //设置角速度参数
LSM6DSR_Write_Reg(LSM6DSR_CTRL7_G, G_HM_MODE_ENABLE); //启用高性能模式
LSM6DSR_Write_Reg(LSM6DSR_CTRL6_C, XL_HM_MODE_ENABLE); //启用高性能模式
// LSM6DSR_Write_Reg(LSM6DSR_CTRL1_XL, PROPERTY_DISABLE); //只是用LPF1不使用LPF2
delay_ms(100);
LSM6DSR_acc_lsb_div = 65536 / (8 * 2); //计算量程，量程=2^16/(所选g值*2)，单位g，这颗芯片官方的代码有给怎么计算，但是他那个的数据精确度不高
LSM6DSR_gyro_lsb_div = 2000; //角速度量程，单位dps
IMU_SENSOR_USE(IMU_USE_LSM6DSR);
return 0;
}
void LSM6DSR_GYRO_ACC_UPDATE(void) //读取原始数据
{
LSM6DSR_accUpdated = 0;
LSM6DSR_gyroUpdated = 0;
// if (LSM6DSR_Read_Reg(LSM6DSR_STATUS_REG) & 0x01)
// {
LSM6DSR_accBuffer[0] = (int16_t)LSM6DSR_Read_Reg(LSM6DSR_OUTX_L_ACC) + (int16_t)LSM6DSR_Read_Reg(LSM6DSR_OUTX_H_ACC) * 256;
LSM6DSR_accBuffer[1] = (int16_t)LSM6DSR_Read_Reg(LSM6DSR_OUTY_L_ACC) + (int16_t)LSM6DSR_Read_Reg(LSM6DSR_OUTY_H_ACC) * 256;
LSM6DSR_accBuffer[2] = (int16_t)LSM6DSR_Read_Reg(LSM6DSR_OUTZ_L_ACC) + (int16_t)LSM6DSR_Read_Reg(LSM6DSR_OUTZ_H_ACC) * 256;
LSM6DSR_accUpdated = 1;
// }
// else
// LSM6DSR_accUpdated = 0;
// if (LSM6DSR_Read_Reg(LSM6DSR_STATUS_REG) & 0x02)
// {
LSM6DSR_gyroBuffer[0] = (int16_t)LSM6DSR_Read_Reg(LSM6DSR_OUTX_L_GYRO) + (int16_t)LSM6DSR_Read_Reg(LSM6DSR_OUTX_H_GYRO) * 256;
LSM6DSR_gyroBuffer[1] = (int16_t)LSM6DSR_Read_Reg(LSM6DSR_OUTY_L_GYRO) + (int16_t)LSM6DSR_Read_Reg(LSM6DSR_OUTY_H_GYRO) * 256;
LSM6DSR_gyroBuffer[2] = (int16_t)LSM6DSR_Read_Reg(LSM6DSR_OUTZ_L_GYRO) + (int16_t)LSM6DSR_Read_Reg(LSM6DSR_OUTZ_H_GYRO) * 256;
LSM6DSR_gyroUpdated = 1;
// }
// else
// LSM6DSR_gyroUpdated = 0;
}
bool LSM6DSR_GET_GYRO_DATA(int16_t *g)
{
g[X] = -LSM6DSR_gyroBuffer[Y];
g[Y] = -LSM6DSR_gyroBuffer[X];
g[Z] = -LSM6DSR_gyroBuffer[Z];
return LSM6DSR_gyroUpdated;
}
bool LSM6DSR_GET_ACC_DATA(int16_t *a)
{
a[X] = -LSM6DSR_accBuffer[Y];
a[Y] = -LSM6DSR_accBuffer[X];
a[Z] = -LSM6DSR_accBuffer[Z];
return LSM6DSR_accUpdated;
}
void LSM6DSR_TEST(void)
{
uint8_t status = 0;
static bool LSM6DSR_INIT = false;
// soft_i2c_scan_device();
// usartPrintf("ID=%x\n",LSM6DSR_Read_Reg(LSM6DSR_WHO_AM_I));
if (Check_LSM6DSR_ID())
{
if (!LSM6DSR_INIT)
{
LSM6DSR_init();
LSM6DSR_INIT = true;
}
LSM6DSR_GYRO_ACC_UPDATE();
}
else
{
usartPrintf("LSM6DSR_Check_ID ERROR!\n");
status |= 0x1;
}
SW_DigitalOsc_Head();
SW_DigitalOsc_int16(LSM6DSR_gyroBuffer[0]);
SW_DigitalOsc_int16(LSM6DSR_gyroBuffer[1]);
SW_DigitalOsc_int16(LSM6DSR_gyroBuffer[2]);
SW_DigitalOsc_int16(LSM6DSR_accBuffer[0]);
SW_DigitalOsc_int16(LSM6DSR_accBuffer[1]);
SW_DigitalOsc_int16(LSM6DSR_accBuffer[2]);
// SW_DigitalOsc_int16(accBuffer[2]);
// SW_DigitalOsc_int16(LSM6DSR_Read_Reg(LSM6DSR_OUTZ_L_ACC));
// SW_DigitalOsc_int16(LSM6DSR_Read_Reg(LSM6DSR_OUTZ_H_ACC));
SW_DigitalOsc_End();
// usartPrintf("status = %d\n",status);
}
int16_t LSM6DSR_get_gyro_scale_factor(void)
{
return LSM6DSR_gyro_lsb_div;
}
int16_t LSM6DSR_get_acc_scale_factor(void)
{
return LSM6DSR_acc_lsb_div;
}
