mpu6050.py

#一阶互补滤波k=0.02,ACC+-2g，Gyro+-2000°/s，初始校正6轴值需要10s，采样频率50Hz=20ms读取一次,角速度传感器带宽20Hz(约25Hz)
#-*- encoding: utf-8 -*-
import time, math

MPU6050_ADDRESS_AD0_LOW  = 0x68	 # 1.检查通讯117
MPU6050_ADDRESS_AD0_HIGH = 0x69
MPU6050_REG_POWER_MGMT_1 = 0x6B  # 2.复位+100ms,107;3.唤醒MPU6050和配置时钟源pll 107
MPU6050_REG_POWER_MGMT_2 = 0x6C  # 8.激活传感器6轴108
MPU6050_REG_INTERRUPT_EN = 0x38  # 禁用中断(自己添加未使用中断功能)	56
MPU6050_REG_READ_SENSORS = 0x3B  # 9.获取传感器数据59-72
MPU6050_REG_CONFIG       = 0x1A  # 6.配置带宽和输出频率26
MPU6050_REG_SMPRT_DIV    = 0x19  # 7.设置采样率25
MPU6050_REG_CONFIG_GYRO  = 0x1B  # 5.设置陀螺仪量程范围27
MPU6050_REG_CONFIG_ACC   = 0x1C  # 4.设置加速度计量程28

