树莓派基础实验31:MPU6050陀螺仪加速度传感器实验

一、介绍

   MPU6050是世界上第一款也是唯一一款专为智能手机、平板电脑和可穿戴传感器的低功耗、低成本和高性能要求而设计的6轴运动跟踪设备。
   它集成了3轴MEMS陀螺仪,3轴MEMS加速度计,以及一个可扩展的数字运动处理器 DMP( DigitalMotion Processor),可用I2C接口连接一个第三方的数字传感器,比如磁力计。扩展之后就可以通过其 I2C或SPI接口输出一个9轴的信号( SPI接口仅在MPU-6000可用)。 MPU-60X0也可以通过其I2C接口连接非惯性的数字传感器,比如压力传感器。

  其它I2C总线实验可以查看前面的PCF8591相关实验,如:
  树莓派基础实验12:PCF8591模数转换器实验

二、组件

★Raspberry Pi主板*1

★树莓派电源*1

★40P软排线*1

★MPU6050陀螺仪加速度传感器模块*1

★面包板*1

★跳线若干

三、实验原理

树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第1张图片
MPU6050陀螺仪加速度传感器
树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第2张图片
MPU6050陀螺仪加速度传感器
树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第3张图片
MPU6050电路图

   MPU6050的工作原理比较复杂,需要深度学习最好是学习官方手册,本文只做简单介绍。
   详细资料可在官网下载最新的芯片手册:
https://invensense.tdk.com/products/motion-tracking/6-axis/mpu-6050/

树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第4张图片
官网下载芯片手册
树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第5张图片
MPU6050基本性能参数

1.传感器

   ①陀螺仪传感器:
   陀螺仪的原理就是,一个旋转物体的旋转轴所指的方向在不受外力影响时,是不会改变的。人们根据这个道理,用它来保持方向。然后用多种方法读取轴所指示的方向,并自动将数据信号传给控制系统。我们骑自行车其实也是利用了这个原理。轮子转得越快越不容易倒,因为车轴有一股保持水平的力量。

  现代陀螺仪可以精确地确定运动物体的方位的仪器,它在现代航空,航海,航天和国防工业中广泛使用的一种惯性导航仪器。传统的惯性陀螺仪主要部分有机械式的陀螺仪,而机械式的陀螺仪对工艺结构的要求很高。

  70年代提出了现代光纤陀螺仪的基本设想,到八十年代以后,光纤陀螺仪就得到了非常迅速的发展,激光谐振陀螺仪也有了很大的发展。光纤陀螺仪具有结构紧凑,灵敏度高,工作可靠。光纤陀螺仪在很多的领域已经完全取代了机械式的传统的陀螺仪,成为现代导航仪器中的关键部件。光纤陀螺仪同时发展的除了环式激光陀螺仪外。

树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第6张图片
陀螺仪

  MPU-60X0由三个独立的振动MEMS速率陀螺仪组成,可检测旋转角度X轴,Y轴和Z轴。 当陀螺仪围绕任何感应轴旋转时,科里奥利效应就会产生电容式传感器检测到的振动。 所得到的信号被放大,解调和滤波产生与角速度成比例的电压。 该电压使用单独的片内数字化16位模数转换器(ADC)对每个轴进行采样。 陀螺仪传感器可以全面范围的被数字编程为每秒±250,±500,±1000或±2000度(dps)。 ADC样本速率可以从每秒8,000个采样点编程到每秒3.9个采样点,并且可由用户选择低通滤波器可实现广泛的截止频率。

  ②加速度传感器:
  加速度传感器是一种能够测量加速度的传感器。通常由质量块、阻尼器、弹性元件、敏感元件和适调电路等部分组成。传感器在加速过程中,通过对质量块所受惯性力的测量,利用牛顿第二定律获得加速度值。根据传感器敏感元件的不同,常见的加速度传感器包括电容式、电感式、应变式、压阻式、压电式等。

  MPU-60X0的3轴加速度计为每个轴使用单独的检测质量。 加速沿着一条特定轴在相应的检测质量上引起位移,并且电容式传感器检测到该位移位移有差别。 MPU-60X0的架构降低了加速度计的敏感度制造变化以及热漂移。 当设备放置在平坦的表面上时,将进行测量在X和Y轴上为 0g,在Z轴上为+ 1g。 加速度计的比例因子在工厂进行校准并且在名义上与电源电压无关。 每个传感器都有一个专用的sigma-delta ADC来提供数字输出。 数字输出的满量程范围可以调整到±2g,±4g,±8g或±16g。

  其实说简单点,在mpu6050中我们用陀螺仪传感器测角度,用加速度传感器测加速度。

   MPU-60X0是全球首例9轴运动处理传感器。它集成了3轴MEMS陀螺仪, 3轴MEMS 加速度计,以及一个可扩展的数字运动处理器 DMP(DigitalMotion Processor),可用 I2C 接口连接一个第三方的数字传感器,比如磁力计。扩展之后就可以通过其 I2C 或 SPI 接口 输出一个 9 轴的信号(SPI 接口仅在 MPU-6000 可用)。MPU-60X0 也可以通过其 I2C 接口连接非惯性的数字传感器,比如压力传感器。

