/****************************************************************************
* 文 件 名: mpu6050.c
* 作    者: 小浩电子科技
* 修    订: 2020-04-27
* 版    本: 1.0
* 描    述: mpu6050的驱动程序
****************************************************************************/

#include <ioCC2530.h>

#include "MPU6050.h"

#define I2C_PORT_NUM        1
#define BUFSIZE             14  //确认大小够不够


//函数声明 
void DelayI2C(uint32_t m);
void MPU6050_Read(unsigned char SlaveAddress,unsigned char REG_Address,unsigned char readNum);
unsigned char MPU6050_Read_1BYTE(unsigned char SlaveAddress,unsigned char REG_Address);
void MPU6050_Write(unsigned char SlaveAddress,unsigned char REG_Address,unsigned char REG_data) ;
void MPU6050_WriteBit(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) ;
void MPU6050_ReadBits(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) ;
void MPU6050_Initialize(void);
void MPU6050_Initialize(void);
void accStop(void);
void accWriteReg(uint8 reg, uint8 val);
void accReadReg(uint8 reg, uint8 *pVal);
void accReadAcc(int8 *pXVal, int8 *pYVal, int8 *pZVal);
uint8_t MPU6050_GetDeviceID();

void accReadAccGro(int16_t *pXAcc, int16_t *pYAcc, int16_t *pZAcc, int16_t *pXGro, int16_t *pYGro, int16_t *pZGro);

volatile uint8_t I2CMasterBuffer[I2C_PORT_NUM][BUFSIZE]={0};
volatile uint8_t I2CSlaveBuffer[I2C_PORT_NUM][BUFSIZE]={0};
volatile uint32_t I2CReadLength[I2C_PORT_NUM]={0};
volatile uint32_t I2CWriteLength[I2C_PORT_NUM]={0};

//函数声明 

uchar MPU6050_Read_Byte(void);
void MPU6050_IIC_Init(void);//IIC初始化
void MPU6050_Signal_Start(void);//IIC起始信号
void MPU6050_Signal_Stop(void);//IIC停止信号
void MPU6050_Write_Byte(uchar wdata);
void MPU6050_Respons(void);//答应信号
void MPU6050_Write_Addr(uchar add,uchar wdata,uchar comd);
uchar MPU6050_Read_Addr(uchar add,uchar comd);

