/*
Restore the default configuration (default register values)
*/
int mpu_reset(void)
{
int ret=0;
if(write_mpu_reg(MPU_RA_PWR_MGMT1,MPU_RESET_BIT)) {
clock_delay(10000);
ret=1;
}
/*
write_(MPU_ADDR_W,MPU_RA_PWR_MGMT1,MPU_RESET_BIT);
ret = 1;
*/
return ret;
}
/*
* Wake up MPU from sleep (default) mode and turn sensors on
*/
int mpu_wakeup(void)
{
uint8_t reg_val=0x02; //any number other than 0x40 and 0x00
int ret=0;
write_mpu_reg(MPU_RA_PWR_MGMT1,MPU_RV_PWR_MGMT1_AWAKE);
read_mpu_reg(MPU_RA_PWR_MGMT1,®_val);
if(reg_val==MPU_RV_PWR_MGMT1_AWAKE) {
printf("MPU awake and all sensors ON.\n");
ret=1;
}
else
printf("Couldn't turn on MPU sensors.\n",reg_val);
return ret;
}
/*---------------------------------------------------------------*/
PROCESS_THREAD(null_app_process, ev, data)
{
PROCESS_BEGIN();
printf("MPU6050 Started\n");
#ifdef SF_FEATURE_SHELL_OPT
serial_shell_init();
remote_shell_init();
shell_reboot_init();
shell_blink_init();
shell_sky_init();
#endif
uint8_t i;
app_conn_open(&nullApp_callback);
if (node_id > 0)
{
MPU_status = 0;
for(i = 0; i < 100 &(~MPU_status);i++)
{
MPU_status = mpu_enable();
}
if(MPU_status == 0)
{
printf("MPU could not be enabled\n");
}
MPU_status = 0;
for(i = 0; i < 100 &(~MPU_status);i++)
{
MPU_status = mpu_wakeup();
}
if(MPU_status == 0)
{
printf("MPU could not be awakened\n");
}
/* configurate MPU6050 sensor */
uint8_t MPU_config = 0;
// disable sleep model
read_mpu_reg(MPU_RA_PWR_MGMT1,&MPU_config);
MPU_config = MPU_config & ~BV(6); // set bit 6 to 0
write_mpu_reg(MPU_RA_PWR_MGMT1,MPU_config);
#if DEBUG
read_mpu_reg(MPU_RA_PWR_MGMT1,&MPU_config);
PRINTF("power management 1: %u\n",MPU_config);
#endif
// disable cycle
read_mpu_reg(MPU_RA_PWR_MGMT1,&MPU_config);
MPU_config = MPU_config & ~BV(5); // set bit 5 to 0
write_mpu_reg(MPU_RA_PWR_MGMT1,MPU_config);
#if DEBUG
read_mpu_reg(MPU_RA_PWR_MGMT1,&MPU_config);
PRINTF("power management 1: %u\n",MPU_config);
#endif
// gyro range: -/+ 250 degree/sec
read_mpu_reg(MPU_GYRO_CONFIG,&MPU_config);
MPU_config = MPU_config & ~BV(3); // set bit 3 to zero
write_mpu_reg(MPU_GYRO_CONFIG,MPU_config);
#if DEBUG
read_mpu_reg(MPU_GYRO_CONFIG,&MPU_config);
PRINTF("Gyro config: %u\n",MPU_config);
#endif
// accelerometer range: -/+ 2g
read_mpu_reg(MPU_ACCEL_CONFIG,&MPU_config);
MPU_config = MPU_config & ~BV(3); // set bit 3 to zero -/+ 2g
//MPU_config = MPU_config | BV(4); // set bit 4 to one -/+ 8g
write_mpu_reg(MPU_ACCEL_CONFIG,MPU_config);
read_mpu_reg(MPU_ACCEL_CONFIG,&MPU_config);
printf("Acceleromter config: %u\n",MPU_config);
// LPF: cut-off 21Hz for accel and 20Hz for gyro; DLPF_CFG = 4
read_mpu_reg(MPU_CONFIG,&MPU_config);
MPU_config = MPU_config | BV(2); // set bit 2 to 1, DLPF_CFG = 4
write_mpu_reg(MPU_CONFIG,MPU_config);
#if DEBUG
read_mpu_reg(MPU_CONFIG,&MPU_config);
PRINTF("MPU 6050 config: %u\n",MPU_config);
#endif
// sampling rate 1kHz
write_mpu_reg(MPU_SMPLRT_DIV,0);
#if DEBUG
read_mpu_reg(MPU_SMPLRT_DIV,&MPU_config);
PRINTF("sample divider: %u\n",MPU_config);
#endif
// start sampling
ctimer_set(&ct,SAMPLE_RATE,sample_fun,(void*)NULL);
ctimer_set(&reset_timer,SAMPLE_RATE*50,reset_sample_timer,(void*)NULL);
}
else
{
print_MPU = 1;
}
PROCESS_END();
}
