kriswiner/MPU6050库翻译
地址
https://github.com/kriswiner
1. 简介
-
演示了MPU-6050的基本功能，包括初始化，加速度计和陀螺仪校准，睡眠模式功能以及参数化寄存器地址。添加了显示功能，以允许显示在板载显示器上。不使用DMP。我们只想获取加速度，温度和陀螺仪读数。
-
在3.3V 8 MHz Pro Mini和Teensy 3.1上运行。
-
添加了基于Madgwick的开源传感器融合算法的四元数滤波器。MPU-6050缺少用于绝对方向估计的磁性矢量，这与MPU-9150或LSM9DS0一样。该算法允许估算四元数和相对方向，从而允许偏航，俯仰和侧倾的输出受陀螺仪偏置漂移的影响而产生偏航漂移。尽管在传感器融合算法中包括了陀螺仪偏置漂移校正组件，但偏航漂移约为每分钟半度或更小，这还不错。原则上，Yaw不可能仅凭一个绝对参考（重力下降）进行估算，但是该算法在较短时间范围内以良好的稳定性估算相对的Yaw表现出色。
-
我已经添加使用mbed编译器编译了代码，以使用STM32MF401 ARM处理器和MPU-6050运行6轴传感器融合。为什么这是必要或可取的？
- 滤波器在3.3 V 8 MHz Pro Mini AVR（Arduino）微控制器上以200 Hz更新速率运行。
- 该滤波器在3.3 V 96 MHz Teensy 3.1 ARM微控制器上以3200 Hz更新速率运行。
- 该滤波器在3.3 V 84 MHz STM32F401 ARM微控制器上以5500 Hz更新速率运行。
- 人们不需要超过1000 Hz的传感器融合滤波器更新速率就能获得6轴和9轴运动传感器的最佳性能，但是功耗与微控制器时钟速度成正比。如果可以在较低的时钟速度下达到1000 Hz的速率，则可以降低功耗。这是便携式（可穿戴）运动感应和运动控制设备的关键考虑因素。
假设传感器融合滤波器的更新速率与时钟速度成线性比例（一个很好的假设）。然后，要获得Teensy的1000 Hz更新速率，仅需要96 / 3.2 = 30 MHz时钟速度。实际上，在24 MHz时钟速度下，Teensy 3.1的更新速率为1365 Hz。对于STM32F401，时钟速度仅需要84 / 5.5 = 16 MHz即可达到1000 Hz的传感器融合滤波器更新速率。实际上，时钟速度不是任意的，而是必须满足某些约束。可能无法以如此低的速度运行STM32F401。到目前为止，我仅在84和42 MHz下运行它；在42 MHz时，我得到的传感器融合滤波器更新速率约为4400 Hz。看来，与Teensy 3.1相比，STM32F401所需的功率不到一半，以实现相同水平的传感器融合性能。这意味着所有其他条件都是平等的（永远不会！），使用STM32F401作为微控制器的可穿戴设备在需要更换电池或充电之前的使用寿命可能会延长两倍。这是一个主要的商业优势，为什么STM32 M4 Cortex系列处理器如此有趣。
2. MPU6050.h
#include "Arduino.h"
#include "Wire.h"
// Using the GY-521 breakout board, I set ADO to 0 by grounding through a 4k7 resistor
// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
#define ADO 0
#if ADO
#define MPU6050_ADDRESS 0x69 // Device address when ADO = 1
#else
#define MPU6050_ADDRESS 0x68 // Device address when ADO = 0
#endif
// Set initial input parameters
enum Ascale {
AFS_2G = 0,
AFS_4G,
AFS_8G,
AFS_16G
};
enum Gscale {
GFS_250DPS = 0,
GFS_500DPS,
GFS_1000DPS,
GFS_2000DPS
};
#define XGOFFS_TC 0x00 // Bit 7 PWR_MODE, bits 6:1 XG_OFFS_TC, bit 0 OTP_BNK_VLD
#define YGOFFS_TC 0x01
#define ZGOFFS_TC 0x02
#define X_FINE_GAIN 0x03 // [7:0] fine gain
#define Y_FINE_GAIN 0x04
#define Z_FINE_GAIN 0x05
#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer
#define XA_OFFSET_L_TC 0x07
#define YA_OFFSET_H 0x08
#define YA_OFFSET_L_TC 0x09
#define ZA_OFFSET_H 0x0A
#define ZA_OFFSET_L_TC 0x0B
#define SELF_TEST_X 0x0D
#define SELF_TEST_Y 0x0E
#define SELF_TEST_Z 0x0F
#define SELF_TEST_A 0x10
#define XG_OFFS_USRH 0x13 // User-defined trim values for gyroscope; supported in MPU-6050?
#define XG_OFFS_USRL 0x14
#define YG_OFFS_USRH 0x15
#define YG_OFFS_USRL 0x16
#define ZG_OFFS_USRH 0x17
#define ZG_OFFS_USRL 0x18
#define SMPLRT_DIV 0x19
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
#define FF_THR 0x1D // Free-fall
#define FF_DUR 0x1E // Free-fall
#define MOT_THR 0x1F // Motion detection threshold bits [7:0]
#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0]
#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
#define FIFO_EN 0x23
#define I2C_MST_CTRL 0x24
#define I2C_SLV0_ADDR 0x25
#define I2C_SLV0_REG 0x26
#define I2C_SLV0_CTRL 0x27
#define I2C_SLV1_ADDR 0x28
#define I2C_SLV1_REG 0x29
#define I2C_SLV1_CTRL 0x2A
#define I2C_SLV2_ADDR 0x2B
#define I2C_SLV2_REG 0x2C
#define I2C_SLV2_CTRL 0x2D
#define I2C_SLV3_ADDR 0x2E
#define I2C_SLV3_REG 0x2F
#define I2C_SLV3_CTRL 0x30
#define I2C_SLV4_ADDR 0x31
#define I2C_SLV4_REG 0x32
#define I2C_SLV4_DO 0x33
#define I2C_SLV4_CTRL 0x34
#define I2C_SLV4_DI 0x35
#define I2C_MST_STATUS 0x36
#define INT_PIN_CFG 0x37
#define INT_ENABLE 0x38
#define DMP_INT_STATUS 0x39 // Check DMP interrupt
#define INT_STATUS 0x3A
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define EXT_SENS_DATA_00 0x49
#define EXT_SENS_DATA_01 0x4A
#define EXT_SENS_DATA_02 0x4B
#define EXT_SENS_DATA_03 0x4C
#define EXT_SENS_DATA_04 0x4D
#define EXT_SENS_DATA_05 0x4E
#define EXT_SENS_DATA_06 0x4F
#define EXT_SENS_DATA_07 0x50
#define EXT_SENS_DATA_08 0x51
#define EXT_SENS_DATA_09 0x52
#define EXT_SENS_DATA_10 0x53
#define EXT_SENS_DATA_11 0x54
#define EXT_SENS_DATA_12 0x55
#define EXT_SENS_DATA_13 0x56
#define EXT_SENS_DATA_14 0x57
#define EXT_SENS_DATA_15 0x58
#define EXT_SENS_DATA_16 0x59
#define EXT_SENS_DATA_17 0x5A
#define EXT_SENS_DATA_18 0x5B
#define EXT_SENS_DATA_19 0x5C
#define EXT_SENS_DATA_20 0x5D
#define EXT_SENS_DATA_21 0x5E
#define EXT_SENS_DATA_22 0x5F
#define EXT_SENS_DATA_23 0x60
#define MOT_DETECT_STATUS 0x61
#define I2C_SLV0_DO 0x63
#define I2C_SLV1_DO 0x64
#define I2C_SLV2_DO 0x65
#define I2C_SLV3_DO 0x66
#define I2C_MST_DELAY_CTRL 0x67
#define SIGNAL_PATH_RESET 0x68
#define MOT_DETECT_CTRL 0x69
#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP
#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode
#define PWR_MGMT_2 0x6C
#define DMP_BANK 0x6D // Activates a specific bank in the DMP
#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank
#define DMP_REG 0x6F // Register in DMP from which to read or to which to write
#define DMP_REG_1 0x70
#define DMP_REG_2 0x71
#define FIFO_COUNTH 0x72
#define FIFO_COUNTL 0x73
#define FIFO_R_W 0x74
#define WHO_AM_I_MPU6050 0x75 // Should return 0x68
class MPU6050lib
{
public:
float getGres();
float getAres();
void readAccelData(int16_t * destination);
void readGyroData(int16_t * destination);
int16_t readTempData();
void LowPowerAccelOnlyMPU6050();
void initMPU6050();
void calibrateMPU6050(float * dest1, float * dest2);
void MPU6050SelfTest(float * destination);
void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
uint8_t readByte(uint8_t address, uint8_t subAddress);
void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
};
3. MPU6050.CPP
#include "MPU6050.h"
int Gscale = GFS_250DPS;
int Ascale = AFS_2G;
float MPU6050lib::getGres() {
switch (Gscale)
{
// Possible gyro scales (and their register bit settings) are:
// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:-( )
/*
//可能的陀螺仪刻度（及其寄存器位设置）为：
// 250 DPS（00），500 DPS（01），1000 DPS（10）和2000 DPS（11）。
//以下是根据该2位值计算DPS /（ADC tick）的算法：
*/
case GFS_250DPS:
return 250.0 / 32768.0;
break;
case GFS_500DPS:
return 500.0 / 32768.0;
break;
case GFS_1000DPS:
return 1000.0 / 32768.0;
break;
case GFS_2000DPS:
return 2000.0 / 32768.0;
break;
}
}
float MPU6050lib::getAres() {
switch (Ascale)
{
// Possible accelerometer scales (and their register bit settings) are:
// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:-( )
/*
//可能的加速度计刻度（及其寄存器位设置）为：
// 2 Gs（00），4 Gs（01），8 Gs（10）和16 Gs（11）。
//以下是根据该2位值计算DPS /（ADC tick）的算法：
*/
case AFS_2G:
return 2.0 / 32768.0;
break;
case AFS_4G:
return 4.0 / 32768.0;
break;
case AFS_8G:
return 8.0 / 32768.0;
break;
case AFS_16G:
return 16.0 / 32768.0;
break;
}
}
void MPU6050lib::readAccelData(int16_t * destination)
{
uint8_t rawData[6]; // x/y/z accel register data stored here-( x / y / z加速寄存器数据存储在此处 )
readBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array-(将六个原始数据寄存器读入数据数组 )
destination[0] = (int16_t)((rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value-( 将MSB和LSB转换为带符号的16位值)
destination[1] = (int16_t)((rawData[2] << 8) | rawData[3]) ;
destination[2] = (int16_t)((rawData[4] << 8) | rawData[5]) ;
}
void MPU6050lib::readGyroData(int16_t * destination)
{
uint8_t rawData[6]; // x/y/z gyro register data stored here
readBytes(MPU6050_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array-(依次将六个原始数据寄存器读入数据数组)
destination[0] = (int16_t)((rawData[0] << 8) | rawData[1]) ;
/* Turn the MSB and LSB into a signed 16-bit value-
(将MSB和LSB转换为带符号的16位值)*/
destination[1] = (int16_t)((rawData[2] << 8) | rawData[3]) ;
destination[2] = (int16_t)((rawData[4] << 8) | rawData[5]) ;
}
int16_t MPU6050lib::readTempData()
{
uint8_t rawData[2]; // x/y/z gyro register data stored here
readBytes(MPU6050_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
return ((int16_t)rawData[0]) << 8 | rawData[1] ; // Turn the MSB and LSB into a 16-bit value
}
// Configure the motion detection control for low power accelerometer mode-(将运动检测控件配置为低功耗加速度计模式 )
void MPU6050lib::LowPowerAccelOnlyMPU6050()
{
// The sensor has a high-pass filter necessary to invoke to allow the sensor motion detection algorithms work properly
// Motion detection occurs on free-fall (acceleration below a threshold for some time for all axes), motion (acceleration above a threshold for some time on at least one axis),
//and zero-motion toggle (acceleration on each axis less than a threshold for some time sets this flag,
// motion above the threshold turns it off).
// The high-pass filter takes gravity out consideration for these threshold evaluations;
// otherwise, the flags would be set all the time!
/*
传感器具有调用所需的高通滤波器，以使传感器运动检测算法正常运行。
运动检测发生在自由落体（所有轴的加速度低于阈值一段时间），
运动（至少一个轴的加速度超过阈值一段时间）和零运动切换
（每个轴上的加速度小于阈值一段时间会设置此标志，
超过阈值的运动将其关闭）。 对于这些阈值评估，
高通滤波器会忽略重力。 否则，标志将一直设置！
*/
uint8_t c = readByte(MPU6050_ADDRESS, PWR_MGMT_1);
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c & ~0x30); // Clear sleep and cycle bits [5:6]-( 清除睡眠和循环位[5：6])
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c | 0x30); // Set sleep and cycle bits [5:6] to zero to make sure accelerometer is running-(将睡眠和循环位[5：6]设置为零，以确保加速度计正在运行 )
c = readByte(MPU6050_ADDRESS, PWR_MGMT_2);
writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c & ~0x38); // Clear standby XA, YA, and ZA bits [3:5]-( 清除备用XA，YA和ZA位[3：5])
writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c | 0x00); // Set XA, YA, and ZA bits [3:5] to zero to make sure accelerometer is running-(将XA，YA和ZA位[3：5]设置为零，以确保加速度计正在运行 )
c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG);
writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x07); // Clear high-pass filter bits [2:0]-( 清除高通滤波器位[2：0])
// Set high-pass filter to 0) reset (disable), 1) 5 Hz, 2) 2.5 Hz, 3) 1.25 Hz, 4) 0.63 Hz, or 7) Hold-(将高通滤波器设置为0）重置（禁用），1）5 Hz，2）2.5 Hz，3）1.25 Hz，4）0.63 Hz或7）保持 )
writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c | 0x00); // Set ACCEL_HPF to 0; reset mode disbaling high-pass filter-(将ACCEL_HPF设置为0; 复位模式平衡高通滤波器)
c = readByte(MPU6050_ADDRESS, CONFIG);
writeByte(MPU6050_ADDRESS, CONFIG, c & ~0x07); // Clear low-pass filter bits [2:0]-( 清除低通滤波器位[2：0])
writeByte(MPU6050_ADDRESS, CONFIG, c | 0x00);
/* Set DLPD_CFG to 0; 260 Hz bandwidth, 1 kHz rate-
( 将DLPD_CFG设置为0；260 Hz带宽，1 kHz速率)*/
c = readByte(MPU6050_ADDRESS, INT_ENABLE);
writeByte(MPU6050_ADDRESS, INT_ENABLE, c & ~0xFF);
// Clear all interrupts-( 清除所有中断)
writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x40);
/* Enable motion threshold (bits 5) interrupt only-
(仅启用运动阈值（位5）中断 )*/
// Motion detection interrupt requires the absolute value of any axis to lie above the detection threshold
// for at least the counter duration
/* 运动检测中断要求任何轴的绝对值都必须高于检测阈值
//至少在计数器持续时间内
*/
writeByte(MPU6050_ADDRESS, MOT_THR, 0x80); // Set motion detection to 0.256 g; LSB = 2 mg-(将运动检测设置为0.256 g；LSB = 2毫克 )
writeByte(MPU6050_ADDRESS, MOT_DUR, 0x01); // Set motion detect duration to 1 ms; LSB is 1 ms @ 1 kHz rate-(将运动检测持续时间设置为1 ms；LSB为1 ms @ 1 kHz速率 )
delay (100);
/* Add delay for accumulation of samples-( 添加延迟以累积样本)*/
c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG);
writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x07);
/* Clear high-pass filter bits [2:0]-( 清除高通滤波器位[2：0])*/
writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c | 0x07);
/* Set ACCEL_HPF to 7; hold the initial accleration value as a referance-
(将ACCEL_HPF设置为7；保留初始加速度值作为参考 )*/
c = readByte(MPU6050_ADDRESS, PWR_MGMT_2);
writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c & ~0xC7);
/* Clear standby XA, YA, and ZA bits [3:5] and LP_WAKE_CTRL bits [6:7]-
(清除备用XA，YA和ZA位[3：5]和LP_WAKE_CTRL位[6：7] )*/
writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c | 0x47);
/* Set wakeup frequency to 5 Hz, and disable XG, YG, and ZG gyros (bits [0:2])-
( 将唤醒频率设置为5 Hz，并禁用XG，YG和ZG陀螺仪（位[0：2]）)
*/
c = readByte(MPU6050_ADDRESS, PWR_MGMT_1);
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c & ~0x20);
// Clear sleep and cycle bit 5-( 清除睡眠并循环第5位)
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c | 0x20);
/* Set cycle bit 5 to begin low power accelerometer motion interrupts-
(将周期位5设置为开始低功耗加速度计运动中断 )*/
}
void MPU6050lib::initMPU6050()
{
// wake up device-don't need this here if using calibration function below
//writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00);
// Clear sleep mode bit (6), enable all sensors
// delay(100); // Delay 100 ms for PLL to get established on x-axis gyro;
// should check for PLL ready interrupt
// get stable time source
/*
//唤醒设备-如果在下面使用校准功能，则不需要此功能
// writeByte（MPU6050_ADDRESS，PWR_MGMT_1，0x00）; //清除睡眠模式位（6），启用所有传感器
// delay（100）; //延迟100毫秒使PLL在x轴陀螺仪上建立；应该检查PLL准备中断
//获得稳定的时间源
*/
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001-(将时钟源设置为带有x轴陀螺仪参考的PLL，位2：0 = 001)
// Configure Gyro and Accelerometer
// Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively;
// DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both
// Maximum delay time is 4.9 ms corresponding to just over 200 Hz sample rate
/*
//配置陀螺仪和加速度计
//禁用FSYNC并将加速度计和陀螺仪带宽分别设置为44和42 Hz；
// DLPF_CFG =位2：0 = 010; 这会将两个采样率都设置为1 kHz
//最大延迟时间为4.9 ms，对应刚好超过200 Hz的采样率
*/
writeByte(MPU6050_ADDRESS, CONFIG, 0x03);
// Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)-(设置采样率=陀螺仪输出率/（1 + SMPLRT_DIV） )
writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x04);
/* Use a 200 Hz rate; the same rate set in CONFIG above-
(使用200 Hz的频率；在上面的CONFIG中设置的相同速率 )
Set gyroscope full scale range-(设置陀螺仪的满量程范围 )
Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3-
(范围选择FS_SEL和AFS_SEL为0-3，因此2位值左移到位置4：3 )*/
uint8_t c = readByte(MPU6050_ADDRESS, GYRO_CONFIG);
writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c & ~0xE0);
// Clear self-test bits [7:5]-(清除自检位[7：5] )
writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c & ~0x18);
// Clear AFS bits [4:3]-(清除AFS位[4：3] )
writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c | Gscale << 3);
// Set full scale range for the gyro-(设置陀螺仪的满量程范围)
// Set accelerometer configuration-( )
c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG);
writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0xE0);
// Clear self-test bits [7:5]-( )
writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x18);
// Clear AFS bits [4:3]-( )
writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c | Ascale << 3);
// Set full scale range for the accelerometer-(设置加速度计的满量程范围 )
// Configure Interrupts and Bypass Enable
// Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips
// can join the I2C bus and all can be controlled by the Arduino as master
/*
配置中断和旁路使能，将中断引脚设置为高电平有效，
推挽并在读取INT_STATUS时清除，启用I2C_BYPASS_EN，
以便其他芯片可以加入I2C总线，
并且所有这些都可以由Arduino作为主机进行控制
*/
writeByte(MPU6050_ADDRESS, INT_PIN_CFG, 0x22);
writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
}
// Function which accumulates gyro and accelerometer data after device initialization.
// It calculates the average of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
/*
在设备初始化后累积陀螺仪和加速度计数据的功能。
计算平均值静态读数，
然后将所得的偏移量加载到加速度计和陀螺仪偏置寄存器中。
*/
void MPU6050lib::calibrateMPU6050(float * dest1, float * dest2)
{
uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
uint16_t ii, packet_count, fifo_count;
int32_t gyro_bias[3] = {
0, 0, 0}, accel_bias[3] = {
0, 0, 0};
/* reset device, reset all registers, clear gyro and accelerometer bias registers-
( 重置设备，重置所有寄存器，清除陀螺仪和加速度计偏置寄存器)
*/
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80);
// Write a one to bit 7 reset bit; toggle reset device
delay(100);
// get stable time source-(获得稳定的时间源 )
/* Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001-
(将时钟源设置为带有x轴陀螺仪参考的PLL，位2：0 = 001 )*/
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01);
writeByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00);
delay(200);
// Configure device for bias calculation-(配置设备进行偏差计算 )
writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source-(打开内部时钟源 )
writeByte(MPU6050_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
writeByte(MPU6050_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes-(禁用FIFO和I2C主模式 )
writeByte(MPU6050_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP-(重置FIFO和DMP )
delay(15);
// Configure MPU6050 gyro and accelerometer for bias calculation-
//(配置MPU6050陀螺仪和加速度计进行偏置计算)
writeByte(MPU6050_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz-(将低通滤波器设置为188 Hz )
writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz-( 将采样率设置为1 kHz)
writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity-(将陀螺仪满量程设置为每秒250度，最大灵敏度)
writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity-(将加速度计满量程设置为2 g，最大灵敏度 )
uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec-(= 131 LSB /度/秒 )
uint16_t accelsensitivity = 16384; // = 16384 LSB/g-(= 16384 LSB / g )
// Configure FIFO to capture accelerometer and gyro data for bias calculation-(配置FIFO以捕获加速度计和陀螺仪数据以进行偏差计算 )
writeByte(MPU6050_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
writeByte(MPU6050_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 1024 bytes in MPU-6050)-(为FIFO启用陀螺仪和加速度传感器（在MPU-6050中最大为1024字节） )
delay(80); // accumulate 80 samples in 80 milliseconds = 960 bytes-(在80毫秒= 960字节中累积80个样本 )
// At end of sample accumulation, turn off FIFO sensor read-(在样品积累结束时，关闭FIFO传感器读取 )
writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO-(禁用用于FIFO的陀螺仪和加速度传感器 )
readBytes(MPU6050_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count-(读取FIFO采样数 )
fifo_count = ((uint16_t)data[0] << 8) | data[1];
packet_count = fifo_count / 12; // How many sets of full gyro and accelerometer data for averaging-(求平均多少套完整陀螺仪和加速度计数据 )
for (ii = 0; ii < packet_count; ii++) {
int16_t accel_temp[3] = {
0, 0, 0}, gyro_temp[3] = {
0, 0, 0};
readBytes(MPU6050_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO-(为FIFO中的每个样本形成带符号的16位整数)
accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ;
accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ;
gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ;
gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ;
gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases-(将各个有符号的16位偏差相加，以获得累积的有符号的32位偏差 )
accel_bias[1] += (int32_t) accel_temp[1];
accel_bias[2] += (int32_t) accel_temp[2];
gyro_bias[0] += (int32_t) gyro_temp[0];
gyro_bias[1] += (int32_t) gyro_temp[1];
gyro_bias[2] += (int32_t) gyro_temp[2];
}
accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases-(标准化总和以获得平均计数偏差 )
accel_bias[1] /= (int32_t) packet_count;
accel_bias[2] /= (int32_t) packet_count;
gyro_bias[0] /= (int32_t) packet_count;
gyro_bias[1] /= (int32_t) packet_count;
gyro_bias[2] /= (int32_t) packet_count;
if (accel_bias[2] > 0L) {
accel_bias[2] -= (int32_t) accelsensitivity; // Remove gravity from the z-axis accelerometer bias calculation-( 从z轴加速度计偏差计算中去除重力)
}
else {
accel_bias[2] += (int32_t) accelsensitivity;
}
// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup-(构造陀螺偏置，以推送到硬件陀螺偏置寄存器，在设备启动时将其重置为零 )
data[0] = (-gyro_bias[0] / 4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format-(除以4得到每度/秒32.9 LSB，以符合预期的偏置输入格式 )
data[1] = (-gyro_bias[0] / 4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases-(偏差是累加的，因此请更改计算出的平均陀螺仪偏差的符号 )
data[2] = (-gyro_bias[1] / 4 >> 8) & 0xFF;
data[3] = (-gyro_bias[1] / 4) & 0xFF;
data[4] = (-gyro_bias[2] / 4 >> 8) & 0xFF;
data[5] = (-gyro_bias[2] / 4) & 0xFF;
// Push gyro biases to hardware registers将陀螺仪偏置推入硬件寄存器
writeByte(MPU6050_ADDRESS, XG_OFFS_USRH, data[0]);// might not be supported in MPU6050-(MPU6050可能不支持 )
writeByte(MPU6050_ADDRESS, XG_OFFS_USRL, data[1]);
writeByte(MPU6050_ADDRESS, YG_OFFS_USRH, data[2]);
writeByte(MPU6050_ADDRESS, YG_OFFS_USRL, data[3]);
writeByte(MPU6050_ADDRESS, ZG_OFFS_USRH, data[4]);
writeByte(MPU6050_ADDRESS, ZG_OFFS_USRL, data[5]);
dest1[0] = (float) gyro_bias[0] / (float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction-( 以度/秒为单位构造陀螺仪偏置，以便以后手动减法)
dest1[1] = (float) gyro_bias[1] / (float) gyrosensitivity;
dest1[2] = (float) gyro_bias[2] / (float) gyrosensitivity;
// Construct the accelerometer biases for push to the hardware accelerometer bias registers.
// These registers contain factory trim values which must be added to the calculated accelerometer biases;
// on boot up these registers will hold non-zero values.
// In addition, bit 0 of the lower byte must be preserved since it is used for temperature compensation calculations.
// Accelerometer bias registers expect bias input as 2048 LSB per g,
// so that the accelerometer biases calculated above must be divided by 8.
/*
构造加速度计偏置以推送到硬件加速度计偏置寄存器。 这些寄存器包含工厂调整值，必须将其添加到计算出的加速度计偏差中； 在启动时，这些寄存器将保存非零值。 此外，低位字节的位0必须保留，因为它用于温度补偿计算。 加速度计偏置寄存器期望的偏置输入为2048 LSB/g，因此，上面计算的加速度计偏置必须除以8。
*/
int32_t accel_bias_reg[3] = {
0, 0, 0}; // A place to hold the factory accelerometer trim biases-( 存放工厂加速度计微调偏差的地方)
readBytes(MPU6050_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values-(读取工厂加速度计调整值 )
accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1];
readBytes(MPU6050_ADDRESS, YA_OFFSET_H, 2, &data[0]);
accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1];
readBytes(MPU6050_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1];
uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers-( 定义加速度计偏置寄存器低字节温度补偿位0的掩码)
uint8_t mask_bit[3] = {
0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis-(定义数组以保留每个加速度计偏置轴的掩码位 )
for (ii = 0; ii < 3; ii++) {
if (accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit-(如果设置了温度补偿位，则将该事实记录在mask_bit中 )
}
// Construct total accelerometer bias, including calculated average accelerometer bias from above-(构造总加速度计偏差，包括从上方计算的平均加速度计偏差 )
accel_bias_reg[0] -= (accel_bias[0] / 8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)-(减去计算出的平均加速度计偏差，缩放为2048 LSB / g（满量程为16 g） )
accel_bias_reg[1] -= (accel_bias[1] / 8);
accel_bias_reg[2] -= (accel_bias[2] / 8);
data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
data[1] = (accel_bias_reg[0]) & 0xFF;
data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers-( 回写加速度计偏置寄存器时保留温度补偿位)
data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
data[3] = (accel_bias_reg[1]) & 0xFF;
data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers-( )
data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
data[5] = (accel_bias_reg[2]) & 0xFF;
data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers-( )
// Push accelerometer biases to hardware registers-(将加速度计偏置推到硬件寄存器 )
writeByte(MPU6050_ADDRESS, XA_OFFSET_H, data[0]); // might not be supported in MPU6050-(MPU6050可能不支持)
writeByte(MPU6050_ADDRESS, XA_OFFSET_L_TC, data[1]);
writeByte(MPU6050_ADDRESS, YA_OFFSET_H, data[2]);
writeByte(MPU6050_ADDRESS, YA_OFFSET_L_TC, data[3]);
writeByte(MPU6050_ADDRESS, ZA_OFFSET_H, data[4]);
writeByte(MPU6050_ADDRESS, ZA_OFFSET_L_TC, data[5]);
// Output scaled accelerometer biases for manual subtraction in the main program-(在主程序中输出缩放的加速度计偏差以进行手动减法 )
dest2[0] = (float)accel_bias[0] / (float)accelsensitivity;
dest2[1] = (float)accel_bias[1] / (float)accelsensitivity;
dest2[2] = (float)accel_bias[2] / (float)accelsensitivity;
}
// Accelerometer and gyroscope self test; check calibration wrt factory settings-(加速度计和陀螺仪自检；检查校准出厂设置 )
void MPU6050lib::MPU6050SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass-(应该返回与工厂调整值的百分比偏差，偏差为+/- 14或更小 )
{
uint8_t rawData[4];
uint8_t selfTest[6];
float factoryTrim[6];
// Configure the accelerometer for self-test-(配置加速度计进行自检)
writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0xF0);
/* Enable self test on all three axes and set accelerometer range to +/- 8 g-
( 启用所有三个轴的自检并将加速度计范围设置为+/- 8 g)*/
writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0xE0);
/* Enable self test on all three axes and set gyro range to +/- 250 degrees/s
( 在所有三个轴上启用自检并将陀螺仪范围设置为+/- 250度/秒)*/
delay(250);
/*Delay a while to let the device execute the self-test
(稍等片刻，让设备执行自检 )
*/
rawData[0] = readByte(MPU6050_ADDRESS, SELF_TEST_X); // X-axis self-test results-( )
rawData[1] = readByte(MPU6050_ADDRESS, SELF_TEST_Y); // Y-axis self-test results-( )
rawData[2] = readByte(MPU6050_ADDRESS, SELF_TEST_Z); // Z-axis self-test results-( )
rawData[3] = readByte(MPU6050_ADDRESS, SELF_TEST_A); // Mixed-axis self-test results-( 混合轴自检结果)
/* Extract the acceleration test results first-
( 首先提取加速度测试结果)*/
selfTest[0] = (rawData[0] >> 3) | (rawData[3] & 0x30) >> 4 ;
/* XA_TEST result is a five-bit unsigned integer-
(XA_TEST结果是一个五位无符号整数 )*/
selfTest[1] = (rawData[1] >> 3) | (rawData[3] & 0x0C) >> 2 ;
/* YA_TEST result is a five-bit unsigned integer-
(YA_TEST结果是一个五位无符号整数 )*/
selfTest[2] = (rawData[2] >> 3) | (rawData[3] & 0x03) ;
/* ZA_TEST result is a five-bit unsigned integer-
(ZA_TEST结果是一个五位无符号整数 )
Extract the gyration test results first-
(首先提取回转测试结果)*/
selfTest[3] = rawData[0] & 0x1F ;
/* XG_TEST result is a five-bit unsigned integer-
(XG_TEST结果是一个五位无符号整数)*/
selfTest[4] = rawData[1] & 0x1F ;
/* YG_TEST result is a five-bit unsigned integer-
(YG_TEST结果是一个五位无符号整数)*/
selfTest[5] = rawData[2] & 0x1F ;
/* ZG_TEST result is a five-bit unsigned integer-
(ZG_TEST结果是一个五位无符号整数)
/Process results to allow final comparison with factory set values-
(处理结果以允许与出厂设置值进行最终比较 )*/
factoryTrim[0] = (4096.0 * 0.34) * (pow( (0.92 / 0.34) , (((float)selfTest[0] - 1.0) / 30.0)));
// FT[Xa] factory trim calculation-(FT [Xa]工厂调整计算)
factoryTrim[1] = (4096.0 * 0.34) * (pow( (0.92 / 0.34) , (((float)selfTest[1] - 1.0) / 30.0)));
// FT[Ya] factory trim calculation
factoryTrim[2] = (4096.0 * 0.34) * (pow( (0.92 / 0.34) , (((float)selfTest[2] - 1.0) / 30.0))); // FT[Za] factory trim calculation
factoryTrim[3] = ( 25.0 * 131.0) * (pow( 1.046 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation
factoryTrim[4] = (-25.0 * 131.0) * (pow( 1.046 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation
factoryTrim[5] = ( 25.0 * 131.0) * (pow( 1.046 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation
/* Output self-test results and factory trim calculation if desired-
(如果需要，输出自检结果和工厂修整计算 )
// Serial.println(selfTest[0]); Serial.println(selfTest[1]); Serial.println(selfTest[2]);
// Serial.println(selfTest[3]); Serial.println(selfTest[4]); Serial.println(selfTest[5]);
// Serial.println(factoryTrim[0]); Serial.println(factoryTrim[1]); Serial.println(factoryTrim[2]);
// Serial.println(factoryTrim[3]); Serial.println(factoryTrim[4]); Serial.println(factoryTrim[5]);
// Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response-
(以（STR-FT）/ FT的比率报告结果；自检响应的工厂剪裁的更改)
// To get to percent, must multiply by 100 and subtract result from 100-
(要达到百分比，必须乘以100并从100中减去结果 )*/
for (int i = 0; i < 6; i++) {
destination[i] = 100.0 + 100.0 * ((float)selfTest[i] - factoryTrim[i]) / factoryTrim[i];
// Report percent differences-(报告百分比差异 )
}
}
void MPU6050lib::writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
{
Wire.beginTransmission(address); // Initialize the Tx buffer-(初始化发送缓冲区 )
Wire.write(subAddress); // Put slave register address in Tx buffer-(将从寄存器地址放入发送缓冲区 )
Wire.write(data); // Put data in Tx buffer-(将数据放入发送缓冲区 )
Wire.endTransmission(); // Send the Tx buffer-( 发送发送缓冲区)
}
uint8_t MPU6050lib::readByte(uint8_t address, uint8_t subAddress)
{
uint8_t data; // `data` will store the register data-( `data`将存储寄存器数据)
Wire.beginTransmission(address); // Initialize the Tx buffer-(初始化发送缓冲区 )
Wire.write(subAddress); // Put slave register address in Tx buffer-( 将从寄存器地址放入发送缓冲区)
Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive-(发送Tx缓冲区，但发送重新启动以保持连接有效 )
Wire.requestFrom(address, (uint8_t) 1); // Read one byte from slave register address-( 从从寄存器地址读取一个字节)
data = Wire.read(); // Fill Rx buffer with result-(用结果填充Rx缓冲区 )
return data; // Return data read from slave register-(返回从从寄存器读取的数据 )
}
void MPU6050lib::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{
Wire.beginTransmission(address); // Initialize the Tx buffer-( )
Wire.write(subAddress); // Put slave register address in Tx buffer-( )
Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive-( )
uint8_t i = 0;
Wire.requestFrom(address, count); // Read bytes from slave register address-( )
while (Wire.available()) {
dest[i++] = Wire.read();
} // Put read results in the Rx buffer-( )
}
4. MPU6050/MPU6050Library/MPU6050IMU.ino
以下代码中的注释翻译成中文
/* MPU6050 Basic Example with IMU
by: Kris Winer
date: May 10, 2014
license:
Demonstrate MPU-6050 basic functionality including initialization, accelerometer trimming, sleep mode functionality as well as parameterizing the register addresses. Added display functions to allow display to on breadboard monitor.
No DMP use. We just want to get out the accelerations, temperature, and gyro readings.
SDA and SCL should have external pull-up resistors (to 3.3V).
10k resistors worked for me. They should be on the breakout board.
Hardware setup:
MPU6050 Breakout --------- Arduino
3.3V --------------------- 3.3V
SDA ----------------------- A4
SCL ----------------------- A5
GND ---------------------- GND
Note: The MPU6050 is an I2C sensor and uses the Arduino Wire library.
Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
*/
/*
演示MPU-6050的基本功能，包括初始化，加速度计调整，
睡眠模式功能以及参数化寄存器地址。添加了显示功能，可以在面包板监视器上显示。
不使用DMP。我们只想获取加速度，温度和陀螺仪读数。
SDA和SCL应该具有外部上拉电阻（至3.3V）。
我们使用10K电阻。他们安装在面包板上。
硬件设置：
MPU6050 Breakout--------- Arduino
3.3V --------------------- 3.3V
SDA ----------------------- A4
SCL ----------------------- A5
GND ---------------------- GND
注意：MPU6050是I2C传感器，并使用Arduino Wire库。
由于传感器不能承受5V的电压，因此我们使用的是3.3 V 8 MHz Pro Mini或3.3 V Teensy 3.1。
我们已在Wire.h / twi.c实用程序文件中禁用了Wire库使用的内部上拉。
通过将TWI_FREQ设置为400000L /twi.h实用程序文件，我们还使用了400 kHz快速I2C模式。
*/
#include