/*********************************************************************************************************************
* 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_imu963ra
* ˾          ɶɿƼ޹˾
* 汾Ϣ          鿴 libraries/doc ļ version ļ 汾˵
*           ADS v1.9.4
* ƽ̨          TC264D
*           https://seekfree.taobao.com/
*
* ޸ļ¼
*                               ע
* 2022-09-15       pudding            first version
* 2023-04-28       pudding            ע˵
********************************************************************************************************************/
/********************************************************************************************************************
* ߶壺
*                  ------------------------------------
*                  ģܽ             Ƭܽ
*                  // Ӳ SPI 
*                  SCL/SPC            鿴 zf_device_imu963ra.h  IMU963RA_SPC_PIN 궨
*                  SDA/DSI            鿴 zf_device_imu963ra.h  IMU963RA_SDI_PIN 궨
*                  SA0/SDO            鿴 zf_device_imu963ra.h  IMU963RA_SDO_PIN 궨
*                  CS                 鿴 zf_device_imu963ra.h  IMU963RA_CS_PIN  궨
*                  VCC                3.3VԴ
*                  GND                Դ
*                  
*
*                  //  IIC 
*                  SCL/SPC            鿴 zf_device_imu963ra.h  IMU963RA_SCL_PIN 궨
*                  SDA/DSI            鿴 zf_device_imu963ra.h  IMU963RA_SDA_PIN 궨
*                  VCC                3.3VԴ
*                  GND                Դ
*                  
*                  ------------------------------------
********************************************************************************************************************/

#include "zf_common_debug.h"
#include "zf_driver_delay.h"
#include "zf_driver_spi.h"
#include "zf_driver_soft_iic.h"
#include "zf_device_imu963ra.h"

int16 imu963ra_gyro_x = 0, imu963ra_gyro_y = 0, imu963ra_gyro_z = 0;       //       GYRO ()
int16 imu963ra_acc_x = 0,  imu963ra_acc_y = 0,  imu963ra_acc_z = 0;        // ٶȼ     ACC  (accelerometer ٶȼ)
int16 imu963ra_mag_x = 0,  imu963ra_mag_y = 0,  imu963ra_mag_z = 0;        //       MAG  (magnetometer )
float imu963ra_transition_factor[3] = {4098, 14.3, 3000};                  // תʵֵı

#if IMU963RA_USE_SOFT_IIC
static soft_iic_info_struct imu963ra_iic_struct;

//-------------------------------------------------------------------------------------------------------------------
//      IMU963RA дĴ
// ˵     reg             Ĵַ
// ˵     data            
// ز     void
// ʹʾ     imu963ra_write_acc_gyro_register(IMU963RA_SLV0_CONFIG, 0x00);
// עϢ     ڲ
//-------------------------------------------------------------------------------------------------------------------
#define imu963ra_write_acc_gyro_register(reg,data)       (soft_iic_write_8bit_register(&imu963ra_iic_struct,reg,data))

//-------------------------------------------------------------------------------------------------------------------
//      IMU963RA Ĵ
// ˵     reg             Ĵַ
// ز     uint8           
// ʹʾ     imu963ra_read_acc_gyro_register(IMU963RA_STATUS_MASTER);
// עϢ     ڲ
//-------------------------------------------------------------------------------------------------------------------
#define imu963ra_read_acc_gyro_register(reg)             (soft_iic_sccb_read_register(&imu963ra_iic_struct,reg))

//-------------------------------------------------------------------------------------------------------------------
//      IMU963RA  ڲ
// ˵     reg             Ĵַ
// ˵     data            ݻ
// ˵     len             ݳ
// ز     void
// ʹʾ     imu963ra_read_acc_gyro_registers(IMU963RA_OUTX_L_A, dat, 6);
// עϢ     ڲ
//-------------------------------------------------------------------------------------------------------------------
#define imu963ra_read_acc_gyro_registers(reg,data,len)   (soft_iic_read_8bit_registers(&imu963ra_iic_struct,reg,data,len))
#else
//-------------------------------------------------------------------------------------------------------------------
//      IMU963RA дĴ
// ˵     reg             Ĵַ
// ˵     data            
// ز     void
// ʹʾ     imu963ra_write_acc_gyro_register(IMU963RA_SLV0_CONFIG, 0x00);
// עϢ     ڲ
//-------------------------------------------------------------------------------------------------------------------
static void imu963ra_write_acc_gyro_register (uint8 reg, uint8 data)
{
    IMU963RA_CS(0);
    spi_write_8bit_register(IMU963RA_SPI, reg | IMU963RA_SPI_W, data);

    IMU963RA_CS(1);
}

