/*********************************************************************
Function : InitialSensor
Purpose : sensor initialization
Parameter : none
Return value : none
*********************************************************************/
void InitialSensor(int sample_rate_div,int low_pass_filter)
{
  U8 tmpRead[3] = {0};
  U8 MPU_Init_Data[][3] = {
        {PWR_MGMT_1,        0x80, 2},     // [0]  Reset Device
        {PWR_MGMT_1,        0x01, 2},     // [1]  Clock Source
        {INT_PIN_CFG,       0x10, 2},    // [2]  Set INT_ANYRD_2CLEAR
        {INT_ENABLE,        0x01, 2},     // [3]  Set RAW_RDY_EN
        {PWR_MGMT_2,        0x00, 2},     // [4]  Enable Acc & Gyro
        {SMPLRT_DIV,        0x00, 2},     // [5]  Sample Rate Divider
        {GYRO_CONFIG,       0x18, 2},    // [6]  default : +-2000dps
        {ACCEL_CONFIG,      0x08, 2},   // [7]  default : +-4G
        {CONFIG,            low_pass_filter, 2},         // [8]  default : LPS_41Hz
        {ACCEL_CONFIG_2,    0x03, 2}, // [9]  default : LPS_41Hz
        {USER_CTRL,         0x30, 2}      // [10] Set I2C_MST_EN, I2C_IF_DIS
};

    U8 i,j,u8InitCnt;
    U8 u8len = sizeof(MPU_Init_Data) / 3;

    unsigned char testAcc, testGyro;

    if(g_Main.SysPar.SysID > 0)
    {
        u8InitCnt = 1;
    }
    else
    {
        u8InitCnt = 18;
    }

    SPI_CLK_SLOW();

    for(j=0; j<u8InitCnt; j++)
    {
        for(i=0; i<u8len; i++)
        {
            WriteReg(0,MPU_Init_Data[i][0], MPU_Init_Data[i][1]);
            MPU_Init_Data[i][1] = 0;
            // delay
            Delayms(MPU_Init_Data[i][2]);
        }
        set_acc_scale(g_Main.AccScale);
        set_gyro_scale(g_Main.GyroScale);
        //calib_mag();
    }

    WriteMagReg(0, AK8963_CNTL2, 0x01);       // Reset Device
    Delayms(2);
    WriteMagReg(0, AK8963_CNTL1, 0x10);       // Power-down mode
    Delayms(2);
    WriteMagReg(0, AK8963_CNTL1, 0x1F);       // Fuse ROM access mode
    Delayms(2);
    ReadMagRegs(0, AK8963_ASAX, tmpRead, 3);  // Read sensitivity adjustment values
    Delayms(2);
    WriteMagReg(0, AK8963_CNTL1, 0x10);       // Power-down mode
    Delayms(2);


    WriteReg(0, I2C_MST_CTRL, 0x5D);
    Delayms(2);
    WriteReg(0, I2C_SLV0_ADDR, AK8963_I2C_ADDR | 0x80);
    Delayms(2);
    WriteReg(0, I2C_SLV0_REG, AK8963_ST1);
    Delayms(2);
    WriteReg(0, I2C_SLV0_CTRL, I2C_SLVx_EN | 8);
    Delayms(2);

    WriteMagReg(0, AK8963_CNTL1, 0x16);       // Continuous measurement mode 2
    Delayms(2);

    WriteReg(0, I2C_SLV4_CTRL, 0x09);
    Delayms(2);
    WriteReg(0, I2C_MST_DELAY_CTRL, 0x81);
    Delayms(100);

        /*
        WriteReg(0, I2C_SLV0_REG, AK8963_ST1);
        WriteReg(0, I2C_SLV0_ADDR, AK8963_I2C_ADDR | 0x80);
        WriteReg(0, I2C_SLV0_CTRL, 0x80 | 8);*/
    //WriteReg(0,ACCEL_CONFIG,      BITS_FS_4G);
    //WriteReg(0,GYRO_CONFIG,       BITS_FS_2000DPS);
    SPI_CLK_FAST();
    Delayms(10);
}