数字运动处理器(DMP):

  嵌入式数字运动处理器(DMP)位于MPU-60X0内部,可从主机处理器中卸载运动处理算法的运算。 DMP从加速度计,陀螺仪以及其他第三方传感器(如磁力计)获取数据,并处理数据。结果数据可以从DMP的寄存器中读取,或者可以在FIFO中缓冲。 DMP可以访问其中的一个MPU的外部引脚,可用于产生中断。

  DMP的目的是卸载主机处理器的时序要求和处理能力。通常,运动处理算法应该以高速运行,通常在200Hz左右,以提供低延迟的精确结果。即使应用程序以更低的速率更新,这也是必需的。例如,一个低功率的用户界面可能会以5Hz的速度更新,但运动处理仍然应该以200Hz运行。 DMP可以作为一种工具使用,以最大限度地降低功耗,简化定时,简化软件架构,并在主机处理器上节省宝贵的MIPS,以便在应用中使用。

2.数据分析

(1)加速度计

  下图标明了传感器的参考坐标系( XYZ组成右手系)以及 3个测量轴和旋转方向。旋转的正向可用右手螺旋定则判断

树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第7张图片
传感器的参考坐标系

三轴加速度计:

ACCEL_XOUT 16位二进制补码值。存储最近的X轴加速计测量值。
ACCEL_YOUT 16位二进制补码值。存储最近的Y轴加速度计测量值。
ACCEL_ZOUT 16位二进制补码值。存储最近的Z轴加速计测量值。

  三个加速度分量均以重力加速度 g 的倍数为单位,能够表示的加速度范围,即倍率可以统一设定,有4个可选倍率:±2g、±4g、±8g、±16g。初始化MPU6050设置加速度计输出的满量程范围为± 2g,加速度计每个 LSB 的灵敏度应为 16384 LSB/g。

满量程范围± 2g和灵敏度16384 LSB/g有啥关系?

  上面说了这三个加速度分量是16位的二进制补码值,且是有符号的。故而其输出范围 -32768~32767。((2^16)/2)

  32767/2 = 16384 即加速度计的灵敏度

那这个灵敏度又有啥用呢?我们拿一组数据来举个例子:

  A X: 03702 Y: 12456 Z: 06268 G X:-00023 Y:-00059 Z: 00005

  加速度计 X 轴获取原始数据位 03702,那么它对应的加速度数据是:03702/16384 = 0.23g.

  g为加速度的单位,重力加速度定义为1g, 等于9.8米每平方秒。

  具体的加速度公式:加速度数据 = 加速度轴原始数据 / 加速度灵敏度

  或者:加速度数据 = (加速度轴原始数据 / 32767) X 可选倍率(即±2g、±4g、±8g、±16g)

(2)陀螺仪

三轴陀螺仪:

GYRO_XOUT 16位二进制补码值。存储最新的X轴陀螺仪测量。
GYRO_YOUT 16位二进制补码值。存储最新的Y轴陀螺仪测量结果。
GYRO_ZOUT 16位二进制补码值。存储最新的Z轴陀螺仪测量结果。

  三个角速度分量均以“度/秒”为单位,能够表示的角速度范围,即倍率可统一设定,有4个可选倍率:±250°/s, ±500°/s, ±1000°/s, ±2000°/s。初始化MPU6050设置陀螺仪输出满量程范围为 ± 2000 °/s,陀螺仪每个 LSB 的灵敏度为 16.4 LSB/°/s。