//-------------------------------------------------------------------------------------------------------------------
//      IMU963RA Ĵ
// ˵     reg             Ĵַ
// ز     uint8           
// ʹʾ     imu963ra_read_acc_gyro_register(IMU963RA_STATUS_MASTER);
// עϢ     ڲ
//-------------------------------------------------------------------------------------------------------------------
static uint8 imu963ra_read_acc_gyro_register (uint8 reg)
{
    uint8 data = 0;
    IMU963RA_CS(0);
    data = spi_read_8bit_register(IMU963RA_SPI, reg | IMU963RA_SPI_R);

    IMU963RA_CS(1);
    return data;
}

//-------------------------------------------------------------------------------------------------------------------
//      IMU963RA  ڲ
// ˵     reg             Ĵַ
// ˵     data            ݻ
// ˵     len             ݳ
// ز     void
// ʹʾ     imu963ra_read_acc_gyro_registers(IMU963RA_OUTX_L_A, dat, 6);
// עϢ     ڲ
//-------------------------------------------------------------------------------------------------------------------
static void imu963ra_read_acc_gyro_registers (uint8 reg, uint8 *data, uint32 len)
{
    IMU963RA_CS(0);
    spi_read_8bit_registers(IMU963RA_SPI, reg | IMU963RA_SPI_R, data, len);

    IMU963RA_CS(1);
}
#endif

//-------------------------------------------------------------------------------------------------------------------
//      IMU963RA Ϊ IIC д
// ˵     addr            Ŀַ
// ˵     reg             ĿĴ
// ˵     data            
// ز     uint8           1-ʧ 0-ɹ
// ʹʾ     imu963ra_write_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_CONTROL2, 0x80);
// עϢ     ڲ
//-------------------------------------------------------------------------------------------------------------------
static uint8 imu963ra_write_mag_register (uint8 addr, uint8 reg, uint8 data)
{
    uint8 return_state = 0;
    uint16 timeout_count = 0;

    addr = addr << 1;
    imu963ra_write_acc_gyro_register(IMU963RA_SLV0_CONFIG, 0x00);               // ӻ0
    imu963ra_write_acc_gyro_register(IMU963RA_SLV0_ADD, addr | 0);              // õشżƵַעҪ8λI2Cַ 0x2C
    imu963ra_write_acc_gyro_register(IMU963RA_SLV0_SUBADD, reg);                // ҪдļĴַ
    imu963ra_write_acc_gyro_register(IMU963RA_DATAWRITE_SLV0, data);            // Ҫд
    imu963ra_write_acc_gyro_register(IMU963RA_MASTER_CONFIG, 0x4C);             // ڵһͨѶ  I2Cʹ
    
    // ȴͨѶɹ
    while(0 == (0x80 & imu963ra_read_acc_gyro_register(IMU963RA_STATUS_MASTER)))
    {
        if(IMU963RA_TIMEOUT_COUNT < timeout_count ++)
        {
            return_state = 1;
            break;
        }
        system_delay_ms(2);
    }
    return return_state;
}

//-------------------------------------------------------------------------------------------------------------------
//      IMU963RA Ϊ IIC ƶ
// ˵     addr            Ŀַ
// ˵     reg             ĿĴ
// ز     uint8           ȡ
// ʹʾ     imu963ra_read_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_CHIP_ID);
// עϢ     ڲ
//-------------------------------------------------------------------------------------------------------------------
static uint8 imu963ra_read_mag_register (uint8 addr, uint8 reg)
{
    uint16 timeout_count = 0;

    addr = addr << 1;
    imu963ra_write_acc_gyro_register(IMU963RA_SLV0_ADD, addr | 1);              // õشżƵַעҪ8λI2Cַ 0x2C
    imu963ra_write_acc_gyro_register(IMU963RA_SLV0_SUBADD, reg);                // ҪȡļĴַ
    imu963ra_write_acc_gyro_register(IMU963RA_SLV0_CONFIG, 0x01);    
    imu963ra_write_acc_gyro_register(IMU963RA_MASTER_CONFIG, 0x4C);             // ڵһͨѶ  I2Cʹ
    
    // ȴͨѶɹ
    while(0 == (0x01 & imu963ra_read_acc_gyro_register(IMU963RA_STATUS_MASTER)))
    {
        if(IMU963RA_TIMEOUT_COUNT < timeout_count ++)
        {
            break;
        }
        system_delay_ms(2);
    }
    
    return (imu963ra_read_acc_gyro_register(IMU963RA_SENSOR_HUB_1));            // ضȡ
}

