#define MPU6050_ADDRESS    0xD0

// 指定地址写
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
    MyI2C_Start();
    MyI2C_SendByte(MPU6050_ADDRESS);
    MyI2C_ReceiveAck();
    MyI2C_SendByte(RegAddress);
    MyI2C_ReceiveAck();
    MyI2C_SendByte(Data);
    MyI2C_ReceiveAck();
    MyI2C_Stop();
}

// 指定地址读
uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
    uint8_t Data;
    MyI2C_Start();
    MyI2C_SendByte(MPU6050_ADDRESS);
    MyI2C_ReceiveAck();
    MyI2C_SendByte(RegAddress);
    MyI2C_ReceiveAck();
    MyI2C_Start();
    MyI2C_SendByte(MPU6050_ADDRESS | 0x01);
    MyI2C_ReceiveAck();
    Data = MyI2C_ReceiveByte(1);
    MyI2C_Stop();
    return Data;
}

// 初始化MPU6050
uint8_t MPU6050_Init(void)
{
    uint8_t WhoAmI;
    MyI2C_Init();
    WhoAmI = MPU6050_ReadReg(MPU6050_REG_WHO_AM_I);
    if (WhoAmI != 0x68) return 1;
    MPU6050_WriteReg(MPU6050_REG_PWR_MGMT_1, 0x00);
    MPU6050_WriteReg(MPU6050_REG_SMPLRT_DIV, 0x07);
    MPU6050_WriteReg(MPU6050_REG_CONFIG, 0x00);
    MPU6050_WriteReg(MPU6050_REG_GYRO_CONFIG, 0x00);
    MPU6050_WriteReg(MPU6050_REG_ACCEL_CONFIG, 0x00);
    return 0;
}

// 获取加速度
void MPU6050_GetAccel(int16_t *AccelX, int16_t *AccelY, int16_t *AccelZ)
{
    uint8_t Data[6];
    MPU6050_ReadRegs(MPU6050_REG_ACCEL_XOUT_H, Data, 6);
    *AccelX = (Data[0] << 8) | Data[1];
    *AccelY = (Data[2] << 8) | Data[3];
    *AccelZ = (Data[4] << 8) | Data[5];
}

// 获取角速度
void MPU6050_GetGyro(int16_t *GyroX, int16_t *GyroY, int16_t *GyroZ)
{
    uint8_t Data[6];
    MPU6050_ReadRegs(MPU6050_REG_GYRO_XOUT_H, Data, 6);
    *GyroX = (Data[0] << 8) | Data[1];
    *GyroY = (Data[2] << 8) | Data[3];
    *GyroZ = (Data[4] << 8) | Data[5];
}

// 获取温度值
void MPU6050_GetTemp(int16_t *Temp)
{
    uint8_t Data[2];
    MPU6050_ReadRegs(MPU6050_REG_TEMP_OUT_H, Data, 2);
    *Temp = (Data[0] << 8) | Data[1];
}

// 获取所有数据
void MPU6050_GetAllData(int16_t *AccelX, int16_t *AccelY, int16_t *AccelZ, int16_t *GyroX, int16_t *GyroY, int16_t *GyroZ, int16_t *Temp)
{
    uint8_t Data[14];
    MPU6050_ReadRegs(MPU6050_REG_ACCEL_XOUT_H, Data, 14);
    *AccelX = (Data[0] << 8) | Data[1];
    *AccelY = (Data[2] << 8) | Data[3];
    *AccelZ = (Data[4] << 8) | Data[5];
    *Temp = (Data[6] << 8) | Data[7];
    *GyroX = (Data[8] << 8) | Data[9];
    *GyroY = (Data[10] << 8) | Data[11];
    *GyroZ = (Data[12] << 8) | Data[13];
}