/*====================================================================================================*/
void System_Init( void )
{
MPU_InitTypeDef MPU_InitStruct;
HAL_Init();
GPIO_Config();
Serial_Config();
MPU6050_Config();
Delay_100ms(1);
printf("\r\nHello World!\r\n\r\n");
LED_B_Reset();
MPU_InitStruct.MPU_Gyr_FullScale = MPU_GyrFS_2000dps;
MPU_InitStruct.MPU_Gyr_LowPassFilter = MPU_GyrLPS_41Hz;
MPU_InitStruct.MPU_Acc_FullScale = MPU_AccFS_4g;
MPU_InitStruct.MPU_Acc_LowPassFilter = MPU_AccLPS_41Hz;
while(MPU6050_Init(&MPU_InitStruct) != SUCCESS) {
LED_R_Toggle();
Delay_100ms(1);
}
LED_R_Set();
LED_B_Set();
Delay_100ms(1);
}
/** Power on and prepare for general usage.
* This will activate the device and take it out of sleep mode (which must be done
* after start-up). This function also sets both the accelerometer and the gyroscope
* to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets
* the clock source to use the X Gyro for reference, which is slightly better than
* the default internal clock source.
*/
void MPU6050_Initialize()
{
MPU6050_SetSleepModeStatus(DISABLE);
//MPU6050_WriteBit(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, DISABLE);
delay(100);
MPU6050_SetClockSource(MPU6050_CLOCK_PLL_XGYRO);
delay(100);
MPU6050_Config(0x01);
delay(100);
//MPU6050_WriteBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_SMPLRT_DIV, 7, 8, 0x00);
//delay(100);
MPU6050_SetFullScaleGyroRange(MPU6050_GYRO_FS_2000);
delay(100);
MPU6050_SetFullScaleAccelRange(MPU6050_ACCEL_FS_16);
delay(100);
MPU6050_WriteBit(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_INT_PIN_CFG, 4, ENABLE);
//delay(100);
MPU6050_WriteBit(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_INT_ENABLE, 0, ENABLE);
//delay(100);
//MPU6050_WriteBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 2, 3, 0x07);
//delay(100);
//MPU6050_enablemagnetometer();
//delay(100);
// MPU6050_WriteByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, 1<<7);//reset the whole module first
/* MPU6050_WriteBit(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, DISABLE);
delay(50); //wait for 50ms for the gyro to stable
MPU6050_WriteByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_CLOCK_PLL_ZGYRO);//PLL with Z axis gyroscope reference
MPU6050_WriteByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_CONFIG, 0x00); //DLPF_CFG = 0: Fs=8khz; bandwidth=256hz
MPU6050_WriteByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x00); //1000Hz sample rate ~ 2ms
MPU6050_WriteByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GYRO_FS_2000); //Gyro full scale setting
MPU6050_WriteByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACCEL_FS_16); //Accel full scale setting
MPU6050_WriteByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_INT_PIN_CFG, 1<<4); //interrupt status bits are cleared on any read operation
MPU6050_WriteByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_INT_ENABLE, 1<<0); //interupt occurs when data is ready. The interupt routine is in the receiver.c file.
MPU6050_WriteByte(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x07);//reset gyro and accel sensor
*/
}
