mpu6050.py

#一阶互补滤波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相关推荐

mpu6050 重力加速度_MPU6050抄底解读
背景介绍: 很多读者是从飞控相关推文开始关注这个公众号的,在飞控上 IMU 惯性传感器非常重要.Sugar 在线一对一过程中发现一些入手玩飞控的人直接扎到代码里忽视了这个重中之重.本篇从最基础的内容入 ...
【MM32F5270开发板试用】定制MicroPython及读取MPU6050数据到OLED1306
本篇文章来自极术社区与灵动组织的MM32F5270开发板评测活动,更多开发板试用活动请关注极术社区网站.作者:HonestQiao 前言这次有幸获得MM32F5270开发板的试用,非常幸运. 收到板 ...
【MicroPython ESP32】ssd1306驱动0.96“I2C屏幕+mpu6050图形控制
[MicroPython ESP32]ssd1306驱动0.96"I2C屏幕+mpu6050图形控制效果演示随着mpu6050模块的移动,oled屏幕矩形线框内的小方块也随对应的方向移动 ...
mpu6050算速度_MPU6050 抄底解读
背景介绍: 很多读者是从飞控相关推文开始关注这个公众号的,在飞控上 IMU 惯性传感器非常重要. Sugar 在线一对一过程中发现一些入手玩飞控的人直接扎到代码里忽视了这个重中之重. 本篇从最基础的内 ...
3.12 haas506开发教程-example-i2c多设备测试
haas506开发教程-example-i2c多设备测试 1.测试代码 2.测试结果 1.测试代码多设备i2c设备测试,注意设备的频率需要一致 main.py # coding=utf-8 impo ...
汉字的首拼音字母生成
生成助记码(取汉字的第一个字母) SET NOCOUNT ON GO IF EXISTS(SELECT name FROM sysobjects WHERE name = N'hzp ...
第44章 MPU6050传感器—姿态检测—零死角玩转STM32-F429系列
第44章 MPU6050传感器-姿态检测全套200集视频教程和1000页PDF教程请到秉火论坛下载:www.firebbs.cn 野火视频教程优酷观看网址:http://i.youku.co ...
android 陀螺仪滤波_Arduino MPU6050陀螺仪运用卡尔曼滤波姿态解算实验
Arduino MPU6050陀螺仪运用卡尔曼滤波姿态解算实验版权声明:本文为博主原创文章,未经博主允许不得转载. 2019年3月20日发布实例效果输出效果: 首先看看本例程XYZ轴的输出效果 ...
android 陀螺仪滤波_Arduino+mpu6050陀螺仪运用卡尔曼滤波姿态解算实验
MPU6050六轴陀螺仪作用于四轴无人机,平衡车,机器人等等的电子实作当中,用于姿态判断,掌握了可以发挥自己的想象完成更多更有趣的作品. 本例程输出XYZ的角度,正负90度. 运用卡尔曼滤波算法解算 ...

mpu6050.py

mpu6050.py相关推荐

最新文章

热门文章