/*********************************************************************************************************************
* TC264 Opensourec Library TC264 Դ⣩һڹٷ SDK ӿڵĵԴ
* Copyright (c) 2022 SEEKFREE ɿƼ
*
* ļ TC264 Դһ
*
* TC264 Դ 
* Ըᷢ GPLGNU General Public License GNUͨù֤
*  GPL ĵ3棨 GPL3.0ѡģκκİ汾·/޸
*
* Դķϣܷãδκεı֤
* ûԻʺض;ı֤
* ϸμ GPL
*
* ӦյԴͬʱյһ GPL ĸ
* ûУ<https://www.gnu.org/licenses/>
*
* ע
* Դʹ GPL3.0 Դ֤Э Ϊİ汾
* Ӣİ libraries/doc ļµ GPL3_permission_statement.txt ļ
* ֤ libraries ļ ļµ LICENSE ļ
* ӭλʹò ޸ʱ뱣ɿƼİȨ
*
* ļ          zf_device_mpu6050
* ˾          ɶɿƼ޹˾
* 汾Ϣ          鿴 libraries/doc ļ version ļ 汾˵
*           ADS v1.9.4
* ƽ̨          TC264D
*           https://seekfree.taobao.com/
*
* ޸ļ¼
*                               ע
* 2022-09-15       pudding            first version
* 2023-04-28       pudding            ע˵
********************************************************************************************************************/
/*********************************************************************************************************************
* ߶壺
*                  ------------------------------------
*                  ģܽ             Ƭܽ
*                   IIC ͨŶӦϵ
*                  SCL                鿴 zf_device_mpu6050.h  MPU6050_SOFT_IIC_SCL 궨
*                  SDA                鿴 zf_device_mpu6050.h  MPU6050_SOFT_IIC_SDA 궨
*                  VCC                3.3VԴ
*                  GND                Դ
*                  
*
*                  Ӳ IIC ͨӦϵ
*                  SCL                鿴 zf_device_mpu6050.h  MPU6050_IIC_SCL 궨
*                  SDA                鿴 zf_device_mpu6050.h  MPU6050_IIC_SDA 궨
*                  VCC                3.3VԴ
*                  GND                Դ
*                  
*                  ------------------------------------
********************************************************************************************************************/

#include "zf_common_debug.h"
#include "zf_driver_delay.h"
#include "zf_driver_soft_iic.h"
#include "zf_device_mpu6050.h"

int16 mpu6050_gyro_x = 0, mpu6050_gyro_y = 0, mpu6050_gyro_z = 0;                       //      GYRO ()
int16 mpu6050_acc_x  = 0, mpu6050_acc_y  = 0, mpu6050_acc_z  = 0;                       // ٶȼ    ACC  (accelerometer ٶȼ)

#if MPU6050_USE_SOFT_IIC
static soft_iic_info_struct mpu6050_iic_struct;                                         //  mpu6050 IICͨѶṹ

#define mpu6050_write_register(reg, data)       (soft_iic_write_8bit_register(&mpu6050_iic_struct, (reg), (data)))
#define mpu6050_read_register(reg)              (soft_iic_read_8bit_register(&mpu6050_iic_struct, (reg)))
#define mpu6050_read_registers(reg, data, len)  (soft_iic_read_8bit_registers(&mpu6050_iic_struct, (reg), (data), (len)))
#else
#error "ݲ֧ӲIICͨѶ"
#endif

//-------------------------------------------------------------------------------------------------------------------
//      MPU6050 Լ
// ˵     void
// ز     uint8           1-Լʧ 0-Լɹ
// ʹʾ     if(mpu6050_self1_check())
// עϢ     ڲ
//-------------------------------------------------------------------------------------------------------------------
static uint8 mpu6050_self1_check (void)
{
    uint8 dat = 0, return_state = 0;
    uint16 timeout_count = 0;

    mpu6050_write_register(MPU6050_PWR_MGMT_1, 0x00);                                   // ״̬
    mpu6050_write_register(MPU6050_SMPLRT_DIV, 0x07);                                   // 125HZ
    while(0x07 != dat)
    {
        if(MPU6050_TIMEOUT_COUNT < timeout_count ++)
        {
            return_state =  1;
            break;
        }
        dat = mpu6050_read_register(MPU6050_SMPLRT_DIV);
        system_delay_ms(10);
    }
    return return_state;
}

//-------------------------------------------------------------------------------------------------------------------
//      ȡ MPU6050 ٶȼ
// ˵     void
// ز     void
// ʹʾ     mpu6050_get_acc();                              // ִиúֱӲ鿴Ӧı
// עϢ
//-------------------------------------------------------------------------------------------------------------------
void mpu6050_get_acc (void)
{
    uint8 dat[6];

    mpu6050_read_registers(MPU6050_ACCEL_XOUT_H, dat, 6);
    mpu6050_acc_x = (int16)(((uint16)dat[0] << 8 | dat[1]));
    mpu6050_acc_y = (int16)(((uint16)dat[2] << 8 | dat[3]));
    mpu6050_acc_z = (int16)(((uint16)dat[4] << 8 | dat[5]));
}

//-------------------------------------------------------------------------------------------------------------------
//      ȡ MPU6050 
// ˵     void
// ز     void
// ʹʾ     mpu6050_get_gyro();                             // ִиúֱӲ鿴Ӧı
// עϢ
//-------------------------------------------------------------------------------------------------------------------
void mpu6050_get_gyro (void)
{
    uint8 dat[6];

    mpu6050_read_registers(MPU6050_GYRO_XOUT_H, dat, 6);
    mpu6050_gyro_x = (int16)(((uint16)dat[0] << 8 | dat[1]));
    mpu6050_gyro_y = (int16)(((uint16)dat[2] << 8 | dat[3]));
    mpu6050_gyro_z = (int16)(((uint16)dat[4] << 8 | dat[5]));
}

