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
}
