大家好,又见面了,我是你们的朋友全栈君。
MPU9250 内部包括 3 轴陀螺仪、3 轴加速度计和 3 轴磁力计,这3个功能输出都是 16 位的数字量; 可以通过常用的数据总线( IIC) 接口和单片机进行数据交互,传输速率 400 kHz /s。陀螺仪的角速度测量范围±2000(° /s),具有良好的动态响应特性。加速度计的测量范围最大为±16g( g 为重力加速度),静态测量精度高。磁力计采用高灵度霍尔型传感器进行数据采集,磁感应强度测量范围为±4800μT,可用于对偏航角的辅助测量。
MPU9250 自带的数字运动处理器DMP硬件加速引擎,可以整合九轴传感器数据,向应用端输出完整的 9 轴融合演算数据。 有了 DMP,我们可以使用运动处理库非常方便的实现姿态解算,降低了运动处理运算对操作系统的负荷,同时大大降低了开发难度。
三轴陀螺仪
MPU9250陀螺仪是由三个独立检测X, Y, Z轴的MEMS组成。检测每个轴的转动(一但某个轴发生变化,相应的电容传感器会发生相应的变化,产生的信号被放大,调解,滤波,最后产生个与角速率成正比的电压,然后将每一个轴的电压转换成16位的数据。ADC的采样速率也是可编程的,从每秒3.9-8000个。
三轴加速度
MPU9250的三轴加速度也是单独分开测量的。根据每个轴上的电容来测量轴的偏差度。结构上降低了各种因素造成的测量偏差。加速度计的校准是根据工厂的标准来设定的,电源电压也许和你用的不一样。每一个传感器都有专门的ADC来提供数字性的输出。
三轴磁力计
三轴磁力计采用高精度的霍尔效应传感器,通过驱动电路,信号放大和计算电路来处理信号来采集地磁场在X, Y, Z轴上的电磁强度。
IIC通信
MPU9250的电路图连接如下
我们使用IIC让MPU9250和单片机通信,并且输出获取到的传感器值。
IIC数据总线是由两根通信线组成,必要的是包含一个主控制器件和多个从控制器件,不同的从器件通过地址与主器件通信。
实际使用中,一般是单片机作为主机,其它器件作为从机,单片机先向器件发送信息表示要读取数据,之后转变传输方向,器件发送数据到单片机。
在通信时,IIC通信线只有只有两根,数据线SDA的高低电平传输2进制的数据,时钟线SCL通过方波信号提供时钟节拍。在时钟的高电平周期内,SDA线上的数据必须保持稳定,数据线仅可以在时钟SCL为低电平时改变。
IIC的通信数据包含起始信号应答信号和结束信号等。
其中起始信号产生的条件是当SCL为高电平的时候,SDA线上由高到低的跳变被定义为起始条件。结束信号产生的条件是SCL为高电平的时候,SDA线上由低到高的跳变被定义为停止条件。
从机应答主机所需要的时钟仍是主机提供的,应答出现在每一次主机完成8个数据位传输后紧跟着的时钟周期,低电平0表示应答,1表示非应答。
关于通信协议具体的内容,可以网上找找详细介绍。作为嵌入式软件工程师,这些常用协议一定要去仔细研究一下,只有理解了协议才能在程序上理清协议实现的逻辑。
程序
由于使用IIC通信协议控制MPU9250,我们需要实现IIC协议。
代码参考正点原子的源码,封装好的函数用起来比较高效。
void IIC_Init(void)
{
GPIO_InitTypeDef GPIO_Initure;
__HAL_RCC_GPIOA_CLK_ENABLE(); //使能GPIOB时钟
//PH4,5初始化设置
GPIO_Initure.Pin=GPIO_PIN_4|GPIO_PIN_5;
GPIO_Initure.Mode=GPIO_MODE_OUTPUT_PP; //推挽输出
GPIO_Initure.Pull=GPIO_PULLUP; //上拉
GPIO_Initure.Speed=GPIO_SPEED_FAST; //快速
HAL_GPIO_Init(GPIOA,&GPIO_Initure);
IIC_SDA=1;
IIC_SCL=1;
}
//产生IIC起始信号
void IIC_Start(void)
{
SDA_OUT(); //sda线输出
IIC_SDA=1;
IIC_SCL=1;
delay_us(4);
IIC_SDA=0;//START:when CLK is high,DATA change form high to low
delay_us(4);
IIC_SCL=0;//钳住I2C总线,准备发送或接收数据
}
//产生IIC停止信号
void IIC_Stop(void)
{
SDA_OUT();//sda线输出
IIC_SCL=0;
IIC_SDA=0;//STOP:when CLK is high DATA change form low to high
delay_us(4);
IIC_SCL=1;
delay_us(4);
IIC_SDA=1;//发送I2C总线结束信号
}
//等待应答信号到来
//返回值:1,接收应答失败
// 0,接收应答成功
u8 IIC_Wait_Ack(void)
{
u8 ucErrTime=0;
SDA_IN(); //SDA设置为输入
IIC_SDA=1;delay_us(1);
IIC_SCL=1;delay_us(1);
while(READ_SDA)
{
ucErrTime++;
if(ucErrTime>250)
{
IIC_Stop();
return 1;
}
}
IIC_SCL=0;//时钟输出0
return 0;
}
//产生ACK应答
void IIC_Ack(void)
{
IIC_SCL=0;
SDA_OUT();
IIC_SDA=0;
delay_us(2);
IIC_SCL=1;
delay_us(2);
IIC_SCL=0;
}
//不产生ACK应答
void IIC_NAck(void)
{
IIC_SCL=0;
SDA_OUT();
IIC_SDA=1;
delay_us(2);
IIC_SCL=1;
delay_us(2);
IIC_SCL=0;
}
//IIC发送一个字节
//返回从机有无应答
//1,有应答
//0,无应答
void IIC_Send_Byte(u8 txd)
{
u8 t;
SDA_OUT();
IIC_SCL=0;//拉低时钟开始数据传输
for(t=0;t<8;t++)
{
IIC_SDA=(txd&0x80)>>7;
txd<<=1;
delay_us(2); //对TEA5767这三个延时都是必须的
IIC_SCL=1;
delay_us(2);
IIC_SCL=0;
delay_us(2);
}
}
//读1个字节,ack=1时,发送ACK,ack=0,发送nACK
u8 IIC_Read_Byte(unsigned char ack)
{
unsigned char i,receive=0;
SDA_IN();//SDA设置为输入
for(i=0;i<8;i++ )
{
IIC_SCL=0;
delay_us(2);
IIC_SCL=1;
receive<<=1;
if(READ_SDA)receive++;
delay_us(1);
}
if (!ack)
IIC_NAck();//发送nACK
else
IIC_Ack(); //发送ACK
return receive;
}
u8 MPU9250_Init(void)
{
u8 res=0;
IIC_Init(); //初始化IIC总线
MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X80);//复位MPU9250
delay_ms(100); //延时100ms
MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X00);//唤醒MPU9250
MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
MPU_Set_Accel_Fsr(0); //加速度传感器,±2g
MPU_Set_Rate(50); //设置采样率50Hz
MPU_Write_Byte(MPU9250_ADDR,MPU_INT_EN_REG,0X00); //关闭所有中断
MPU_Write_Byte(MPU9250_ADDR,MPU_USER_CTRL_REG,0X00);//I2C主模式关闭
MPU_Write_Byte(MPU9250_ADDR,MPU_FIFO_EN_REG,0X00); //关闭FIFO
MPU_Write_Byte(MPU9250_ADDR,MPU_INTBP_CFG_REG,0X82);//INT引脚低电平有效,开启bypass模式,可以直接读取磁力计
res=MPU_Read_Byte(MPU9250_ADDR,MPU_DEVICE_ID_REG); //读取MPU6500的ID
if(res==MPU6500_ID) //器件ID正确
{
MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考
MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作
MPU_Set_Rate(50); //设置采样率为50Hz
}else return 1;
res=MPU_Read_Byte(AK8963_ADDR,MAG_WIA); //读取AK8963 ID
if(res==AK8963_ID)
{
MPU_Write_Byte(AK8963_ADDR,MAG_CNTL1,0X11); //设置AK8963为单次测量模式
}else return 1;
return 0;
}
//设置MPU9250陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
return MPU_Write_Byte(MPU9250_ADDR,MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
}
//设置MPU9250加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
return MPU_Write_Byte(MPU9250_ADDR,MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
}
//设置MPU9250的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_LPF(u16 lpf)
{
u8 data=0;
if(lpf>=188)data=1;
else if(lpf>=98)data=2;
else if(lpf>=42)data=3;
else if(lpf>=20)data=4;
else if(lpf>=10)data=5;
else data=6;
return MPU_Write_Byte(MPU9250_ADDR,MPU_CFG_REG,data);//设置数字低通滤波器
}
//设置MPU9250的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
// 其他,设置失败
u8 MPU_Set_Rate(u16 rate)
{
u8 data;
if(rate>1000)rate=1000;
if(rate<4)rate=4;
data=1000/rate-1;
data=MPU_Write_Byte(MPU9250_ADDR,MPU_SAMPLE_RATE_REG,data); //设置数字低通滤波器
return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU9250_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
if(res==0)
{
*gx=((u16)buf[0]<<8)|buf[1];
*gy=((u16)buf[2]<<8)|buf[3];
*gz=((u16)buf[4]<<8)|buf[5];
}
return res;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
u8 buf[6],res;
res=MPU_Read_Len(MPU9250_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
if(res==0)
{
*ax=((u16)buf[0]<<8)|buf[1];
*ay=((u16)buf[2]<<8)|buf[3];
*az=((u16)buf[4]<<8)|buf[5];
}
return res;;
}
//得到磁力计值(原始值)
//mx,my,mz:磁力计x,y,z轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
u8 MPU_Get_Magnetometer(short *mx,short *my,short *mz)
{
u8 buf[6],res;
res=MPU_Read_Len(AK8963_ADDR,MAG_XOUT_L,6,buf);
if(res==0)
{
*mx=((u16)buf[1]<<8)|buf[0];
*my=((u16)buf[3]<<8)|buf[2];
*mz=((u16)buf[5]<<8)|buf[4];
}
MPU_Write_Byte(AK8963_ADDR,MAG_CNTL1,0X11); //AK8963每次读完以后都需要重新设置为单次测量模式
return res;;
}
//IIC连续写
//addr:器件地址
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
// 其他,错误代码
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
u8 i;
IIC_Start();
IIC_Send_Byte((addr<<1)|0); //发送器件地址+写命令
if(IIC_Wait_Ack()) //等待应答
{
IIC_Stop();
return 1;
}
IIC_Send_Byte(reg); //写寄存器地址
IIC_Wait_Ack(); //等待应答
for(i=0;i<len;i++)
{
IIC_Send_Byte(buf[i]); //发送数据
if(IIC_Wait_Ack()) //等待ACK
{
IIC_Stop();
return 1;
}
}
IIC_Stop();
return 0;
}
//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
// 其他,错误代码
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
IIC_Start();
IIC_Send_Byte((addr<<1)|0); //发送器件地址+写命令
if(IIC_Wait_Ack()) //等待应答
{
IIC_Stop();
return 1;
}
IIC_Send_Byte(reg); //写寄存器地址
IIC_Wait_Ack(); //等待应答
IIC_Start();
IIC_Send_Byte((addr<<1)|1); //发送器件地址+读命令
IIC_Wait_Ack(); //等待应答
while(len)
{
if(len==1)*buf=IIC_Read_Byte(0);//读数据,发送nACK
else *buf=IIC_Read_Byte(1); //读数据,发送ACK
len--;
buf++;
}
IIC_Stop(); //产生一个停止条件
return 0;
}
//IIC写一个字节
//devaddr:器件IIC地址
//reg:寄存器地址
//data:数据
//返回值:0,正常
// 其他,错误代码
u8 MPU_Write_Byte(u8 addr,u8 reg,u8 data)
{
IIC_Start();
IIC_Send_Byte((addr<<1)|0); //发送器件地址+写命令
if(IIC_Wait_Ack()) //等待应答
{
IIC_Stop();
return 1;
}
IIC_Send_Byte(reg); //写寄存器地址
IIC_Wait_Ack(); //等待应答
IIC_Send_Byte(data); //发送数据
if(IIC_Wait_Ack()) //等待ACK
{
IIC_Stop();
return 1;
}
IIC_Stop();
return 0;
}
//IIC读一个字节
//reg:寄存器地址
//返回值:读到的数据
u8 MPU_Read_Byte(u8 addr,u8 reg)
{
u8 res;
IIC_Start();
IIC_Send_Byte((addr<<1)|0); //发送器件地址+写命令
IIC_Wait_Ack(); //等待应答
IIC_Send_Byte(reg); //写寄存器地址
IIC_Wait_Ack(); //等待应答
IIC_Start();
IIC_Send_Byte((addr<<1)|1); //发送器件地址+读命令
IIC_Wait_Ack(); //等待应答
res=IIC_Read_Byte(0); //读数据,发送nACK
IIC_Stop(); //产生一个停止条件
return res;
}
下面的代码是控制MPU9250的关键代码,是针对芯片本身的。也是主要代码。
首先初始化DMP
u8 mpu_dmp_init(void)
{
u8 res=0;
struct int_param_s int_param;
unsigned char accel_fsr;
unsigned short gyro_rate, gyro_fsr;
unsigned short compass_fsr;
IIC_Init(); //初始化IIC总线
if(mpu_init(&int_param)==0) //初始化MPU9250
{
res=inv_init_mpl(); //初始化MPL
if(res)return 1;
inv_enable_quaternion();
inv_enable_9x_sensor_fusion();
inv_enable_fast_nomot();
inv_enable_gyro_tc();
inv_enable_vector_compass_cal();
inv_enable_magnetic_disturbance();
inv_enable_eMPL_outputs();
res=inv_start_mpl(); //开启MPL
if(res)return 1;
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL|INV_XYZ_COMPASS);//设置所需要的传感器
if(res)return 2;
res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); //设置FIFO
if(res)return 3;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率
if(res)return 4;
res=mpu_set_compass_sample_rate(1000/COMPASS_READ_MS); //设置磁力计采样率
if(res)return 5;
mpu_get_sample_rate(&gyro_rate);
mpu_get_gyro_fsr(&gyro_fsr);
mpu_get_accel_fsr(&accel_fsr);
mpu_get_compass_fsr(&compass_fsr);
inv_set_gyro_sample_rate(1000000L/gyro_rate);
inv_set_accel_sample_rate(1000000L/gyro_rate);
inv_set_compass_sample_rate(COMPASS_READ_MS*1000L);
inv_set_gyro_orientation_and_scale(
inv_orientation_matrix_to_scalar(gyro_orientation),(long)gyro_fsr<<15);
inv_set_accel_orientation_and_scale(
inv_orientation_matrix_to_scalar(gyro_orientation),(long)accel_fsr<<15);
inv_set_compass_orientation_and_scale(
inv_orientation_matrix_to_scalar(comp_orientation),(long)compass_fsr<<15);
res=dmp_load_motion_driver_firmware(); //加载dmp固件
if(res)return 6;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
if(res)return 7;
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
DMP_FEATURE_GYRO_CAL);
if(res)return 8;
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz)
if(res)return 9;
res=run_self_test(); //自检
if(res)return 10;
res=mpu_set_dmp_state(1); //使能DMP
if(res)return 11;
}
return 0;
}
获取mp1的数据
u8 mpu_mpl_get_data(float *pitch,float *roll,float *yaw)
{
unsigned long sensor_timestamp,timestamp;
short gyro[3], accel_short[3],compass_short[3],sensors;
unsigned char more;
long compass[3],accel[3],quat[4],temperature;
long data[9];
int8_t accuracy;
if(dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors,&more))return 1;
if(sensors&INV_XYZ_GYRO)
{
inv_build_gyro(gyro,sensor_timestamp); //把新数据发送给MPL
mpu_get_temperature(&temperature,&sensor_timestamp);
inv_build_temp(temperature,sensor_timestamp); //把温度值发给MPL,只有陀螺仪需要温度值
}
if(sensors&INV_XYZ_ACCEL)
{
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel,0,sensor_timestamp); //把加速度值发给MPL
}
if (!mpu_get_compass_reg(compass_short, &sensor_timestamp))
{
compass[0]=(long)compass_short[0];
compass[1]=(long)compass_short[1];
compass[2]=(long)compass_short[2];
inv_build_compass(compass,0,sensor_timestamp); //把磁力计值发给MPL
}
inv_execute_on_data();
inv_get_sensor_type_euler(data,&accuracy,×tamp);
*roll = (data[0]/q16);
*pitch = -(data[1]/q16);
*yaw = -data[2] / q16;
return 0;
}
其中,数据从队列中读取代码如下
int dmp_read_fifo(short *gyro, short *accel, long *quat,
unsigned long *timestamp, short *sensors, unsigned char *more)
{
unsigned char fifo_data[MAX_PACKET_LENGTH];
unsigned char ii = 0;
sensors[0] = 0;
if (mpu_read_fifo_stream(dmp.packet_length, fifo_data, more))
return -1;
if (dmp.feature_mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT)) {
#ifdef FIFO_CORRUPTION_CHECK
long quat_q14[4], quat_mag_sq;
#endif
quat[0] = ((long)fifo_data[0] << 24) | ((long)fifo_data[1] << 16) |
((long)fifo_data[2] << 8) | fifo_data[3];
quat[1] = ((long)fifo_data[4] << 24) | ((long)fifo_data[5] << 16) |
((long)fifo_data[6] << 8) | fifo_data[7];
quat[2] = ((long)fifo_data[8] << 24) | ((long)fifo_data[9] << 16) |
((long)fifo_data[10] << 8) | fifo_data[11];
quat[3] = ((long)fifo_data[12] << 24) | ((long)fifo_data[13] << 16) |
((long)fifo_data[14] << 8) | fifo_data[15];
ii += 16;
#ifdef FIFO_CORRUPTION_CHECK
quat_q14[0] = quat[0] >> 16;
quat_q14[1] = quat[1] >> 16;
quat_q14[2] = quat[2] >> 16;
quat_q14[3] = quat[3] >> 16;
quat_mag_sq = quat_q14[0] * quat_q14[0] + quat_q14[1] * quat_q14[1] +
quat_q14[2] * quat_q14[2] + quat_q14[3] * quat_q14[3];
if ((quat_mag_sq < QUAT_MAG_SQ_MIN) ||
(quat_mag_sq > QUAT_MAG_SQ_MAX)) {
/@@* Quaternion is outside of the acceptable threshold. */
mpu_reset_fifo();
sensors[0] = 0;
return -1;
}
sensors[0] |= INV_WXYZ_QUAT;
#endif
}
if (dmp.feature_mask & DMP_FEATURE_SEND_RAW_ACCEL) {
accel[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
accel[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
accel[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
ii += 6;
sensors[0] |= INV_XYZ_ACCEL;
}
if (dmp.feature_mask & DMP_FEATURE_SEND_ANY_GYRO) {
gyro[0] = ((short)fifo_data[ii+0] << 8) | fifo_data[ii+1];
gyro[1] = ((short)fifo_data[ii+2] << 8) | fifo_data[ii+3];
gyro[2] = ((short)fifo_data[ii+4] << 8) | fifo_data[ii+5];
ii += 6;
sensors[0] |= INV_XYZ_GYRO;
}
if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
decode_gesture(fifo_data + ii);
get_ms(timestamp);
return 0;
}
结果
发布者:全栈程序员-用户IM,转载请注明出处:https://javaforall.cn/126652.html原文链接:https://javaforall.cn
【正版授权,激活自己账号】: Jetbrains全家桶Ide使用,1年售后保障,每天仅需1毛
【官方授权 正版激活】: 官方授权 正版激活 支持Jetbrains家族下所有IDE 使用个人JB账号...