【MEMS传感器】基于MPU6500/ICM206xx系列的移动监测

最近要做一个物体移动报警的项目，刚好六轴传感器里面就有，选定型号为ICM20602，但是手边只有MPU6500，对比了下寄存器，这两个寄存器地址基本相同。

在MPU6500的手册里面有专门的移动操作指南如下图：

代码贴出来分享下：

---

#ifndef MPU6500_H_
#define MPU6500_H_

#include "include.h"
#define MPU6500 1

#if MPU6500

#define  MPU6500_WHO_AM_I  0x70
/* Hardware registers needed by driver. */
struct pmu6500_sensor_reg {
	unsigned char who_am_i;
	unsigned char rate_div;
	unsigned char lpf;
	//    unsigned char prod_id;
	unsigned char xg_offs_usr;
	unsigned char yg_offs_usr;
	unsigned char zg_offs_usr;
	unsigned char user_ctrl;
	unsigned char fifo_en;
	unsigned char gyro_cfg;
	unsigned char accel_cfg;
	unsigned char accel_cfg2;
	unsigned char lp_mode_cfg;
	unsigned char motion_thr;
	//    unsigned char motion_dur;
	unsigned char fifo_count_h;
	unsigned char fifo_r_w;
	unsigned char raw_gyro;
	unsigned char raw_accel;
	unsigned char temp;
	unsigned char int_enable;
	//    unsigned char dmp_int_status;
	unsigned char int_status;
	unsigned char accel_intel;
	unsigned char pwr_mgmt_1;
	unsigned char pwr_mgmt_2;
	unsigned char int_pin_cfg;
	//    unsigned char mem_r_w;
	unsigned char xa_offset;
	unsigned char ya_offset;
	unsigned char za_offset;
	//    unsigned char i2c_mst;
	//    unsigned char bank_sel;
	//    unsigned char mem_start_addr;
	unsigned char prgm_start_h;
	//    unsigned char fifo_wm_th;
	unsigned char signal_reset;
	//    unsigned char st_gyro;
	unsigned char st_accel;
};
#endif





void MPU6500_Init(void);
void MPU6500_read_ID(void);
void MPU6500_Restart(void);
void MPU6500_CHIP_startup(void);
void MPU6500_CHIP_POWERON(void);
void MPU6500_CHIP_POWEROFF(void);
void MPU6500_ACC_POWERON(void);
void MPU6500_ACC_POWEROFF(void);
void MPU6500_GYRO_POWERON(void);
void MPU6500_GYRO_POWEROFF(void);
void MPU6500_ACC_ENABLE(void);
void MPU6500_GYRO_ENABLE(void);


////////////////////////////////////////////////////////////////////////
//                 FIFO Setting                                     ////
////////////////////////////////////////////////////////////////////////



////////////////////////////////////////////////////////////////////////
//                Lowpower Motion Detect                            ////
////////////////////////////////////////////////////////////////////////

void PPU_6500_WAKEON_motion(void);
////////////////////////////////////////////////////////////////////////
//               GET	   DATA                                     ////
////////////////////////////////////////////////////////////////////////
void MPU6500_GetACC(void);



#endif /* MPU6500_H_ */

---

#include "MPU6500.h"
//////////////////////////////////////////////////////////////////////////
#if MPU6500
uint8_t read_MPU6500_ID=0xff;
const struct pmu6500_sensor_reg mpu6500_reg = {
	.who_am_i       = 0x75,
	.rate_div       = 0x19,
	.lpf            = 0x1A,
	//    .prod_id        = 0x0C,
	.xg_offs_usr    = 0x13,
	.yg_offs_usr    = 0x15,
	.zg_offs_usr    = 0x17,
	.user_ctrl      = 0x6A,
	.fifo_en        = 0x23,
	.gyro_cfg       = 0x1B,
	.accel_cfg      = 0x1C,
	.accel_cfg2     = 0x1D,
	.lp_mode_cfg    = 0x1E,
	.motion_thr     = 0x1F,
	//    .motion_dur     = 0x20,
	.fifo_count_h   = 0x72,
	.fifo_r_w       = 0x74,
	.raw_gyro       = 0x43,
	.raw_accel      = 0x3B,
	.temp           = 0x41,
	.int_enable     = 0x38,
	//    .dmp_int_status = 0x39,
	.int_status     = 0x3A,
	.accel_intel    = 0x69,
	.pwr_mgmt_1     = 0x6B,
	.pwr_mgmt_2     = 0x6C,
	.int_pin_cfg    = 0x37,
	//    .mem_r_w        = 0x6F,
	.xa_offset      = 0x77,
	.ya_offset      = 0x7A,
	.za_offset      = 0x7D,
	//    .i2c_mst        = 0x24,
	//    .bank_sel       = 0x6D,
	//    .mem_start_addr = 0x6E,
	.prgm_start_h   = 0x70,
	//    .fifo_wm_th     = 0x61,
	.signal_reset   = 0x68,
	//    .st_gyro        = 0x00,
	.st_accel       = 0x0D
};
#endif
//////////////////////////////////////////////////////////////////////////

