58 lines
1.8 KiB
C++
58 lines
1.8 KiB
C++
#include "MPU6050_ESP8266.h"
|
|
|
|
MPU6050_ESP8266::MPU6050_ESP8266(){
|
|
Wire.begin();
|
|
}
|
|
|
|
void MPU6050_ESP8266::I2C_Write(uint8_t regAddress, uint8_t data){
|
|
Wire.beginTransmission(MPU6050SlaveAddress);
|
|
Wire.write(regAddress);
|
|
Wire.write(data);
|
|
Wire.endTransmission();
|
|
}
|
|
|
|
//初始化 MPU6050
|
|
void MPU6050_ESP8266::begin(){
|
|
I2C_Write(MPU6050_REGISTER_SMPLRT_DIV, 0x07);
|
|
I2C_Write(MPU6050_REGISTER_PWR_MGMT_1, 0x01);
|
|
I2C_Write(MPU6050_REGISTER_PWR_MGMT_2, 0x00);
|
|
I2C_Write(MPU6050_REGISTER_CONFIG, 0x00);
|
|
I2C_Write(MPU6050_REGISTER_GYRO_CONFIG, 0x00);//set +/-250 degree/second full scale
|
|
I2C_Write(MPU6050_REGISTER_ACCEL_CONFIG, 0x00);// set +/- 2g full scale
|
|
I2C_Write(MPU6050_REGISTER_FIFO_EN, 0x00);
|
|
I2C_Write(MPU6050_REGISTER_INT_ENABLE, 0x01);
|
|
I2C_Write(MPU6050_REGISTER_SIGNAL_PATH_RESET, 0x00);
|
|
I2C_Write(MPU6050_REGISTER_USER_CTRL, 0x00);
|
|
}
|
|
|
|
// 更新数据
|
|
void MPU6050_ESP8266::update(){
|
|
Wire.beginTransmission(MPU6050SlaveAddress);
|
|
Wire.write(MPU6050_REGISTER_ACCEL_XOUT_H);
|
|
Wire.endTransmission();
|
|
Wire.requestFrom(MPU6050SlaveAddress, (uint8_t)14);
|
|
|
|
int16_t AccelX, AccelY, AccelZ, Temperature, GyroX, GyroY, GyroZ;
|
|
|
|
AccelX = (((int16_t)Wire.read()<<8) | Wire.read());
|
|
AccelY = (((int16_t)Wire.read()<<8) | Wire.read());
|
|
AccelZ = (((int16_t)Wire.read()<<8) | Wire.read());
|
|
Temperature = (((int16_t)Wire.read()<<8) | Wire.read());
|
|
GyroX = (((int16_t)Wire.read()<<8) | Wire.read());
|
|
GyroY = (((int16_t)Wire.read()<<8) | Wire.read());
|
|
GyroZ = (((int16_t)Wire.read()<<8) | Wire.read());
|
|
|
|
angleAccX = (double)AccelX/AccelScaleFactor*100;
|
|
angleAccY = (double)AccelY/AccelScaleFactor*100;
|
|
angleZ = (double)AccelZ/AccelScaleFactor*100;
|
|
temp = (double)Temperature/340+36.53; //温度公式
|
|
accX = (double)GyroX/GyroScaleFactor;
|
|
accY = (double)GyroY/GyroScaleFactor;
|
|
accZ = (double)GyroZ/GyroScaleFactor;
|
|
}
|
|
|
|
|
|
|
|
|
|
|