/*********************************************************************
Function : ReadMagRegs
Purpose : Sensor write register function
Parameter : [in]U8 num -> sensor number
            [in]U8 buffer -> data buffer
            [in]U8 len -> length
Return value : None
*********************************************************************/
void ReadMagRegs(U8 num, U8 addr, U8 *buffer, U8 len)
{
  U8 status = 0;
  uint32_t timeout = 256;

  WriteReg(num, I2C_SLV4_ADDR, AK8963_I2C_ADDR | 0x80);
  Delayms(1);
  for(U8 i = 0; i< len; i++) {
    WriteReg(num, I2C_SLV4_REG, addr + i);
    Delayms(1);
    WriteReg(num, I2C_SLV4_CTRL, I2C_SLVx_EN);
    Delayms(1);

    do {
      status = ReadReg(num, I2C_MST_STATUS);
    } while(((status & I2C_SLV4_DONE) == 0) && (timeout--));

    buffer[i] = ReadReg(num, I2C_SLV4_DI);
    Delayms(1);
  }
}



/*********************************************************************
Function : WriteMagReg
Purpose : Sensor write register function
Parameter : [in]U8 num -> sensor number
            [in]U8 reg -> register address
            [in]U8 value -> Register value
Return value : None
*********************************************************************/
void WriteMagReg(U8 num,U8 reg,U8 value)
{
    uint8_t  status = 0;
    uint32_t timeout = 256;

    WriteReg(num,I2C_SLV4_ADDR,AK8963_I2C_ADDR);  //Set the I2C slave addres of AK8963 and set for write.
    Delayus(1);
    WriteReg(num,I2C_SLV4_REG, reg);     //I2C slave 0 register address from where to begin data transfer
    Delayus(1);
    WriteReg(num,I2C_SLV4_DO, value);
    Delayus(1);
    WriteReg(num,I2C_SLV4_CTRL, READ_FLAG);  //Enable I2C and set 1 byte

    do {
        status = ReadReg(0, I2C_MST_STATUS);
        Delayms(1);
  } while(((status & 0x40) == 0) && (timeout--));
}


void ReadAll(int num)
{
  uint8_t response[22] = {0};
    int16_t bit_data;
    float data;
    int i;


    CS_Func(LOW,num);

    if(num > 0)
    {
        UCA0BurstRead(ACCEL_XOUT_H,22,response);
    }
    else
    {
        UCB1BurstRead(ACCEL_XOUT_H,22,response);
    }
    CS_Func(HIGH,num);

    MoveData(&response[0], g_Main.AccTmp, 6);
    MoveData(&response[6], g_Main.TemperatureTmp, 2);
    MoveData(&response[8], g_Main.GyroTmp, 6);

    if(!(!(response[14] & AK8963_STATUS_DRDY) || (response[14] & AK8963_STATUS_DOR) || (response[21] & AK8963_STATUS_HOFL)))
    {
       MoveData(&response[16], &g_Main.MagTmp[0], 1);
       MoveData(&response[15], &g_Main.MagTmp[1], 1);
       MoveData(&response[18], &g_Main.MagTmp[2], 1);
       MoveData(&response[17], &g_Main.MagTmp[3], 1);
       MoveData(&response[20], &g_Main.MagTmp[4], 1);
       MoveData(&response[19], &g_Main.MagTmp[5], 1);
    }

    MoveData(g_Main.AccTmp, &g_Data_Packet.PacketBuf[8], 6);
    MoveData(g_Main.GyroTmp, &g_Data_Packet.PacketBuf[14], 6);
    MoveData(g_Main.MagTmp, &g_Data_Packet.PacketBuf[20], 6);
    UartTXSend(g_Data_Packet.PacketBuf, 26);
    PWR_LED_OFF;
}