#if  MPU6500

void MPU6500_read_ID(void)		// read_ID
{
		uint8_t temp_who_am_i,num;
		for (num=0;num<4;num++)
		{
			SPI_master_read_register(mpu6500_reg.who_am_i,1,&temp_who_am_i);	//  MPU6500 who_am_i address is 0x75,the only read defult value is 0x70;
			if(temp_who_am_i == MPU6500_WHO_AM_I)
			{
				printf("Register = 0x%x,who-am-i  =0x%02x\r\n",mpu6500_reg.who_am_i,temp_who_am_i);break;
			}
			else if(num==3 && temp_who_am_i != 0x12)
			{
				read_MPU6500_ID = false;
				printf("NO MPU6500\r\n");
			}
		}
}

void MPU6500_Init(void)
{
	uint8_t temp;
	MPU6500_Restart();
	delay_ms(100);
	temp=0x3F;
	SPI_master_write_register(mpu6500_reg.pwr_mgmt_2, 1, &temp);	    //disable ACC and GYRO by default.
	delay_ms(10);
	MPU6500_read_ID();
	if (read_MPU6500_ID != 0)
	{
		MPU6500_CHIP_startup();
	}
		
}

void MPU6500_Restart(void)
{
	uint8_t temp;
	temp=0x80;
	SPI_master_write_register(mpu6500_reg.pwr_mgmt_1, 1, &temp);		//reset
}

void MPU6500_CHIP_startup(void)
{
	uint8_t temp,ch;
	MPU6500_CHIP_POWERON();
	MPU6500_ACC_POWERON();
	MPU6500_GYRO_POWERON();
	temp=0x27;				//SMPLRT_DIV=9	SAMPLE_RATE = INTERNAL_SAMPLE_RATE / (1 + SMPLRT_DIV)	Where INTERNAL_SAMPLE_RATE = 1kHz
	SPI_master_write_register(0x19, 1, &temp);		//ODR设置为25HZ
	temp=0x05;				//Gro  3db BW,截止频率10HZ
	SPI_master_write_register(0x1a, 1, &temp);		//Gro滤波器设置
	SPI_master_read_register(0x1b,1,&ch);			//读取gro配置
	ch &= 0xE4;				//250dps
	SPI_master_write_register(0x1b, 1,&ch);			//陀螺量程设置
	temp=0x18;				//[4:3]		±2g (00), ±4g (01), ±8g (10), ±16g (11)
	SPI_master_write_register(0x1c, 1, &temp);		//加表量程设置
	temp=0x05;				//Acc  BW=10HZ
	SPI_master_write_register(0x1d, 1, &temp);		//Acc滤波器设置
	temp=0x00;
	SPI_master_write_register(0x1e, 1, &temp);		//关闭低功耗模式，
	temp=0x00;
	SPI_master_write_register(0x69, 1, &temp);		//Acc Wake-on-Motion设置 ---disable
	delay_ms(10);
}

void MPU6500_CHIP_POWERON(void)
{
	uint8_t temp;
	temp=0x01;	//achieve full gyroscope performance
	SPI_master_write_register(mpu6500_reg.pwr_mgmt_1, 1, &temp);      //00000001 PWR_MGMT_1 [6]SLEEP	When set to 1, the chip is set to sleep mode.
	//[2:0]	CLKSEL[2:0] 1	Auto selects the best available clock source  PLL if ready, else use the Internal oscillator
	delay_ms(10);              //for entering sleep take effect
}

