最近要做一个物体移动报警的项目，刚好六轴传感器里面就有，选定型号为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