static void _status_cb(unsigned char reg, unsigned char *data) { if (data[0] & BIT0) { i2c_bus_init(MPU6050_ADDR); i2c_reg_read(REG_ACCEL_XOUT_H, 14, _raw_cb); } }
static void _who_am_i_cb(unsigned char reg, unsigned char *data) { if (data[0] != MPU6050_ADDR) return; i2c_bus_init(MPU6050_ADDR); //get out of sleep i2c_reg_uchar_write(REG_PWR_MGMT_1, 0, _pw_mgmt_cb); }
void setup_task(void * pvParameters) { /* Initialize the I2C bus. */ trace_printf("initialize I2C bus\n"); i2c_bus_init(); /* Initialize the SPI bus. */ trace_printf("initialize SPI bus\n"); spi_bus_init(); #if BEACON_TASK trace_printf("starting beacon task\n"); xTaskCreate(beacon_task, BEACON_TASK_NAME, BEACON_TASK_STACK_SIZE, (void *)NULL, tskIDLE_PRIORITY, NULL); #endif #if CAMERA_TASK trace_printf("starting camera task\n"); xTaskCreate(camera_task, CAMERA_TASK_NAME, CAMERA_TASK_STACK_SIZE, (void *)NULL, tskIDLE_PRIORITY, NULL); #endif #if SKYWIRE_TASK trace_printf("starting skywire task\n"); xTaskCreate(skywire_task, SKYWIRE_TASK_NAME, SKYWIRE_TASK_STACK_SIZE, (void *)NULL, tskIDLE_PRIORITY, NULL); #endif #if RECEIVE_TASK trace_printf("starting receiver task\n"); xTaskCreate(receive_task, RECEIVE_TASK_NAME, RECEIVE_TASK_STACK_SIZE, (void *)NULL, tskIDLE_PRIORITY, NULL); #endif /* Delete the setup task. */ vTaskDelete(NULL); trace_printf("Setup complete\n"); }
int mpu6050_init(void) { memset(&raw_offset, 0, sizeof(raw_offset)); i2c_bus_init(MPU6050_ADDR); i2c_reg_read(REG_WHO_AM_I, 1, _who_am_i_cb); //config data ready interruption P1DIR |= BIT5;//input P2IE |= BIT5;//enable interruption P2IES &= ~BIT5;//0 = low to high | 1 = high to low P2IFG &= ~BIT5;//clean int return 0; }
static void _pw_mgmt_cb(unsigned char reg) { i2c_bus_init(MPU6050_ADDR); //set Gyroscope Output Rate = 1k and config the low pass filter. i2c_reg_uchar_write(REG_CONFIG, BIT0, _config_cb); }
static void _config_cb(unsigned char reg) { i2c_bus_init(MPU6050_ADDR); //divider = 4+1 = 5; 1k/5=200hz i2c_reg_uchar_write(REG_SMPRT_DIV, 4, _sampler_divider_cb); }
static void _sampler_divider_cb(unsigned char reg) { i2c_bus_init(MPU6050_ADDR); //gyro Full Scale Range = +-500 º/s i2c_reg_uchar_write(REG_GYRO_CONFIG, BIT3, _gyro_config_cb); }
static void _gyro_config_cb(unsigned char reg) { i2c_bus_init(MPU6050_ADDR); //acell Full Scale Range = +-4g i2c_reg_uchar_write(REG_ACCEL_CONFIG, BIT3, _accel_config_cb); }
static void _accel_config_cb(unsigned char reg) { i2c_bus_init(MPU6050_ADDR); //enable data ready interruption i2c_reg_uchar_write(REG_INT_ENABLE, BIT0, _int_enable_cb); }
interrupt(PORT2_VECTOR) mpu_int(void) { i2c_bus_init(MPU6050_ADDR); i2c_reg_read(REG_INT_STATUS, 1, _status_cb); P2IFG &= ~BIT5;//clean interruption flag }