void MPU6500_CHIP_POWEROFF(void)
{
	uint8_t temp;
	temp=0x41;
	SPI_master_write_register(mpu6500_reg.pwr_mgmt_1, 1, &temp);      //00000001 PWR_MGMT_1       [6]	SLEEP	When set to 1, the chip is set to sleep mode.
	//                          [2:0]	CLKSEL[2:0] 1	Auto selects the best available clock source  PLL if ready, else use the Internal oscillator
	delay_ms(10);                  //for exiting sleep take effect
}

void MPU6500_ACC_POWERON(void)
{
	uint8_t ch;
	SPI_master_read_register(mpu6500_reg.pwr_mgmt_2,1,&ch);
	ch &= 0xC7;
	SPI_master_write_register(mpu6500_reg.pwr_mgmt_2, 1,&ch);        //set [5:3] as 0; 1  XYZ accelerometer is disabled. 0  XYZ accelerometer is on.
}
void MPU6500_ACC_POWEROFF(void)
{
	uint8_t ch;
	SPI_master_read_register(mpu6500_reg.pwr_mgmt_2,1,&ch);
	ch |= 0x38;
	SPI_master_write_register(mpu6500_reg.pwr_mgmt_2, 1,&ch);        //set [5:3] as 0; 1  XYZ accelerometer is disabled. 0  XYZ accelerometer is on.
}

void MPU6500_GYRO_POWERON(void)
{
	uint8_t ch;
	SPI_master_read_register(mpu6500_reg.pwr_mgmt_1,1,&ch);	//set gyro clk
	ch |=0x01;
	SPI_master_write_register(mpu6500_reg.pwr_mgmt_1, 1,&ch);
	delay_ms(1);

	SPI_master_read_register(mpu6500_reg.pwr_mgmt_2,1,&ch);
	ch &= 0xF8;
	SPI_master_write_register(mpu6500_reg.pwr_mgmt_2, 1,&ch);        //set [2:0] as 0; 1  XYZ gyroscope is disabled. 0  XYZ gyroscope is on.
	delay_ms(80);                 //for Start up of the gyro
}
void MPU6500_GYRO_POWEROFF(void)
{
	uint8_t ch;
	SPI_master_read_register(mpu6500_reg.pwr_mgmt_1,1,&ch);	//set gyro clk
	ch &= 0xFE;
	SPI_master_write_register(mpu6500_reg.pwr_mgmt_1, 1,&ch);
	delay_ms(1);

	//??? need to double check here whether UI or OIS is still using GYRO , don't do following untill no one use gyro anymore
	SPI_master_read_register(mpu6500_reg.pwr_mgmt_2, 1, &ch);
	ch = 0x80 | 0x07;
	SPI_master_write_register(mpu6500_reg.pwr_mgmt_2, 1, &ch);        //set [2:0] as 0; 1  XYZ gyroscope is disabled. 0  XYZ gyroscope is on.
	delay_ms(200);                 //for discharge
}