满量程范围± 2000 °/s和灵敏度16.4 LSB/°/s有啥关系?
  上面说了这三个陀螺仪分量是16位的二进制补码值,且是有符号的。故而其输出范围 -32768~32767。((2^16)/2)

  32767/2000 = 16.4 即陀螺仪的灵敏度

那这个灵敏度又有啥用呢?我们用一组数据来举个例子:
  A X: 03702 Y: 12456 Z: 06268 G X:-00023 Y:-00059 Z: 00005

  陀螺仪 X 轴获取原始数据位 -00023,那么它对应的陀螺仪数据是:-00023/16.4 = -1.4°/s
  请注意,负号表示设备的旋转方向与传统的正方向相反。
  具体的陀螺仪公式:陀螺仪数据 = 陀螺仪轴原始数据/陀螺仪灵敏度

  或者:陀螺仪数据 = (陀螺仪轴原始数据 / 32767) X 可选倍率(即±250°/s, ±500°/s, ±1000°/s, ±2000°/s)

  MPU6050 是一款姿态传感器,使用它就是为了得到待测物体(如四轴、平衡小车) x、y、z 轴的倾角(俯仰角 Pitch、滚转角 Roll、偏航角 Yaw) 。我们通过 I2C 读取到 MPU6050 的六个数据(三轴加速度 AD 值、三轴角速度 AD 值)经过姿态融合后就可以得到 Pitch、Roll、Yaw 角。

  更多姿态融合等资料,可以参考MPU6050开发的帖子:
https://blog.csdn.net/qq_29350001/category_7303760.html

3.MPU6050寄存器

  这里说明几个重要的寄存器,详情查阅官方文档。

(1)寄存器25 - 采样速率分频器(SMPRT_DIV)
树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第8张图片
采样速率分频器
参数:

  SMPLRT_DIV 为8位无符号值。 采样率是通过将陀螺仪输出速率除以该值来确定的。

描述:

  该寄存器指定用于产生MPU-60X0采样率的陀螺仪输出速率的分频器。传感器寄存器输出,FIFO输出和DMP采样都基于采样率。采样率是通过将陀螺仪输出速率除以 SMPLRT_DIV 产生的:
采样率=陀螺仪输出速率/(1 + SMPLRT_DIV)
当DLPF禁用(DLPF_CFG = 0或7)时,陀螺仪输出速率= 8kHz,当DLPF使能时(见寄存器26)为1kHz。
  注意:加速度计输出速率是1kHz。 这意味着对于大于1kHz的采样率,同一个加速度计采样可能会不止一次输出到FIFO,DMP和传感器寄存器。

(2)寄存器26 - 配置(CONFIG)
配置

注:位7和位6保留

参数:

  EXT_SYNC_SET3位无符号值。 配置FSYNC引脚采样。
  DLPF_CFG3位无符号值。 配置DLPF设置

描述:

  该寄存器为陀螺仪和加速度计配置外部帧同步(FSYNC)引脚采样和数字低通滤波器(DLPF)设置。

  连接到FSYNC引脚的外部信号可以通过配置 EXT_SYNC_SET 进行采样。
  FSYNC 引脚的信号变化被锁存,以便捕获短闪光灯。 锁存的FSYNC信号将按照寄存器 25 中定义的采样速率进行采样。采样后,锁存器将复位为当前的 FSYNC 信号状态。
  根据下表,取样值将被报告在由 EXT_SYNC_SET 的值确定的传感器数据寄存器中的最低有效位的位置。

树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第9张图片

  DLPF由 DLPF_CFG 配置,加速度计和陀螺仪根据 DLPF_CFG 的值进行过滤,如下表所示。

树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第10张图片
(3)寄存器27 - 陀螺仪配置(GYRO_CONFIG)
陀螺仪配置

注:位2到位0被保留。

参数:

  XG_ST设置此位将导致X轴陀螺仪执行自检。
  YG_ST设置此位将使Y轴陀螺仪执行自检。
  ZG_ST 设置该位使Z轴陀螺仪执行自检。
  FS_SEL 2位无符号值。 选择陀螺仪的全量程范围。

