LSM6DS3 gyro数据设置
一 Driver：
1 数据结构
lsm6ds3_core.c 定义了两个结构体
static const struct lsm6ds3_odr_table {
u8 addr[2];
u8 mask[2];
struct lsm6ds3_odr_reg odr_avl[6];
} lsm6ds3_odr_table = {
.addr[LSM6DS3_ACCEL] = LSM6DS3_ACC_ODR_ADDR,
.mask[LSM6DS3_ACCEL] = LSM6DS3_ACC_ODR_MASK,
.addr[LSM6DS3_GYRO] = LSM6DS3_GYR_ODR_ADDR,
.mask[LSM6DS3_GYRO] = LSM6DS3_GYR_ODR_MASK,
.odr_avl[0] = { .hz = 13, .value = LSM6DS3_ODR_13HZ_VAL },
.odr_avl[1] = { .hz = 26, .value = LSM6DS3_ODR_26HZ_VAL },
.odr_avl[2] = { .hz = 52, .value = LSM6DS3_ODR_52HZ_VAL },
.odr_avl[3] = { .hz = 104, .value = LSM6DS3_ODR_104HZ_VAL },
.odr_avl[4] = { .hz = 208, .value = LSM6DS3_ODR_208HZ_VAL },
.odr_avl[5] = { .hz = 416, .value = LSM6DS3_ODR_416HZ_VAL },
};
对应 spec里面的
static struct lsm6ds3_fs_table {
u8 addr;
u8 mask;
struct lsm6ds3_fs_reg fs_avl[LSM6DS3_FS_LIST_NUM];
} lsm6ds3_fs_table[LSM6DS3_SENSORS_NUMB] = {
[LSM6DS3_ACCEL] = {
.addr = LSM6DS3_ACCEL_FS_ADDR,
.mask = LSM6DS3_ACCEL_FS_MASK,
.fs_avl[0] = { .gain = LSM6DS3_ACCEL_FS_2G_GAIN,
.value = LSM6DS3_ACCEL_FS_2G_VAL,
.urv = 2, },
.fs_avl[1] = { .gain = LSM6DS3_ACCEL_FS_4G_GAIN,
.value = LSM6DS3_ACCEL_FS_4G_VAL,
.urv = 4, },
.fs_avl[2] = { .gain = LSM6DS3_ACCEL_FS_8G_GAIN,
.value = LSM6DS3_ACCEL_FS_8G_VAL,
.urv = 8, },
.fs_avl[3] = { .gain = LSM6DS3_ACCEL_FS_16G_GAIN,
.value = LSM6DS3_ACCEL_FS_16G_VAL,
.urv = 16, },
},
[LSM6DS3_GYRO] = {
.addr = LSM6DS3_GYRO_FS_ADDR,
.mask = LSM6DS3_GYRO_FS_MASK,
.fs_avl[0] = { .gain = LSM6DS3_GYRO_FS_245_GAIN,
.value = LSM6DS3_GYRO_FS_245_VAL,
.urv = 245, },
.fs_avl[1] = { .gain = LSM6DS3_GYRO_FS_500_GAIN,
.value = LSM6DS3_GYRO_FS_500_VAL,
.urv = 500, },
.fs_avl[2] = { .gain = LSM6DS3_GYRO_FS_1000_GAIN,
.value = LSM6DS3_GYRO_FS_1000_VAL,
.urv = 1000, },
.fs_avl[3] = { .gain = LSM6DS3_GYRO_FS_2000_GAIN,
.value = LSM6DS3_GYRO_FS_2000_VAL,
.urv = 2000, },
}
};
对应spec 里面
2 初始化
int lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq, u16 bustype)
{
...
for (i = 0; i < LSM6DS3_SENSORS_NUMB; i++) {
sdata = &cdata->sensors[i];
sdata->enabled = false;
sdata->cdata = cdata;
sdata->sindex = i;
sdata->name = lsm6ds3_sensor_name[i].name;
//这里设置
// G_So =
8.75
mg/LSB
，
Angular rate sensitivity表示数据的每一位间隔的大小
//
G_ODR = 12.5 hz，
Angular rate output data rate
// poll_interval 为获取周期，即odr输出速率的倒数 1000 / sdata->c_odr单位为ms
if ((i == LSM6DS3_ACCEL) || (i == LSM6DS3_GYRO)) {
sdata->c_odr = lsm6ds3_odr_table.odr_avl[0].hz;
sdata->c_gain = lsm6ds3_fs_table[i].fs_avl[0].gain;
sdata->poll_interval = 1000 / sdata->c_odr;
}
if (i == LSM6DS3_STEP_COUNTER) {
sdata->c_odr = LSM6DS3_MIN_DURATION_MS;
}
//初始哈input 设备，跟sensor ic的寄存器设置无关
lsm6ds3_input_init(sdata, bustype,
lsm6ds3_sensor_name[i].description);
if (sysfs_create_group(&sdata->input_dev->dev.kobj,
&lsm6ds3_attribute_groups[i])) {
dev_err(cdata->dev,
"failed to create sysfs group for sensor %s",
sdata->name);
input_unregister_device(sdata->input_dev);
sdata->input_dev = NULL;
}
}
...
//相关寄存器，即接口，定时器的设置
err =
lsm6ds3_init_sensors(cdata);
}
3 数据的获取
static void poll_function_work(struct work_struct *input_work)
{
//通过i2c获取原始数据
xyz[0] = (s32)((s16)(data[0] | (data[1] << 8)));
xyz[1] = (s32)((s16)(data[2] | (data[3] << 8)));
xyz[2] = (s32)((s16)(data[4] | (data[5] << 8)));
//gain 是8750，
xyz 为int类型，不能用小数运算，所以将8.75 mdps/LSB * 1000
xyz[0] *= sdata->c_gain;
xyz[1] *= sdata->c_gain;
xyz[2] *= sdata->c_gain;
lsm6ds3_report_3axes_event(sdata, xyz, sdata->timestamp);
}
LSB的意思是最小有效位，为数字输出方式，一般我们可以用mdps/LSB来表示灵敏度。8.75 mdps/LSB，表示一个bit为8.75mdps。
dps ：Degree Per Second的缩写
°/S的意思。
就是一种角速度的单位
1 degree per second = 0.0174532925 radian per second
1 dps（°/s） = 0.0174532925 rad/s
所以这里获取到的数据乘以
灵敏度即为获取到的角度速度 value = xyz * 8.75 *
1000
mdps
二 Hal
hal 的转化数据定义在conf_LSM6DS3.h
#define DPS2RAD ((float)M_PI/180.0f)
#define G_SENSITIVITY (1.0f) //Already applied into the driver
#define CONVERT_GYRO (DPS2RAD * (G_SENSITIVITY / (1000.0f * 1000.0f)))
#define CONVERT_GYRO_X (CONVERT_GYRO)
#define CONVERT_GYRO_Y (CONVERT_GYRO)
#define CONVERT_GYRO_Z (CONVERT_GYRO)
GyroSensor::readEvents
(sensors_event_t* data, int count)
{
......
float value = (float) event->value;
if (event->code == EVENT_TYPE_GYRO_X) {
data_raw[0] = value * CONVERT_GYRO_X;
}
else if (event->code == EVENT_TYPE_GYRO_Y) {
data_raw[1] = value * CONVERT_GYRO_Y;
}
else if (event->code == EVENT_TYPE_GYRO_Z) {
data_raw[2] = value * CONVERT_GYRO_Z;
}
......
}
从driver获取到的数据为
value mdps, 这里将数据转换为 rad /s
即 value mdps = value / 1000 dps = (value / 1000 ) * pi / 180 rad/s
由于driver将 数值乘了1000，所以这里要再除以1000 才能得到实际的值 即
data_raw = (value / (1000 * 1000 )) * pi / 180
2 数据的进一步处理
int GyroSensor::readEvents(sensors_event_t* data, int count)
{
...
//对x, y, z 数据方向的转换， 这个可以根据ic的在整机中的位置进行调整
data_rot[0] = data_raw[0]*matrix_gyr[0][0] +
data_raw[1]*matrix_gyr[1][0] +
data_raw[2]*matrix_gyr[2][0];
data_rot[1] = data_raw[0]*matrix_gyr[0][1] +
data_raw[1]*matrix_gyr[1][1] +
data_raw[2]*matrix_gyr[2][1];
data_rot[2] = data_raw[0]*matrix_gyr[0][2] +
data_raw[1]*matrix_gyr[1][2] +
data_raw[2]*matrix_gyr[2][2];
...
//由于gryo存在零点漂移，需要对数据进行校正， 这部分未研究，默认是没有打开
...
//
data_rot 减去 gbias_out校准的值，即为上层获取到的数据
DecimationCount[Gyro]++;
if(mEnabled & (1<= DecimationBuffer[Gyro])) {
DecimationCount[Gyro] = 0;
mPendingEvent[Gyro].data[0] = data_rot[0] - gbias_out[0];
mPendingEvent[Gyro].data[1] = data_rot[1] - gbias_out[1];
mPendingEvent[Gyro].data[2] = data_rot[2] - gbias_out[2];
mPendingEvent[Gyro].timestamp = timestamp;
mPendingEvent[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
*data++ = mPendingEvent[Gyro];
}
3 数据校准
（1）STORE_CALIB_GYRO_ENABLED