1.初始化ICM-42686陀螺仪为1000dps和加速计为2g
/*
* GYRO_CONFIG0:Gyro Full scale and sampling rate config
* GYRO_FS_SEL[bit5-7],GYRO_ODR[bit0-3],page 58 of 105
* GYRO_FS_SEL:
0x00:+-4000dps(default); 0x02:+-2000dps; 0x40:+-1000dps;0x60:+-500dps
* GYRO_ODR:
0x00:Reserved; 0x01:32KHz; 0x02:16KHz; 0x03:8KHz; 0x04:4KHz;
0x05:2KHz; 0x06:1KHz(default); 0x07:200Hz; 0x08:100Hz;
*/
regmap_write(dev->regmap, GYRO_CONFIG0, 0x43); //+-1000dps//8KHz
/*
* ACCEL_CONFIG0
* ACCEL_FS_SEL[bit5-7] and ACCEL_ODR[bit0-3]) address=0x50,value default 0x06
* ACCEL_FS_SEL: 000: +-32g; 001:+-16g; 010:+-8g; 011:+-4g; 100:+-2g;
* ACCEL_ODR:0001:32KHz; 0010:16KHz; 0011: 8KHz; 0100: 4KHz;0101: 2KHz;0110: 1KHz;
* 0111: 200Hz; 1000: 1000Hz; 1001:50Hz; 1010:25Hz
*/
regmap_write(dev->regmap, ACCEL_CONFIG0, 0x83);/* full scale: +-2g,accel sampling rate: 8KHz*/
2.用量程和位宽换算IMU的值
/* accelerometer */
struct imu_acc
{
float ax; /* x axes */
float ay; /* y axes */
float az; /* z axes */
};
/* gyroscope */
struct imu_gry
{
float gx; /* x axes */
float gy; /* y axes */
float gz; /* z axes */
};
/*
* 相邻两个8位组成16位数据,然后转换为浮点数
* @data_h :高8位
* @data_l :低8位
*/
static float imu_data_to_float( const unsigned char data_h, const unsigned char data_l)
{
union
{
short data_short;
unsigned short data_ushort;
} us2f;
us2f.data_ushort = (((unsigned short)data_h) << 8) | data_l;
return (float)us2f.data_short;
}
/*
* IMU数据解析
* @pimu : imu 采集到的原始数据, pimu[0]为高8位,pimu[1]为低8位;
*/
void imu_data_resolving(unsigned char * pimu, struct imu_acc * acc, struct imu_gry * gry)
{
acc->ax = (imu_data_to_float(pimu[0], pimu[1])) * 4 / 65536.0;
acc->ay = (imu_data_to_float(pimu[2], pimu[3])) * 4 / 65536.0;
acc->az = (imu_data_to_float(pimu[4], pimu[5])) * 4 / 65536.0;
gry->gx = (imu_data_to_float(pimu[6], pimu[7])) * 2000 / 65536.0;
gry->gy = (imu_data_to_float(pimu[8], pimu[9])) * 2000 / 65536.0;
gry->gz = (imu_data_to_float(pimu[10], pimu[11])) * 2000 / 65536.0;
}