描述:

  该寄存器用于触发陀螺仪自检并配置陀螺仪的满量程范围。
  陀螺仪自检允许用户测试机械和电气部分陀螺仪。每个陀螺仪轴的自检可通过控制该寄存器的XG_ST,YG_ST和ZG_ST位来激活。每个轴的自检可以独立进行,也可以同时进行。
  当自检被激活时,车载电子装置将启动适当的传感器。这种驱动将使传感器的检测质量移动一段相当于预先确定的科里奥利力的距离。这种检测质量位移导致传感器输出发生变化,这反映在输出信号中。输出信号用于观察自检响应。
自检响应定义如下:

  自检响应=启用自检的传感器输出 - 未启用自检的传感器输出

  每个陀螺仪轴的自检限制在MPU-6000 / MPU-6050产品规格文件。当自检的价值响应在产品规格的最小/最大范围内,零件已通过自​​检。当自检响应超过文档中指定的最小/最大值时,该部分被认为是自检失败。

  FS_SEL根据下表选择陀螺仪输出的满量程范围。

树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第11张图片
(4)寄存器28 - 加速度计配置(ACCEL_CONFIG)
加速度计配置
参数:

  XA_ST 当设置为1时,X轴加速度计执行自检。
  YA_ST当设置为1时,Y轴加速度计执行自检。
  ZA_ST设置为1时,Z轴加速计执行自检。
  AFS_SEL 2位无符号值。 选择加速度计的全量程范围。

描述:

  该寄存器用于触发加速度计自检并配置加速度计满量程范围。该寄存器还配置数字高通滤波器(DHPF)。
  加速度计自检允许用户测试加速度计的机械和电子部分。每个加速度计轴的自检可通过控制该寄存器的XA_ST,YA_ST和ZA_ST位来激活。每个轴的自检可以独立进行,也可以同时进行。
  当自检被激活时,车载电子装置将启动适当的传感器。这种致动模拟外力。被驱动的传感器又将产生相应的输出信号。输出信号用于观察自检响应。
自测响应定义如下:
  自检响应=启用自检的传感器输出 - 未启用自检的传感器输出
  MPU-6000 / MPU-6050产品规格文档的电气特性表中提供了每个加速度计轴的自检限制。当自检响应值在产品规格的最小/最大范围内时,该部件已通过自​​检。当自测响应超过文档中指定的最小/最大值时,该部分被认为是自检失败。

  AFS_SEL 根据下表选择加速度计输出的满量程范围。

树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第12张图片
(5)寄存器59到64 - 加速度计测量
树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第13张图片
加速度计测量
参数:

  ACCEL_XOUT 16位二进制补码值。存储最近的X轴加速计测量值。
  ACCEL_YOUT 16位二进制补码值。存储最近的Y轴加速度计测量值。
  ACCEL_ZOUT 16位二进制补码值。存储最近的Z轴加速计测量值。

描述:

  这些寄存器存储最新的加速度计测量结果。
  按照 寄存器25 中定义的采样率将加速度计测量值写入这些寄存器。
  加速度计测量寄存器以及温度测量寄存器,陀螺仪测量寄存器和外部传感器数据寄存器由两组寄存器组成:内部寄存器组和面向用户的读取寄存器组。
  加速度计传感器内部寄存器组内的数据总是以采样率更新。同时,只要串行接口空闲,面向用户的读取寄存器组就会复制内部寄存器组的数据值。这保证了传感器寄存器的突发读取将从相同的采样时刻读取测量结果。请注意,如果不使用突发读取,则用户负责通过检查数据就绪中断来确保一组单个字节的读取对应于单个采样时刻。
  每个16位加速度计的测量结果都在 ACCEL_FS(寄存器28)中定义了一个满量程。对于每个满量程设置,ACCEL_xOUT 中的每个 LSB 的加速度计灵敏度如下表所示。

树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第14张图片
(6)寄存器65和66 - 温度测量(TEMP_OUT_H和TEMP_OUT_L)
树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第15张图片
温度测量
参数:

  TEMP_OUT 16位有符号值。存储最近的温度传感器测量值。