class MPU6050():

    def __init__(self, i2c):
        self.captures = bytearray(14)  # 14个字节数组
        self.AngleX = 0#Roll	角度表示方法：欧拉角、四元数、轴角（用一个向量+角度表示）
        self.AngleY = 0#Pitch
        self.AngleZ = 0#Yaw
        self.i2c = i2c
        if self.detect():
           self.reset()
           self.config()
           self.calibration()
           self.temps = time.ticks_ms()#获得ms计数值
	
    def detect(self):
        detect_mpu6050 = False
        i2c_peripheriques = self.i2c.scan()
        for i2c_peripherique in i2c_peripheriques:
            if (i2c_peripherique == MPU6050_ADDRESS_AD0_LOW):
                self.address = MPU6050_ADDRESS_AD0_LOW
                detect_mpu6050 = True
            if (i2c_peripherique == MPU6050_ADDRESS_AD0_HIGH):
                self.address = MPU6050_ADDRESS_AD0_HIGH
                detect_mpu6050 = True
        return detect_mpu6050
		
	#bit7=1复位+100ms
    def reset(self):
        self.data = bytearray(2)
        self.data[0] = MPU6050_REG_POWER_MGMT_1#0x6B
        self.data[1] = 1 << 7  # 10000000 : reset mpu6050
        self.i2c.writeto(self.address, self.data)
        time.sleep(0.100)  # 延迟100ms
	
    def config(self):
		#配置时钟源：
		#Bit6=0工作模式，非睡眠模式；bit3=0温度sensor可用；bit321时钟源，默认内部8M RC晶振，精度不高，
		#将器件配置为使用陀螺仪XYZ其中一个（或外部时钟源）作为时钟参考，以提高稳定性。
        self.data = bytearray(2)
        self.data[0] = MPU6050_REG_POWER_MGMT_1#0x6B
        self.data[1] = 1  #配置外部时钟源PLL,(PLL WITH X AXIS GYROSCOPE REFERENCE)
        self.i2c.writeto(self.address, self.data)
		
		#配置加速度计量程:bit4 bit3：0(+-2g),1(+—4g),2(+-8g),3(+-16g)
        self.data[0] = MPU6050_REG_CONFIG_ACC#0x1C
        self.data[1] = 0 #+/- 2g(65536/4=16384LSB/g) 
        self.i2c.writeto(self.address, self.data)
		
		#配置陀螺仪量程:bit4 bit3：0(+-250°/s),1(+—500°/s),2(+-1000°/s),3(+-2000°/s)
        self.data[0] = MPU6050_REG_CONFIG_GYRO#0x1B
        self.data[1] = 24  #+/- 2000deg/s(65536/4000=16.4LSB/(°/s)) 
        self.i2c.writeto(self.address, self.data)
				
		#配置数字低通滤波器DLPF[2:0]：配置陀螺仪输出频率
		'''
		DLPF_CFG[2:0] 		加速度传感器 Fs=1Khz 			角速度传感器 （陀螺仪） 
							 带宽(Hz) 	延迟（ms） 			带宽(Hz) 	延迟（ms） 	Fs(Khz) 
		000 				  260 			0 				  256 		   0.98 	  8 
		001 				  184 			2.0 			  188 	 	   1.9 	  	  1 
		010 				  94 			3.0 			  98 		   2.8 		  1 
		011 				  44 			4.9 			  42 		   4.8 		  1 
		100 				  21 			8.5 			  20 		   8.3 	      1 
		101 				  10 			13.8 			  10 		   13.4 	  1 
		110 				  5 			19.0 			  5 		   18.6 	  1 
		111 						保留 							保留 			  8 

		'''
		self.data[0] = MPU6050_REG_CONFIG#0x1A 
        self.data[1] = 100 #角速度传感器带宽20Hz，约25Hz，一般设置角速度传感器的带宽为其采样率的一半
        self.i2c.writeto(self.address, self.data)
		
		#bit7-bit0
		#配置采样频率=陀螺仪输出频率/(1+SMPLRT_DIV)=1000Hz / 20 = 50Hz , 0.02s更新一次6轴数据寄存器，主函数是20ms读取一次
        self.data[0] = MPU6050_REG_SMPRT_DIV#0x19
        self.data[1] = 19 
        self.i2c.writeto(self.address, self.data)
		
		#激活6轴为非待机模式bit5-bit0
        self.data[0] = MPU6050_REG_POWER_MGMT_2#0x6C
        self.data[1] = 0  
        self.i2c.writeto(self.address, self.data)		

    def sensor_captures(self):	
		if self.detect():
			self.i2c.readfrom_mem_into(self.address,
										MPU6050_REG_READ_SENSORS,
										self.captures)
			self.acc()
			self.temp()
			self.gyro()
			self.angle()
		else:
			print('mpu6050 communication error')
			
	#读取3轴加速度0x3B-0x40(59-64)
    def acc(self):
        self.accX_high_byte = self.captures[0]
        self.accX_low_byte = self.captures[1]
        self.accX = self.bytes_to_int(self.accX_high_byte, self.accX_low_byte)
		#初始化是第99（从0开始，相当于第100次）次-100次平均值，调用是第101次-100次平均值，才开始显示角度。
        self.accX_calibre = self.accX - self.accX_calibration

        self.accY_high_byte = self.captures[2]
        self.accY_low_byte = self.captures[3]
        self.accY = self.bytes_to_int(self.accY_high_byte, self.accY_low_byte)
        self.accY_calibre = self.accY - self.accY_calibration

        self.accZ_high_byte = self.captures[4]
        self.accZ_low_byte = self.captures[5]
        self.accZ = self.bytes_to_int(self.accZ_high_byte, self.accZ_low_byte)
        self.accZ_calibre = self.accZ - self.accZ_calibration
	
	#读取温度值0x41-0x42(65-66)
    def temp(self):
        self.temp_high_byte = self.captures[6]
        self.temp_low_byte = self.captures[7]
        self.temperature = self.bytes_to_int(self.temp_high_byte,
                                             self.temp_low_byte)
        self.temperature = self.temperature / 340.00 + 36.53#MPU6050参考公式:Temperature=regval/340+36.53
		
	#读取3轴角速度0x43-0x48(67-72)
    def gyro(self):
        self.gyroX_high_byte = self.captures[8] 
        self.gyroX_low_byte = self.captures[9] 
        self.gyroX = self.bytes_to_int(self.gyroX_high_byte, self.gyroX_low_byte)#100ms陀螺仪输出一次值
        self.gyroX_calibre = self.gyroX - self.gyroX_calibration
		
        self.gyroY_high_byte = self.captures[10] 
        self.gyroY_low_byte = self.captures[11]
        self.gyroY = self.bytes_to_int(self.gyroY_high_byte, self.gyroY_low_byte)
        self.gyroY_calibre = self.gyroY - self.gyroY_calibration

        self.gyroZ_high_byte = self.captures[12] 
        self.gyroZ_low_byte = self.captures[13] 
        self.gyroZ = self.bytes_to_int(self.gyroZ_high_byte, self.gyroZ_low_byte)
        self.gyroZ_calibre = self.gyroZ - self.gyroZ_calibration
		
	#移位合并
    def bytes_to_int(self, firstbyte, secondbyte):
		#读取值为正数时(-128~127)
        if not firstbyte & 0x80:#为0时直接合并
            return (firstbyte << 8 | secondbyte)
		#读取值为负数时，异或255，合并，再加1，变为正数，再加负号;
		return - (((firstbyte ^ 255) << 8) | (secondbyte ^ 255) + 1)  
		
	#读取100次求出平均值，第一次初始化时求出陀螺仪6轴初始值，不动的情况下，理论初始值等于0
    def calibration(self):
        i = 0
		#实例方法里的变量在实例属性里初始化，那变量的值与实例属性一样可保持；
        self.gyroX_calibration = 0
        self.gyroY_calibration = 0
        self.gyroZ_calibration = 0
        self.accX_calibration = 0
        self.accY_calibration = 0
        self.accZ_calibration = 0
        while i < 100:
			#在self.address从属设备中的MPU6050_REG_READ_SENSORS地址读取14个字节到self.captures;
            self.i2c.readfrom_mem_into(self.address, MPU6050_REG_READ_SENSORS, self.captures)
            self.acc()
            self.accX_calibration += self.accX
            self.accY_calibration += self.accY
            self.accZ_calibration += self.accZ
			self.gyro()
            self.gyroX_calibration += self.gyroX
            self.gyroY_calibration += self.gyroY
            self.gyroZ_calibration += self.gyroZ
            i += 1
            time.sleep(0.100)#每100ms读取一次值，读取100次10s求平均值，第一次初始化时求出陀螺仪6轴初始平均值
        self.accX_calibration /= 100
        self.accY_calibration /= 100
        self.accZ_calibration /= 100
        self.gyroX_calibration /= 100
        self.gyroY_calibration /= 100
        self.gyroZ_calibration /= 100
		
    def angle(self):
        self.temps_precedent = self.temps#第一次是初始化完成后开始时间，也即开始调用时间；
        self.temps = time.ticks_ms()
        self.interval = time.ticks_diff(self.temps, self.temps_precedent) / 1000#dt两次采样时间间隔，
		
		self.aX = self.accX_calibre / 16384.0  # +/-2g,32767/2=16384LSB/g
        self.aY = self.accY_calibre / 16384.0
        self.aZ = self.accZ_calibre / 16384.0	
		
		#DMP输出的姿态：Pitch(Y)范围是-90~90，Roll(X)范围-180~180，Yaw(Z)范围-180~180
		#atan(y / x):-PI/2<θ<PI/2;atan2(y, x):-PI<θ=PI （直接计算公式）
		self.accX_angle=atan2(self.aY,self.aZ)*57.3#angel_X=roll=atan2(Accel_Y,Accel_Z)*180/PI
		self.accY_angle=atan2(self.aX,self.aZ)*57.3#angel_Y=pitch=atan2(Accel_X,Accel_Z)*180/PI
		
		'''
		self.accX_angle = math.degrees (math.atan(self.aY / math.sqrt((self.aX * self.aX) + (self.aZ * self.aZ))))#accX_Roll
        self.accY_angle = math.degrees (math.atan(-1 * self.aX / math.sqrt((self.aY * self.aY) + (self.aZ * self.aZ))))#accY_Pitch	
        self.accZ_angle = math.degrees (math.atan(math.sqrt((self.aX * self.aX) + (self.aY * self.aY)) / self.aZ ))
        '''
		self.gyroX_angle = self.gyroX_calibre / 16.4  #  +/- 2000deg/s，65536/4000=16.4LSB/(°/s)
        self.gyroY_angle = self.gyroY_calibre / 16.4
        self.gyroZ_angle = self.gyroZ_calibre / 16.4
		#self.gyroX_angle=self.gyroX_angle + self.gyroX_angle * self.interval(正规积分累加值计算）
		
		#一阶互补滤波		
		self.AngleX = 0.98 * (self.AngleX + self.gyroX_angle * self.intervalle) + 0.02 * self.accX_angle#Roll
        self.AngleY = 0.98 * (self.AngleY + self.gyroY_angle * self.intervalle) + 0.02 * self.accY_angle#Pitch
        self.AngleZ = 0.98 * (self.AngleZ + self.gyroZ_angle * self.intervalle) + 0.02 * self.accZ_angle#Yaw
		'''
		不管是四元数解算还是MPU6050读出的Z轴角速度积分解算，静止不动时Yaw会很缓慢的增大，
		静止时Z轴角速度输出并不干净，不是绝对干净的0，所以在积分作用下会缓慢变化。	
        '''