#一階互補濾波k=0.02,ACC+-2g，Gyro+-2000°/s，初始校正6軸值需要10s，采樣頻率50Hz=20ms讀取一次,角速度傳感器帶寬20Hz(約25Hz) #-*- encoding: utf-8 -*- import time, mathMPU6050_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.設置加速度計量程28class MPU6050():def __init__(self, i2c):self.captures = bytearray(14) # 14個字節數組self.AngleX = 0#Roll 角度表示方法：歐拉角、四元數、軸角（用一個向量+角度表示）self.AngleY = 0#Pitchself.AngleZ = 0#Yawself.i2c = i2cif self.detect():self.reset()self.config()self.calibration()self.temps = time.ticks_ms()#獲得ms計數值def detect(self):detect_mpu6050 = Falsei2c_peripheriques = self.i2c.scan()for i2c_peripherique in i2c_peripheriques:if (i2c_peripherique == MPU6050_ADDRESS_AD0_LOW):self.address = MPU6050_ADDRESS_AD0_LOWdetect_mpu6050 = Trueif (i2c_peripherique == MPU6050_ADDRESS_AD0_HIGH):self.address = MPU6050_ADDRESS_AD0_HIGHdetect_mpu6050 = Truereturn detect_mpu6050#bit7=1復位+100msdef reset(self):self.data = bytearray(2)self.data[0] = MPU6050_REG_POWER_MGMT_1#0x6Bself.data[1] = 1 << 7 # 10000000 : reset mpu6050self.i2c.writeto(self.address, self.data)time.sleep(0.100) # 延遲100msdef config(self):#配置時鐘源：#Bit6=0工作模式，非睡眠模式；bit3=0溫度sensor可用；bit321時鐘源，默認內部8M RC晶振，精度不高，#將器件配置為使用陀螺儀XYZ其中一個（或外部時鐘源）作為時鐘參考，以提高穩定性。self.data = bytearray(2)self.data[0] = MPU6050_REG_POWER_MGMT_1#0x6Bself.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#0x1Cself.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#0x1Bself.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#0x19self.data[1] = 19 self.i2c.writeto(self.address, self.data)#激活6軸為非待機模式bit5-bit0self.data[0] = MPU6050_REG_POWER_MGMT_2#0x6Cself.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_calibrationself.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_calibrationself.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_calibrationself.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_calibrationself.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軸初始值，不動的情況下，理論初始值等于0def calibration(self):i = 0#實例方法里的變量在實例屬性里初始化，那變量的值與實例屬性一樣可保持；self.gyroX_calibration = 0self.gyroY_calibration = 0self.gyroZ_calibration = 0self.accX_calibration = 0self.accY_calibration = 0self.accZ_calibration = 0while 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.accXself.accY_calibration += self.accYself.accZ_calibration += self.accZself.gyro()self.gyroX_calibration += self.gyroXself.gyroY_calibration += self.gyroYself.gyroZ_calibration += self.gyroZi += 1time.sleep(0.100)#每100ms讀取一次值，讀取100次10s求平均值，第一次初始化時求出陀螺儀6軸初始平均值self.accX_calibration /= 100self.accY_calibration /= 100self.accZ_calibration /= 100self.gyroX_calibration /= 100self.gyroY_calibration /= 100self.gyroZ_calibration /= 100def 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/gself.aY = self.accY_calibre / 16384.0self.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/PIself.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_Rollself.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.4self.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#Rollself.AngleY = 0.98 * (self.AngleY + self.gyroY_angle * self.intervalle) + 0.02 * self.accY_angle#Pitchself.AngleZ = 0.98 * (self.AngleZ + self.gyroZ_angle * self.intervalle) + 0.02 * self.accZ_angle#Yaw'''不管是四元數解算還是MPU6050讀出的Z軸角速度積分解算，靜止不動時Yaw會很緩慢的增大，靜止時Z軸角速度輸出并不干凈，不是絕對干凈的0，所以在積分作用下會緩慢變化。 '''

總結

以上是生活随笔為你收集整理的mpu6050.py的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。