void MPU6500_ACC_ENABLE(void)
{
	uint8_t temp;
	temp=0x01;
	//量程，8g
	SPI_master_write_register(mpu6500_reg.accel_cfg, 1, &temp);//00010000 ACCEL_CONFIG [4:3]	AFS_SEL[1:0]	UI Path Accel Full Scale Select:10 = ∮8g
	//量程，16g
	//  write_1B(reg.accel_cfg,0x18);
	//速度，200Hz
	temp=0x04;
	SPI_master_write_register(mpu6500_reg.rate_div, 1, &temp);//00000100 SMPLRT_DIV SMPLRT_DIV[7:0]   200Hz = Divides the internal sample rate (see register CONFIG) to generate the sample rate that controls sensor data output rate, FIFO sample rate.  NOTE: This register is only effective when FCHOICE_B register bits are 2ˇb00, and (0 < DLPF_CFG < 7). This is the update rate of the sensor register: SAMPLE_RATE = INTERNAL_SAMPLE_RATE / (1 + SMPLRT_DIV) Where INTERNAL_SAMPLE_RATE = 1kHz
	//滤波器截止频率和噪声带宽
	temp=0x02;
	SPI_master_write_register(mpu6500_reg.accel_cfg2, 1,  &temp);//00100000 ACCEL_CONFIG2       [5:4]  DEC2_CFG[1:0] 0
	//                          [3]    ACCEL_FCHOICE_B    Used to bypass DLPF as shown in the table below
	//                          [2:0]    A_DLPF_CFG    100Hz Bend Pass UI path accelerometer low pass filter setting as shown in the table below  0 ODR 1/(1+SMPLRT_DIV)  : Filter BW 218(Hz)    Filter Delay 1.88(ms)
}
void MPU6500_GYRO_ENABLE(void)
{
	uint8_t  temp;
	//量程，2000dps
	temp=0x0c;
	SPI_master_write_register(mpu6500_reg.gyro_cfg, 1, &temp);//00001100 GYROSCOPE CONFIGURATION [4:2] UI Path Gyro Full Scale Select 011 = ∮2000dps
	//滤波器截止频率和噪声带宽
	temp=0x01;
	SPI_master_write_register(mpu6500_reg.lpf, 1, &temp);//00000001 CONFIGURATION [2:0]	DLPF_CFG[2:0] Filter BW 184(Hz) Filter Delay 2.9 (ms)
	//速度
	temp=0x04;
	SPI_master_write_register(mpu6500_reg.rate_div, 1, &temp);//00000100 SMPLRT_DIV SMPLRT_DIV[7:0]   200Hz = Divides the internal sample rate (see register CONFIG) to generate the sample rate that controls sensor data output rate, FIFO sample rate.  NOTE: This register is only effective when FCHOICE_B register bits are 2ˇb00, and (0 < DLPF_CFG < 7). This is the update rate of the sensor register: SAMPLE_RATE = INTERNAL_SAMPLE_RATE / (1 + SMPLRT_DIV) Where INTERNAL_SAMPLE_RATE = 1kHz
	//是否配置Gyro低功耗模式
	temp=0x00;
	SPI_master_write_register(mpu6500_reg.lp_mode_cfg, 1, &temp);//00000000 LP_MODE_CONFIG [7]	GYRO_CYCLE	When set to ˉ1ˇ low-power gyroscope mode is enabled.  Default setting is ˉ0ˇ
}

void MPU6500_GetACC(void)
{
	uint8_t buf[6]={0};
	uint8_t temp,value1,value2;
	int16_t acc[3]={0};
	float value;
	SPI_master_read_register(mpu6500_reg.raw_accel,6,&buf);
	acc[0] = (int16_t)(((int16_t)buf[0]) << 8 | buf[1]);		//x
	acc[1] = (int16_t)(((int16_t)buf[2]) << 8 | buf[3]);		//y
	acc[2] = (int16_t)(((int16_t)buf[4]) << 8 | buf[5]);		//z
	printf("ACC,%d,%d,%d\r\n",acc[0],acc[1],acc[2]);
}
void PPU_6500_WAKEON_motion(void)
{
	uint8_t temp,ch;
	//Make Sure Accel is running:
	temp=0x01;	
	SPI_master_write_register(mpu6500_reg.pwr_mgmt_1, 1, &temp);
	temp=0x07;
	SPI_master_write_register(mpu6500_reg.pwr_mgmt_2, 1, &temp);
	//Set Accel LPF setting to 184 Hz Bandwidth:
	temp=0x01;
	SPI_master_write_register(mpu6500_reg.accel_cfg2, 1, &temp);
	//Enable Motion Interrupt:
	temp=0x40;
	SPI_master_write_register(mpu6500_reg.int_enable, 1, &temp);
	//Enable Accel Hardware Intelligence:
	temp=0xc0;
	SPI_master_write_register(mpu6500_reg.accel_intel, 1, &temp);
	//Set Motion Threshold:
	temp=0x32;			//8bit 0-255   range:0~1020mg  resolution:4LSB/mg
	SPI_master_write_register(0x1f, 1, &temp);
	//Set Frequency of Wake-up:
	temp=0x0b;
	SPI_master_write_register(0x1e, 1, &temp);
	//Enable Cycle Mode (Accel Low Power Mode):
	SPI_master_read_register(mpu6500_reg.pwr_mgmt_1,1,&ch);
	ch &= 0x20;
	SPI_master_write_register(mpu6500_reg.pwr_mgmt_1, 1, &ch);
}


#endif