描述:

  这些寄存器存储最新的温度传感器测量值。
  温度测量值按照 寄存器25 中定义的采样率写入这些寄存器。
  这些温度测量寄存器以及加速度计测量寄存器,陀螺仪测量寄存器和外部传感器数据寄存器由两组寄存器组成:内部寄存器组和面向用户的读取寄存器组。
  温度传感器内部寄存器组内的数据始终以采样率进行更新。
  同时,只要串行接口空闲,面向用户的读取寄存器组就会复制内部寄存器组的数据值。这保证了传感器寄存器的突发读取将从相同的采样时刻读取测量结果。请注意,如果不使用突发读取,则用户负责通过检查数据就绪中断来确保一组单个字节的读取对应于单个采样时刻。
  电气规格表(MPU-6000 / MPU-6050产品规格文档第6.4节)中提供了温度传感器的比例因子和偏移量。
  对于给定的寄存器值,温度(摄氏度)可以被计算为:
以℃为单位的温度=(TEMP_OUT寄存器值作为有符号数量)/ 340 + 36.53
  请注意,上述公式中的数学是十进制的。

(7)寄存器67至72 - 陀螺仪测量
树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第16张图片
陀螺仪测量
参数:

  GYRO_XOUT 16位二进制补码值。存储最新的X轴陀螺仪测量。
  GYRO_YOUT 16位二进制补码值。存储最新的Y轴陀螺仪测量结果。
  GYRO_ZOUT16位二进制补码值。存储最新的Z轴陀螺仪测量结果。

描述:

  这些寄存器存储最近的陀螺仪测量结果。
  陀螺仪测量值按寄存器25中定义的采样率写入这些寄存器。
这些陀螺仪测量寄存器以及加速度计测量寄存器,温度测量寄存器和外部传感器数据寄存器由两组寄存器组成:内部寄存器组和面向用户的读取寄存器组。
  陀螺仪传感器内部寄存器组内的数据总是以采样率更新。
  同时,只要串行接口空闲,面向用户的读取寄存器组就会复制内部寄存器组的数据值。这保证了传感器寄存器的突发读取将从相同的采样时刻读取测量结果。请注意,如果不使用突发读取,则用户负责通过检查数据就绪中断来确保一组单个字节的读取对应于单个采样时刻。
  每个16位陀螺仪测量具有在 FS_SEL(寄存器27)中定义的满量程。对于每个满量程设置,GYRO_xOUT中陀螺仪每个LSB的灵敏度如下表所示:

树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第17张图片
(8)寄存器107 - 电源管理1 (PWR_MGMT_1)
电源管理1

注:位4保留。

参数:

  DEVICE_RESET设置为1时,该位将所有内部寄存器复位为默认值。一旦复位完成,该位自动清零。每个寄存器的默认值可以在第3节找到。
  SLEEP当该位置1时,该位将MPU-60X0置于睡眠模式。
  CYCLE当该位设置为1且SLEEP被禁止时,MPU-60X0将循环。在睡眠模式和唤醒之间以LP_WAKE_CTRL(寄存器108)确定的速率从活动传感器获取单个样本数据。
  TEMP_DIS设置为1时,该位禁用温度传感器。
  CLKSEL 3位无符号值。 指定设备的时钟源。

描述:

  该寄存器允许用户配置电源模式和时钟源。它还提供了一些重置整个设备,以及一些禁用温度传感器。
  通过将 SLEEP 设置为1,MPU-60X0 可以进入低功耗睡眠模式。当 CYCLE 设置为1而睡眠模式被禁用时,MPU-60X0 将进入循环模式。在周期模式下,器件在休眠模式和唤醒之间循环,以由 LP_WAKE_CTRL(寄存器108)确定的速率从加速计获取单个采样。要配置唤醒频率,请使用 电源管理2寄存器(寄存器108)内的 LP_WAKE_CTRL。
  MPU-60X0 时钟源可选择内部 8MHz 振荡器,基于陀螺仪的时钟或外部时钟源。当选择内部 8MHz 振荡器或外部时钟源作为时钟源时,MPU-60X0 可以在陀螺仪禁用的低功耗模式下工作。
  上电时,MPU-60X0 时钟源默认为内部振荡器。但是,强烈建议将器件配置为使用其中一个陀螺仪(或外部时钟源)作为时钟参考,以提高稳定性。时钟源可以按照下表进行选择。

树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第18张图片
(9)寄存器117 - 我是谁(WHO_AM_I)
我是谁

注:位0和7保留。 (硬编码为0)

参数:

  WHO_AM_I包含MPU-60X0的6位I2C地址。
  位6:位1的上电复位值为110 100。

