/* 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(); }