void MPU6050_IIC_Init(void)//IIC初始化
{
    MPU6050_SDA_PIN_OUT();     //P1.5、P1.6定义为输出
    MPU6050_SCL_PIN_OUT();
    MPU6050_SDA_PIN = 1;
    system_delay_us(1000);
    MPU6050_SCL_PIN = 1;
    system_delay_us(1000);
}
void MPU6050_Signal_Start(void)//IIC起始信号
{
    MPU6050_SCL_PIN_OUT();      //P1.5、P1.6定义为输出
    MPU6050_SDA_PIN_OUT(); 
    MPU6050_SDA_PIN = 1;
    system_delay_us(1000);
    MPU6050_SCL_PIN = 1;
    system_delay_us(1000);
    MPU6050_SDA_PIN = 0;
    system_delay_us(1000); 
}
void MPU6050_Signal_Stop(void)//IIC停止信号
{
    MPU6050_SCL_PIN_OUT();      //P1.5、P1.6定义为输出
    MPU6050_SDA_PIN_OUT(); 
    MPU6050_SDA_PIN = 0;
    system_delay_us(1000);
    MPU6050_SCL_PIN = 1;
    system_delay_us(1000);
    MPU6050_SDA_PIN = 1;
    system_delay_us(1000); 
}
void MPU6050_Respons(void)//答应信号
{
    uint i = 0;

     MPU6050_SCL_PIN_OUT();      //P1.5定义为输出
     MPU6050_SDA_PIN_IN();    //P1.6定义为输入

    MPU6050_SCL_PIN = 1;
    system_delay_us(1000);
    MPU6050_SCL_PIN = 0;
    system_delay_us(1000);
    
    if(i>=300)
    {
      system_delay_us(1000);
    }
}
void MPU6050_Write_Byte(uchar wdata)
{
    uchar i,mdata;

    MPU6050_SCL_PIN_OUT();      //P1.5、P1.6定义为输出
    MPU6050_SDA_PIN_OUT(); 
    mdata = wdata;
    for(i=0;i<8;i++)
    {        
        MPU6050_SCL_PIN = 0;
        system_delay_us(1000);
        if(mdata & 0x80)
        {
          MPU6050_SDA_PIN = 1;
        }
        else
        {
          MPU6050_SDA_PIN = 0;
        }
        system_delay_us(1000);
        MPU6050_SCL_PIN = 1;
        system_delay_us(1000);  
        mdata <<= 1;
    }
    MPU6050_SCL_PIN = 0;
    system_delay_us(1000);
    MPU6050_SCL_PIN = 1;
    system_delay_us(1000);
}
uchar MPU6050_Read_Byte(void)
{
    uchar i,rdata = 0;

    MPU6050_SCL_PIN_OUT();  //P1.5定义为输出
    MPU6050_SDA_PIN_IN();    //P1.6定义为输入

    MPU6050_SCL_PIN = 0;
    system_delay_us(1000);
    MPU6050_SCL_PIN = 1;
    for(i=0;i<8;i++)
    {
        MPU6050_SCL_PIN = 1;
        system_delay_us(1000);
        rdata = (rdata<<1)|MPU6050_SDA_PIN;
        MPU6050_SCL_PIN = 0;
        system_delay_us(1000);
    }
    return rdata;
}
void MPU6050_Write_Addr(uchar add,uchar wdata,uchar comd)
{
    MPU6050_Signal_Start(); //产生一个起始信号
    MPU6050_Write_Byte(comd);
    MPU6050_Respons();      //等待答应
    MPU6050_Write_Byte(add);
    MPU6050_Respons();      //等待答应
    MPU6050_Write_Byte(wdata);
    MPU6050_Respons();      //等待答应
    MPU6050_Signal_Stop();  //产生一个终止信号
}
uchar MPU6050_Read_Addr(uchar add,uchar comd)
{
    uchar tdata;
    MPU6050_Signal_Start();     //产生一个起始信号
    MPU6050_Write_Byte(comd);
    MPU6050_Respons();          //等待答应
    MPU6050_Write_Byte(add);
    MPU6050_Respons();          //等待答应
    MPU6050_Signal_Start();     //再产生一个起始信号
    MPU6050_Write_Byte(comd|0x01);
    MPU6050_Respons();          //等待答应
    tdata = MPU6050_Read_Byte();
    MPU6050_Signal_Stop();      //产生一个终止信号
    return tdata;       
}


void DelayI2C(uint32_t m)
{
  uint32_t i;
  
  for(; m != 0; m--)    
       for (i=0; i<5; i++);
}

void MPU6050_Read(unsigned char SlaveAddress,unsigned char REG_Address,unsigned char readNum)
{
    unsigned char i; 
    for(i = 0; i<readNum; i++)
    {
        //读某个IIC器件的数据
        I2CSlaveBuffer[PORT_USED][i] = MPU6050_Read_Addr(REG_Address + i, SlaveAddress);
    }
}

unsigned char MPU6050_Read_1BYTE(unsigned char SlaveAddress,unsigned char REG_Address)
{
    return MPU6050_Read_Addr(REG_Address, SlaveAddress);
}


void MPU6050_Write(unsigned char SlaveAddress,unsigned char REG_Address,unsigned char REG_data) 
{
    //向某个IIC器件写指令读某个地址里面的数据
    MPU6050_Write_Addr(REG_Address, REG_data, SlaveAddress);
}
/**写入8位寄存器的多个位。  
 * @参数 slaveAddr     I2C从器件内部地址
 * @参数 bitStart 第一位的写入位置（0-7）
 * @参数 length   写的比特数(不超过8)
 * @参数 Data     写入数据
 * @返回值 返回状态 (1=成功)
 */