//-------------------------------------------------------------------------------------------------------------------
//      IMU963RA Ϊ IIC Զд
// ˵     addr            Ŀַ
// ˵     reg             ĿĴ
// ز     void
// ʹʾ     imu963ra_connect_mag(IMU963RA_MAG_ADDR, IMU963RA_MAG_OUTX_L);
// עϢ     ڲ
//-------------------------------------------------------------------------------------------------------------------
static void imu963ra_connect_mag (uint8 addr, uint8 reg)
{
    addr = addr << 1;
    
    imu963ra_write_acc_gyro_register(IMU963RA_SLV0_ADD, addr | 1);              // õشżƵַעҪ8λI2Cַ 0x2C
    imu963ra_write_acc_gyro_register(IMU963RA_SLV0_SUBADD, reg);                // ҪȡļĴַ
    imu963ra_write_acc_gyro_register(IMU963RA_SLV0_CONFIG, 0x06);    
    imu963ra_write_acc_gyro_register(IMU963RA_MASTER_CONFIG, 0x6C);             // ڵһͨѶ  I2Cʹ
}   


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

    while(0x6B != dat)                                                          // ж ID Ƿȷ
    {
        if(IMU963RA_TIMEOUT_COUNT < timeout_count ++)
        {
            return_state = 1;
            break;
        }
        dat = imu963ra_read_acc_gyro_register(IMU963RA_WHO_AM_I);
        system_delay_ms(10);
    }
    return return_state;
}

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

    while(0xff != dat)                                                          // ж ID Ƿȷ
    {
        if(IMU963RA_TIMEOUT_COUNT < timeout_count ++)
        {
            return_state = 1;
            break;
        }
        dat = imu963ra_read_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_CHIP_ID);
        system_delay_ms(10);
    }
    return return_state;
}

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

    imu963ra_read_acc_gyro_registers(IMU963RA_OUTX_L_A, dat, 6);
    imu963ra_acc_x = (int16)(((uint16)dat[1]<<8 | dat[0]));
    imu963ra_acc_y = (int16)(((uint16)dat[3]<<8 | dat[2]));
    imu963ra_acc_z = (int16)(((uint16)dat[5]<<8 | dat[4]));
}


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

    imu963ra_read_acc_gyro_registers(IMU963RA_OUTX_L_G, dat, 6);
    imu963ra_gyro_x = (int16)(((uint16)dat[1]<<8 | dat[0]));
    imu963ra_gyro_y = (int16)(((uint16)dat[3]<<8 | dat[2]));
    imu963ra_gyro_z = (int16)(((uint16)dat[5]<<8 | dat[4]));
}


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

    imu963ra_write_acc_gyro_register(IMU963RA_FUNC_CFG_ACCESS, 0x40);
    temp_status = imu963ra_read_acc_gyro_register(IMU963RA_STATUS_MASTER);
    if(0x01 & temp_status)
    {
        imu963ra_read_acc_gyro_registers(IMU963RA_SENSOR_HUB_1, dat, 6);
        imu963ra_mag_x = (int16)(((uint16)dat[1]<<8 | dat[0]));
        imu963ra_mag_y = (int16)(((uint16)dat[3]<<8 | dat[2]));
        imu963ra_mag_z = (int16)(((uint16)dat[5]<<8 | dat[4]));
    }
    imu963ra_write_acc_gyro_register(IMU963RA_FUNC_CFG_ACCESS, 0x00);
}

