uint8_t MPU9250_Init( MPU_InitTypeDef *MPUx )
{
  uint8_t status = ERROR;
  uint8_t MPU6500_InitData[MPU6500_InitRegNum][2] = {
    {0x80, MPU6500_PWR_MGMT_1},     // Reset Device
    {0x01, MPU6500_PWR_MGMT_1},     // Clock Source
    {0x30, MPU6500_USER_CTRL},      // Set I2C_MST_EN, I2C_IF_DIS
    {0x00, MPU6500_PWR_MGMT_2},     // Enable Acc & Gyro
    {0x07, MPU6500_CONFIG},         // DLPF_CFG[2:0] = 111;
    {0x18, MPU6500_GYRO_CONFIG},    // +-2000dps
    {0x08, MPU6500_ACCEL_CONFIG},   // +-4G
    {0x48, MPU6500_ACCEL_CONFIG_2}, // Set Acc Data Rates
    {0x10, MPU6500_INT_PIN_CFG},    // Set INT_ANYRD_2CLEAR
    {0x10, MPU6500_INT_ENABLE},     // 
    {0x4D, MPU6500_I2C_MST_CTRL},   // I2C Speed 400 kHz
  };
  uint8_t AK8963_InitData[AK8963_InitRegNum][2] = {
    {0x01, AK8963_CNTL2},           // Reset Device
    {0x06, AK8963_CNTL1},           // Continuous measurement mode 2
  };

  MPU6500_InitData[5][0] = MPUx->MPU_Gyr_FullScale;       // MPU6500_GYRO_CONFIG
  MPU6500_InitData[4][0] = MPUx->MPU_Gyr_LowPassFilter;   // MPU6500_CONFIG
  MPU6500_InitData[6][0] = MPUx->MPU_Acc_FullScale;       // MPU6500_ACCEL_CONFIG
  MPU6500_InitData[7][0] = MPUx->MPU_Acc_LowPassFilter;   // MPU6500_ACCEL_CONFIG_2
  AK8963_InitData[1][0] |= MPUx->MPU_Mag_FullScale;

  for(uint8_t i = 0; i < MPU6500_InitRegNum; i++) {
    MPU9250_WriteReg(MPU6500_InitData[i][1], MPU6500_InitData[i][0]);
    Delay_1ms(1);
  }

  status = MPU9250_Check();
  if(status != SUCCESS)
    return ERROR;

  Delay_10ms(1);

#ifdef _USE_MAG_AK8963
  for(uint8_t i = 0; i < AK8963_InitRegNum; i++) {
    MPU9250_Mag_WriteReg(AK8963_InitData[i][1], AK8963_InitData[i][0]);
    Delay_10ms(1);
  }

  MPU9250_WriteReg(MPU6500_I2C_SLV0_REG, AK8963_ST1);
  MPU9250_WriteReg(MPU6500_I2C_SLV0_ADDR, AK8963_I2C_ADDR | 0x80);
  MPU9250_WriteReg(MPU6500_I2C_SLV0_CTRL, 0x80 | 8);
#endif

  MPU9250_SetSpeed(SPIx_SPEED_HIGH);
  Delay_10ms(1);

  return SUCCESS;
}
void MPU9250_Init( MPU_InitTypeDef *MPUx )
{
  u8 i = 0;

  u8 MPU6500_Init_Data[MPU9250_InitRegNum][2] = {
      {0x10, MPU6500_USER_CTRL},      // 
      {0x00, MPU6500_PWR_MGMT_1},     // 
      {0x00, MPU6500_PWR_MGMT_2},     // Enable Acc & Gyro
      {0x07, MPU6500_CONFIG},         // DLPF_CFG[2:0] = 111;
      {0x18, MPU6500_GYRO_CONFIG},    // +-2000dps
      {0x08, MPU6500_ACCEL_CONFIG},   // +-4G
      {0x48, MPU6500_ACCEL_CONFIG_2}, // Set Acc Data Rates
      {0x30, MPU6500_INT_PIN_CFG},    // 
      {0x4D, MPU6500_I2C_MST_CTRL},   // I2C Speed 400 kHz

//      // Set Slave to Read AK8963
//      {0x8C, MPU6500_I2C_SLV0_ADDR},  // AK8963_I2C_ADDR ( 7'b000_1100 )
//      {0x00, MPU6500_I2C_SLV0_REG},   // AK8963_WIA ( 0x00 )
//      {0x81, MPU6500_I2C_SLV0_CTRL},  // Enable
//      {0x01, MPU6500_I2C_MST_DELAY_CTRL}
    };

  MPU6500_Init_Data[4][0] = MPUx->MPU_Gyr_FullScale;      // MPU6500_GYRO_CONFIG
  MPU6500_Init_Data[3][0] = MPUx->MPU_Gyr_LowPassFilter;  // MPU6500_CONFIG
  MPU6500_Init_Data[5][0] = MPUx->MPU_Acc_FullScale;      // MPU6500_ACCEL_CONFIG
  MPU6500_Init_Data[6][0] = MPUx->MPU_Acc_LowPassFilter;  // MPU6500_ACCEL_CONFIG_2
LED_B = 0;
  for(i=0; i<MPU9250_InitRegNum; i++) {
    MPU9250_WriteReg(MPU6500_Init_Data[i][1], MPU6500_Init_Data[i][0]);
    LED_G = 0;
    Delay_1ms(5);
  }

  while(MPU9250_Check() != SUCCESS);

  MPU9250_SetSpeedSPI(SPI_SPEED_HIGH);
}
uint8_t MPU9250_Init( MPU_InitTypeDef *MPUx )
{
  uint8_t status = ERROR;
#ifdef _USE_MAG_AK8963
  uint8_t tmpRead[3] = {0};
#endif
  uint8_t MPU6500_InitData[MPU6500_InitRegNum][2] = {
    {0x80, MPU6500_PWR_MGMT_1},     // [0]  Reset Device
    {0x04, MPU6500_PWR_MGMT_1},     // [1]  Clock Source
    {0x10, MPU6500_INT_PIN_CFG},    // [2]  Set INT_ANYRD_2CLEAR
    {0x01, MPU6500_INT_ENABLE},     // [3]  Set RAW_RDY_EN
    {0x00, MPU6500_PWR_MGMT_2},     // [4]  Enable Acc & Gyro
    {0x00, MPU6500_SMPLRT_DIV},     // [5]  Sample Rate Divider
    {0x18, MPU6500_GYRO_CONFIG},    // [6]  default : +-2000dps
    {0x08, MPU6500_ACCEL_CONFIG},   // [7]  default : +-4G
    {0x07, MPU6500_CONFIG},         // [8]  default : LPS_41Hz
    {0x03, MPU6500_ACCEL_CONFIG_2}, // [9]  default : LPS_41Hz
    {0x30, MPU6500_USER_CTRL},      // [10] Set I2C_MST_EN, I2C_IF_DIS
  };

  MPU6500_InitData[6][0] = MPUx->MPU_Gyr_FullScale;       // MPU6500_GYRO_CONFIG
  MPU6500_InitData[8][0] = MPUx->MPU_Gyr_LowPassFilter;   // MPU6500_CONFIG
  MPU6500_InitData[7][0] = MPUx->MPU_Acc_FullScale;       // MPU6500_ACCEL_CONFIG
  MPU6500_InitData[9][0] = MPUx->MPU_Acc_LowPassFilter;   // MPU6500_ACCEL_CONFIG_2

  for(uint8_t i = 0; i < MPU6500_InitRegNum; i++) {
    Delay_1ms(2);
    MPU9250_WriteReg(MPU6500_InitData[i][1], MPU6500_InitData[i][0]);
  }

  status = MPU9250_Check();
  if(status != SUCCESS)
    return ERROR;

  Delay_10ms(1);

#ifdef _USE_MAG_AK8963
  MPU9250_Mag_WriteReg(AK8963_CNTL2, 0x01);       // Reset Device
  Delay_1ms(1);
  MPU9250_Mag_WriteReg(AK8963_CNTL1, 0x10);       // Power-down mode
  Delay_1ms(1);
  MPU9250_Mag_WriteReg(AK8963_CNTL1, 0x1F);       // Fuse ROM access mode
  Delay_1ms(1);
  MPU9250_Mag_ReadRegs(AK8963_ASAX, tmpRead, 3);  // Read sensitivity adjustment values
  Delay_1ms(1);
  MPU9250_Mag_WriteReg(AK8963_CNTL1, 0x10);       // Power-down mode
  Delay_1ms(1);

	AK8963_ASA[0] = (int16_t)(tmpRead[0]) + 128;
	AK8963_ASA[1] = (int16_t)(tmpRead[1]) + 128;
	AK8963_ASA[2] = (int16_t)(tmpRead[2]) + 128;

  MPU9250_WriteReg(MPU6500_I2C_MST_CTRL, 0x5D);
  Delay_1ms(1);
  MPU9250_WriteReg(MPU6500_I2C_SLV0_ADDR, AK8963_I2C_ADDR | 0x80);
  Delay_1ms(1);
  MPU9250_WriteReg(MPU6500_I2C_SLV0_REG, AK8963_ST1);
  Delay_1ms(1);
  MPU9250_WriteReg(MPU6500_I2C_SLV0_CTRL, MPU6500_I2C_SLVx_EN | 8);
  Delay_1ms(1);

  MPU9250_Mag_WriteReg(AK8963_CNTL1, 0x16);       // Continuous measurement mode 2
  Delay_1ms(1);

	MPU9250_WriteReg(MPU6500_I2C_SLV4_CTRL, 0x09);
	Delay_1ms(1);
	MPU9250_WriteReg(MPU6500_I2C_MST_DELAY_CTRL, 0x81);
	Delay_100ms(1);
#endif

  MPU9250_SetSpeed(SPIx_SPEED_HIGH);
  Delay_10ms(1);

  return SUCCESS;
}