void MPU6050_WriteBits(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) 
{
    //      010 value to write
    // 76543210 bit numbers
    //    xxx   args: bitStart=4, length=3
    // 00011100 mask byte
    // 10101111 original value (sample)
    // 10100011 original & ~mask
    // 10101011 masked | value
  
  		//      010 要写入的值
    // 76543210 比特位
    //    xxx   args: bitStart=4, length=3
    // 00011100 掩码字节
    // 10101111 原始值（样本）
    // 10100011 原始值 & ~掩码
    // 10101011 掩码 | 原始值
    uint8_t tmp;
    uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
    MPU6050_Read(slaveAddr, regAddr, 1);
    tmp = I2CSlaveBuffer[PORT_USED][0];  
//    uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
    data <<= (bitStart - length + 1); // shift data into correct position
    data &= mask;   // zero all non-important bits in data
    tmp &= ~(mask); // zero all important bits in existing byte
    tmp |= data;    // combine data with existing byte
    MPU6050_Write(slaveAddr,regAddr,tmp);   
}

void MPU6050_WriteBit(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) 
{
    uint8_t tmp;
    MPU6050_Read(slaveAddr, regAddr, 1);
    tmp = I2CSlaveBuffer[PORT_USED][0];  
    tmp = (data != 0) ? (tmp | (1 << bitNum)) : (tmp & ~(1 << bitNum));
    MPU6050_Write(slaveAddr,regAddr,tmp); 
}
/**读取8位寄存器的多个位。 
* @参数 addr    I2C从器件内部地址 
* @参数 bitStart第一位的位置读取（0-7） 
* @参数 length  位读取@参数长度数（不超过8） 
* @参数 *data   数据存储地址（即'101'任何bitStart位置读取将等于0X05） 
* @返回值（1=成功） 
*/
void MPU6050_ReadBits(uint8_t slaveAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) 
{
    // 01101001 read byte
    // 76543210 bit numbers
    //    xxx   args: bitStart=4, length=3
    //    010   masked
    //   -> 010 shifted

#if 1 //  这里采取了去除屏蔽的位并且右移动
    uint8_t tmp;
    uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
    MPU6050_Read(slaveAddr, regAddr, 1);
    tmp = I2CSlaveBuffer[PORT_USED][0]; 
//    uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
    tmp &= mask;
    tmp >>= (bitStart - length + 1);
    *data = tmp;
#else
    uint8_t tmp;
    MPU6050_Read(slaveAddr, regAddr, 1);
    tmp = I2CSlaveBuffer[PORT_USED][0]; 
    *data = tmp;
#endif
}

void MPU6050_Initialize(void)
{
    MPU6050_WriteBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, 
        MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO);
    MPU6050_WriteBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG,  //设置陀螺仪 量程  ，量程范围 250 /s
        MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_250);
//    MPU6050_WriteBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG,//设置加速度 量程  
//        MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_2);// ±2g
        MPU6050_WriteBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG,//设置加速度 量程  
        MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_16);// ±16g
        
    MPU6050_WriteBit(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 0);//(DISABLE); 
}

uint8_t MPU6050_GetDeviceID()
{
    uint8_t tmp;

    MPU6050_ReadBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_WHO_AM_I, \
        MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, &tmp);
    
    return tmp<<(MPU6050_WHO_AM_I_BIT-MPU6050_WHO_AM_I_LENGTH+1);  // 注意这里的移位  amomcu
}

//加速度测量, 温度测量， 陀螺仪测量
void MPU6050_GetRawAccelGyro(int16_t* ax, int16_t* ay, int16_t* az, int16_t* temperature, int16_t* gx, int16_t* gy, int16_t* gz) 
{
//    u8 tmpBuffer[14]; 
    //int i;
    MPU6050_Read(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_XOUT_H, 14); 

    /* Get acceleration */
 //   for(i=0; i<3; i++) 
//      AccelGyro[i]=((int16_t)((uint16_t)I2CSlaveBuffer[PORT_USED][2*i] << 8) + I2CSlaveBuffer[PORT_USED][2*i+1]);
   /* Get Angular rate */
//    for(i=4; i<7; i++)
//      AccelGyro[i-1]=((int16_t)((uint16_t)I2CSlaveBuffer[PORT_USED][2*i] << 8) + I2CSlaveBuffer[PORT_USED][2*i+1]); 
    *ax = (((int16_t)I2CSlaveBuffer[PORT_USED][0]) << 8) | ((int16_t)I2CSlaveBuffer[PORT_USED][1]); 
    *ay = (((int16_t)I2CSlaveBuffer[PORT_USED][2]) << 8) | ((int16_t)I2CSlaveBuffer[PORT_USED][3]); 
    *az = (((int16_t)I2CSlaveBuffer[PORT_USED][4]) << 8) | ((int16_t)I2CSlaveBuffer[PORT_USED][5]); 

    // 温度
    *temperature = (((int16_t)I2CSlaveBuffer[PORT_USED][6]) << 8) | ((int16_t)I2CSlaveBuffer[PORT_USED][7]); 
    
    *gx = (((int16_t)I2CSlaveBuffer[PORT_USED][8]) << 8) | ((int16_t)I2CSlaveBuffer[PORT_USED][9]); 
    *gy = (((int16_t)I2CSlaveBuffer[PORT_USED][10]) << 8) | ((int16_t)I2CSlaveBuffer[PORT_USED][11]); 
    *gz = (((int16_t)I2CSlaveBuffer[PORT_USED][12]) << 8) | ((int16_t)I2CSlaveBuffer[PORT_USED][13]); 
}