//-------------------------------------------------------------------------------------------------------------------
//      ʼ IMU963RA
// ˵     void
// ز     uint8           1-ʼʧ 0-ʼɹ
// ʹʾ     imu963ra_init();
// עϢ
//-------------------------------------------------------------------------------------------------------------------
uint8 imu963ra_init (void)
{
    uint8 return_state = 0;
    system_delay_ms(10);                                                        // ϵʱ

#if IMU963RA_USE_SOFT_IIC
    soft_iic_init(&imu963ra_iic_struct, IMU963RA_DEV_ADDR, IMU963RA_SOFT_IIC_DELAY, IMU963RA_SCL_PIN, IMU963RA_SDA_PIN);
#else
    spi_init(IMU963RA_SPI, SPI_MODE0, IMU963RA_SPI_SPEED, IMU963RA_SPC_PIN, IMU963RA_SDI_PIN, IMU963RA_SDO_PIN, SPI_CS_NULL);
    gpio_init(IMU963RA_CS_PIN, GPO, GPIO_LOW, GPO_PUSH_PULL);
#endif

    do
    {
        imu963ra_write_acc_gyro_register(IMU963RA_FUNC_CFG_ACCESS, 0x00);       // رHUBĴ
        imu963ra_write_acc_gyro_register(IMU963RA_CTRL3_C, 0x01);               // λ豸
        system_delay_ms(2);
        imu963ra_write_acc_gyro_register(IMU963RA_FUNC_CFG_ACCESS, 0x00);       // رHUBĴ
        if(imu963ra_acc_gyro_self_check())
        {
            zf_log(0, "IMU963RA acc and gyro self check error.");
            return_state = 1;
            break;
        }

        imu963ra_write_acc_gyro_register(IMU963RA_INT1_CTRL, 0x03);             //  ٶݾж

        // IMU963RA_CTRL1_XL Ĵ
        // Ϊ 0x30 ٶΪ 2  G    ȡļٶȼݳ 16393  תΪλ λ g(m/s^2)
        // Ϊ 0x38 ٶΪ 4  G    ȡļٶȼݳ 8197   תΪλ λ g(m/s^2)
        // Ϊ 0x3C ٶΪ 8  G    ȡļٶȼݳ 4098   תΪλ λ g(m/s^2)
        // Ϊ 0x34 ٶΪ 16 G    ȡļٶȼݳ 2049   תΪλ λ g(m/s^2)
        switch(IMU963RA_ACC_SAMPLE_DEFAULT)
        {
            default:
            {
                zf_log(0, "IMU963RA_ACC_SAMPLE_DEFAULT set error.");
                return_state = 1;
            }break;
            case IMU963RA_ACC_SAMPLE_SGN_2G:
            {
                imu963ra_write_acc_gyro_register(IMU963RA_CTRL1_XL, 0x30);
                imu963ra_transition_factor[0] = 16393;
            }break;
            case IMU963RA_ACC_SAMPLE_SGN_4G:
            {
                imu963ra_write_acc_gyro_register(IMU963RA_CTRL1_XL, 0x38);
                imu963ra_transition_factor[0] = 8197;
            }break;
            case IMU963RA_ACC_SAMPLE_SGN_8G:
            {
                imu963ra_write_acc_gyro_register(IMU963RA_CTRL1_XL, 0x3C);
                imu963ra_transition_factor[0] = 4098;
            }break;
            case IMU963RA_ACC_SAMPLE_SGN_16G:
            {
                imu963ra_write_acc_gyro_register(IMU963RA_CTRL1_XL, 0x34);
                imu963ra_transition_factor[0] = 2049;
            }break;
        }
        if(1 == return_state)
        {
            break;
        }

        // IMU963RA_CTRL2_G Ĵ
        // Ϊ 0x52 Ϊ 125  dps    ȡݳ 228.6   תΪλ λΪ /s
        // Ϊ 0x50 Ϊ 250  dps    ȡݳ 114.3   תΪλ λΪ /s
        // Ϊ 0x54 Ϊ 500  dps    ȡݳ 57.1    תΪλ λΪ /s
        // Ϊ 0x58 Ϊ 1000 dps    ȡݳ 28.6    תΪλ λΪ /s
        // Ϊ 0x5C Ϊ 2000 dps    ȡݳ 14.3    תΪλ λΪ /s
        // Ϊ 0x51 Ϊ 4000 dps    ȡݳ 7.1     תΪλ λΪ /s
        switch(IMU963RA_GYRO_SAMPLE_DEFAULT)
        {
            default:
            {
                zf_log(0, "IMU963RA_GYRO_SAMPLE_DEFAULT set error.");
                return_state = 1;
            }break;
            case IMU963RA_GYRO_SAMPLE_SGN_125DPS:
            {
                imu963ra_write_acc_gyro_register(IMU963RA_CTRL2_G, 0x52);
                imu963ra_transition_factor[1] = 228.6;
            }break;
            case IMU963RA_GYRO_SAMPLE_SGN_250DPS:
            {
                imu963ra_write_acc_gyro_register(IMU963RA_CTRL2_G, 0x50);
                imu963ra_transition_factor[1] = 114.3;
            }break;
            case IMU963RA_GYRO_SAMPLE_SGN_500DPS:
            {
                imu963ra_write_acc_gyro_register(IMU963RA_CTRL2_G, 0x54);
                imu963ra_transition_factor[1] = 57.1;
            }break;
            case IMU963RA_GYRO_SAMPLE_SGN_1000DPS:
            {
                imu963ra_write_acc_gyro_register(IMU963RA_CTRL2_G, 0x58);
                imu963ra_transition_factor[1] = 28.6;
            }break;
            case IMU963RA_GYRO_SAMPLE_SGN_2000DPS:
            {
                imu963ra_write_acc_gyro_register(IMU963RA_CTRL2_G, 0x5C);
                imu963ra_transition_factor[1] = 14.3;
            }break;
            case IMU963RA_GYRO_SAMPLE_SGN_4000DPS:
            {
                imu963ra_write_acc_gyro_register(IMU963RA_CTRL2_G, 0x51);
                imu963ra_transition_factor[1] = 7.1;
            }break;
        }
        if(1 == return_state)
        {
            break;
        }

        imu963ra_write_acc_gyro_register(IMU963RA_CTRL3_C, 0x44);                   // ʹֵͨ˲
        imu963ra_write_acc_gyro_register(IMU963RA_CTRL4_C, 0x02);                   // ʹֵͨ˲
        imu963ra_write_acc_gyro_register(IMU963RA_CTRL5_C, 0x00);                   // ٶȼ
        imu963ra_write_acc_gyro_register(IMU963RA_CTRL6_C, 0x00);                   // ٶȼƸģʽ ǵͨ˲ 133hz
        imu963ra_write_acc_gyro_register(IMU963RA_CTRL7_G, 0x00);                   // Ǹģʽ رոͨ˲
        imu963ra_write_acc_gyro_register(IMU963RA_CTRL9_XL, 0x01);                  // رI3Cӿ

        imu963ra_write_acc_gyro_register(IMU963RA_FUNC_CFG_ACCESS, 0x40);           // HUBĴ õشż
        imu963ra_write_acc_gyro_register(IMU963RA_MASTER_CONFIG, 0x80);             // λI2C
        system_delay_ms(2);                             
        imu963ra_write_acc_gyro_register(IMU963RA_MASTER_CONFIG, 0x00);             // λ־
        system_delay_ms(2);
        
        imu963ra_write_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_CONTROL2, 0x80);// λӵ
        system_delay_ms(2);
        imu963ra_write_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_CONTROL2, 0x00);
        system_delay_ms(2);

        if(imu963ra_mag_self_check())
        {
            zf_log(0, "IMU963RA mag self check error.");
            return_state = 1;
            break;            
        }

        // IMU963RA_MAG_ADDR Ĵ
        // Ϊ 0x09 Ϊ 2G   ȡĴݳ 12000   תΪλ λ G(˹)
        // Ϊ 0x19 Ϊ 8G   ȡĴݳ 3000    תΪλ λ G(˹)
        switch(IMU963RA_MAG_SAMPLE_DEFAULT)
        {
            default:
            {
                zf_log(0, "IMU963RA_MAG_SAMPLE_DEFAULT set error.");
                return_state = 1;
            }break;
            case IMU963RA_MAG_SAMPLE_2G:
            {
                imu963ra_write_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_CONTROL1, 0x09);
                imu963ra_transition_factor[2] = 12000;
            }break;
            case IMU963RA_MAG_SAMPLE_8G:
            {
                imu963ra_write_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_CONTROL1, 0x19);
                imu963ra_transition_factor[2] = 3000;
            }break;
        }
        if(1 == return_state)
        {
            break;
        }

        imu963ra_write_mag_register(IMU963RA_MAG_ADDR, IMU963RA_MAG_FBR, 0x01);
        imu963ra_connect_mag(IMU963RA_MAG_ADDR, IMU963RA_MAG_OUTX_L);

        imu963ra_write_acc_gyro_register(IMU963RA_FUNC_CFG_ACCESS, 0x00);       // رHUBĴ

        system_delay_ms(20);                                                    // ȴƻȡ
    }while(0);
    return return_state;
}