//-------------------------------------------------------------------------------------------------------------------
//       MPU6050 ٶȼתΪʵ
// ˵     gyro_value              // ļٶȼ
// ز     void
// ʹʾ     float data = mpu6050_acc_transition(imu660ra_acc_x);  //λΪ g(m/s^2)
// עϢ
//-------------------------------------------------------------------------------------------------------------------
float mpu6050_acc_transition (int16 acc_value)
{
    float acc_data = 0;
    switch(MPU6050_ACC_SAMPLE)
    {
        case 0x00: acc_data = (float)acc_value / 16384; break;      // 0x00 ٶȼΪ:2g          ȡļٶȼ 16384      תΪλݣλg(m/s^2)
        case 0x08: acc_data = (float)acc_value / 8192;  break;      // 0x08 ٶȼΪ:4g          ȡļٶȼ 8192       תΪλݣλg(m/s^2)
        case 0x10: acc_data = (float)acc_value / 4096;  break;      // 0x10 ٶȼΪ:8g          ȡļٶȼ 4096       תΪλݣλg(m/s^2)
        case 0x18: acc_data = (float)acc_value / 2048;  break;      // 0x18 ٶȼΪ:16g         ȡļٶȼ 2048       תΪλݣλg(m/s^2)
        default: break;
    }
    return acc_data;
}

//-------------------------------------------------------------------------------------------------------------------
//       MPU6050 תΪʵ
// ˵     gyro_value              // 
// ز     void
// ʹʾ     float data = mpu6050_gyro_transition(imu660ra_gyro_x);  // λΪ/s
// עϢ
//-------------------------------------------------------------------------------------------------------------------
float mpu6050_gyro_transition (int16 gyro_value)
{
    float gyro_data = 0;
    switch(MPU6050_GYR_SAMPLE)
    {
        case 0x00: gyro_data = (float)gyro_value / 131.0f;  break;              // 0x00 Ϊ:250 dps     ȡݳ 131           תΪλݣλΪ/s
        case 0x08: gyro_data = (float)gyro_value / 65.5f;   break;              // 0x08 Ϊ:500 dps     ȡݳ 65.5          תΪλݣλΪ/s
        case 0x10: gyro_data = (float)gyro_value / 32.8f;   break;  //  0x10 Ϊ:1000dps     ȡݳ32.8          תΪλݣλΪ/s
        case 0x18: gyro_data = (float)gyro_value / 16.4f;   break;  //  0x18 Ϊ:2000dps     ȡݳ16.4          תΪλݣλΪ/s
        default: break;
    }
    return gyro_data;
}

//-------------------------------------------------------------------------------------------------------------------
//      ʼ MPU6050
// ˵     void
// ز     uint8           1-ʼʧ 0-ʼɹ
// ʹʾ     mpu6050_init();
// עϢ
//-------------------------------------------------------------------------------------------------------------------
uint8 mpu6050_init (void)
{
    uint8 return_state = 0;
#if MPU6050_USE_SOFT_IIC
    soft_iic_init(&mpu6050_iic_struct, MPU6050_DEV_ADDR, MPU6050_SOFT_IIC_DELAY, MPU6050_SCL_PIN, MPU6050_SDA_PIN);
#else
    iic_init(MPU6050_IIC, MPU6050_DEV_ADDR, MPU6050_IIC_SPEED, MPU6050_SCL_PIN, MPU6050_SDA_PIN);
#endif
    system_delay_ms(100);                                                       // ϵʱ

    do
    {
        if(mpu6050_self1_check())
        {
            // ˶Ϣ ʾλ
            // ô MPU6050 Լʱ˳
            // һ½û ûܾǻ
            zf_log(0, "MPU6050 self check error.");
            return_state = 1;
            break;
        }
        mpu6050_write_register(MPU6050_PWR_MGMT_1, 0x00);                       // ״̬
        mpu6050_write_register(MPU6050_SMPLRT_DIV, 0x07);                       // 125HZ
        mpu6050_write_register(MPU6050_CONFIG, 0x04);

        mpu6050_write_register(MPU6050_GYRO_CONFIG, MPU6050_GYR_SAMPLE);        // 2000
        // GYRO_CONFIGĴ
        // Ϊ:0x00 Ϊ:250 dps     ȡݳ131.2         תΪλݣλΪ/s
        // Ϊ:0x08 Ϊ:500 dps     ȡݳ65.6          תΪλݣλΪ/s
        // Ϊ:0x10 Ϊ:1000dps     ȡݳ32.8          תΪλݣλΪ/s
        // Ϊ:0x18 Ϊ:2000dps     ȡݳ16.4          תΪλݣλΪ/s

        mpu6050_write_register(MPU6050_ACCEL_CONFIG, MPU6050_ACC_SAMPLE);       // 8g
        // ACCEL_CONFIGĴ
        // Ϊ:0x00 ٶȼΪ:2g          ȡļٶȼ 16384      תΪλݣλg(m/s^2)
        // Ϊ:0x08 ٶȼΪ:4g          ȡļٶȼ 8192       תΪλݣλg(m/s^2)
        // Ϊ:0x10 ٶȼΪ:8g          ȡļٶȼ 4096       תΪλݣλg(m/s^2)
        // Ϊ:0x18 ٶȼΪ:16g         ȡļٶȼ 2048       תΪλݣλg(m/s^2)

        mpu6050_write_register(MPU6050_USER_CONTROL, 0x00);
        mpu6050_write_register(MPU6050_INT_PIN_CFG, 0x02);
    }while(0);
    return return_state;
}