#if 1
uint8_t bIfConnected = 0; //是否 mpu6050 已经读到标志位 
void accInit(void)
{
    unsigned char tempV = 0;
    MPU6050_IIC_Init();                     //IIC初始化

    tempV = MPU6050_GetDeviceID();
    if(tempV == MPU6050_ADDRESS_AD0_LOW)          //测试是否 mpu6050 已经读到
    {           
        bIfConnected = 1;
    }
    else
    {
        bIfConnected = 0;
    }
    
    if(bIfConnected)
    {
        //mpu6050连接成功

        //初始化mpu6050
        MPU6050_Initialize();

        //读取mpu6050的实时数据           
        //MPU6050_GetRawAccelGyro(&ax, &ay, &az, &gx, &gy, &gz);
        //sprintf(strTemp, "%d %d %d : %d %d %d", ax, ay, az, gx, gy, gz);
        //串口打印该数据
        //UartSendString(strTemp, strlen(strTemp));
    }
}
void accStop(void)
{

}
void accWriteReg(uint8 reg, uint8 val)
{
    if(bIfConnected)
    {
        MPU6050_Write(0x68<<1, reg, val);
    }
}
void accReadReg(uint8 reg, uint8 *pVal)
{
    if(bIfConnected)
    {
        *pVal = MPU6050_Read_1BYTE(0x68<<1, reg);
    }
}
void accReadAcc(int8 *pXVal, int8 *pYVal, int8 *pZVal)
{
    int16_t ax, ay, az;
    int16_t gx, gy, gz;    
    int16_t temperature;
    if(bIfConnected)
    {
        //读取mpu6050的实时数据           
        MPU6050_GetRawAccelGyro(&ax, &ay, &az, &temperature, &gx, &gy, &gz);

        *pXVal = ax/100;
        *pYVal = ay/100;
        *pZVal = az/100;
    }
}

//void accReadAccGro(int8 *pXAcc, int8 *pYAcc, int8 *pZAcc, int8 *pXGro, int8 *pYGro, int8 *pZGro)
//{
//    int16_t ax, ay, az;
//    int16_t gx, gy, gz;    
//    int16_t temperature;
//    if(bIfConnected)
//    {
//        //读取mpu6050的实时数据           
//        MPU6050_GetRawAccelGyro(&ax, &ay, &az, &temperature, &gx, &gy, &gz);
//
//        *pXAcc = ax/100;
//        *pYAcc = ay/100;
//        *pZAcc = az/100;
//
//        *pXGro = gx/100;
//        *pYGro = gy/100;
//        *pZGro = gz/100;        
//    }
//}

void accReadAccGro(int16_t *pXAcc, int16_t *pYAcc, int16_t *pZAcc, int16_t *pXGro, int16_t *pYGro, int16_t *pZGro)
{
    int16_t ax, ay, az;
    int16_t gx, gy, gz;    
    int16_t temperature;
    if(bIfConnected)
    {
        //读取mpu6050的实时数据           
        MPU6050_GetRawAccelGyro(&ax, &ay, &az, &temperature, &gx, &gy, &gz);

        *pXAcc = ax;  //加速度  
        *pYAcc = ay;
        *pZAcc = az;

        *pXGro = gx;//陀螺仪  
        *pYGro = gy;
        *pZGro = gz;        
    }
}

#endif