描述:

  **该寄存器用于验证设备的身份。 **WHO_AM_I的内容是MPU-60X0的7位I2C地址的高6位。 MPU-60X0的I2C地址的最低有效位由AD0引脚的值决定。 该寄存器不反映AD0引脚的值。
  该寄存器的默认值是0x68。

四、实验步骤

  第1步:连接电路。

树莓派 T型转接板 BMP180气压传感器
SCL SCL SCL
SDA SDA SDA
5V 5V VCC
GND GND GND
树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第19张图片
MPU6050实验电路图
树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第20张图片
MPU6050实验实物接线图

  第2步:PCF8591模块采用的是I2C(IIC)总线进行通信的,但是在树莓派的镜像中默认是关闭的,在使用该传感器的时候,我们必须首先允许IIC总线通信。

树莓派基础实验31:MPU6050陀螺仪加速度传感器实验_第21张图片
打开I2C总线通信

  第3步:查询MPU6050的地址。得出地址为0x68。

pi@raspberrypi:~ $ sudo i2cdetect -y 1
     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:          -- -- -- -- -- -- -- -- -- -- -- -- -- 
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- -- 
70: -- -- -- -- -- -- -- --                         

  第4步:编写控制程序。使用Python 2编写的程序比C++简洁许多,smbus函数请在之前的I2C文章中查阅。
  树莓派基础实验12:PCF8591模数转换器实验

  实验结果示例:

gyro_xout :  -546  scaled:  -5
gyro_yout :  -26  scaled:  -1
gyro_zout :  82  scaled:  0
accel_xout:  1132  scaled:  0.069091796875
accel_yout:  3912  scaled:  0.23876953125
accel_zout:  14324  scaled:  0.874267578125
x rotation:  15.2301539313
y rotation:  -4.35957842715

  控制程序:

#!/usr/bin/python

import smbus
import math
import time

# 电源管理寄存器地址
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c

def read_byte(adr):
    return bus.read_byte_data(address, adr)

def read_word(adr):
    high = bus.read_byte_data(address, adr)
    low = bus.read_byte_data(address, adr+1)
    val = (high << 8) + low
    return val

def read_word_2c(adr):
    val = read_word(adr)
    if (val >= 0x8000):
        return -((65535 - val) + 1)
    else:
        return val

def dist(a,b):
    return math.sqrt((a*a)+(b*b))
    #math.sqrt(x) 方法返回数字x的平方根。

def get_y_rotation(x,y,z):
    radians = math.atan2(x, dist(y,z))
    #math.atan2(y, x) 返回给定的 X 及 Y 坐标值的反正切值。
    return -math.degrees(radians)
    #math.degrees(x) 将弧度x转换为角度。

def get_x_rotation(x,y,z):
    radians = math.atan2(y, dist(x,z))
    return math.degrees(radians)


bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards
address = 0x68       # This is the address value read via the i2cdetect command

# Now wake the 6050 up as it starts in sleep mode
bus.write_byte_data(address, power_mgmt_1, 0)

while True:
    time.sleep(0.1)
    gyro_xout = read_word_2c(0x43)
    gyro_yout = read_word_2c(0x45)
    gyro_zout = read_word_2c(0x47)
    
    print ' '
    print "gyro_xout : ", gyro_xout, " scaled: ", (gyro_xout / 131) #倍率:±250°/s
    print "gyro_yout : ", gyro_yout, " scaled: ", (gyro_yout / 131)
    print "gyro_zout : ", gyro_zout, " scaled: ", (gyro_zout / 131)

    accel_xout = read_word_2c(0x3b)
    accel_yout = read_word_2c(0x3d)
    accel_zout = read_word_2c(0x3f)

    accel_xout_scaled = accel_xout / 16384.0 #倍率:±2g
    accel_yout_scaled = accel_yout / 16384.0
    accel_zout_scaled = accel_zout / 16384.0

    print "accel_xout: ", accel_xout, " scaled: ", accel_xout_scaled
    print "accel_yout: ", accel_yout, " scaled: ", accel_yout_scaled
    print "accel_zout: ", accel_zout, " scaled: ", accel_zout_scaled

    print "x rotation: " , get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
    print "y rotation: " , get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)

    time.sleep(0.5)


你可能感兴趣的:(树莓派基础实验31:MPU6050陀螺仪加速度传感器实验)