// ----------------------------------------------------------------------------------------------------------------------
static void _mpu9250_call_back(spi_p spi_instance, uint8_t spi_last_received_byte)
{
uint8_t lsb, msb;
static _mpu9520_states_t state = _mpu9520_init_acc;
switch (state)
{
case _mpu9520_init_acc:
{
// Setup Acc
state = _mpu9520_init_gyro;
_mpu9250_write_2_reg(MPU9520_ACCEL_CONFIG_REG, ACC_FULL_SCALE_2_G);
break;
}
case _mpu9520_init_gyro:
{
if (buffer_no_of_items(&_mpu9520_rx_buffer) == 2) {
// Setup Gyro
state = _mpu9520_poll_acc;
buffer_clear(&_mpu9520_rx_buffer);
_mpu9250_write_2_reg(MPU9520_GYRO_CONFIG_REG, GYRO_FULL_SCALE_500_DPS);
}
break;
}
case _mpu9520_poll_acc:
{
if (buffer_no_of_items(&_mpu9520_rx_buffer) == 2) {
state = _mpu9520_read_acc;
buffer_clear(&_mpu9520_rx_buffer);
_poll_acc();
}
break;
}
case _mpu9520_read_acc:
{
if (buffer_no_of_items(&_mpu9520_rx_buffer) == 7) {
uint8_t _sreg = SREG;
cli();
buffer_get_item(&_mpu9520_rx_buffer, &lsb); // Throw away the command response
buffer_get_item(&_mpu9520_rx_buffer, &msb);
buffer_get_item(&_mpu9520_rx_buffer, &lsb);
_x_acc = (msb << 8) | lsb;
buffer_get_item(&_mpu9520_rx_buffer, &msb);
buffer_get_item(&_mpu9520_rx_buffer, &lsb);
_y_acc = (msb << 8) | lsb;
buffer_get_item(&_mpu9520_rx_buffer, &msb);
buffer_get_item(&_mpu9520_rx_buffer, &lsb);
SREG = _sreg;
_z_acc = (msb << 8) | lsb;
state = _mpu9520_read_gyro;
_poll_gyro();
}
break;
}
case _mpu9520_read_gyro:
{
if (buffer_no_of_items(&_mpu9520_rx_buffer) == 7) {
uint8_t _sreg = SREG;
cli();
buffer_get_item(&_mpu9520_rx_buffer, &lsb); // Throw away the command response
buffer_get_item(&_mpu9520_rx_buffer, &msb);
buffer_get_item(&_mpu9520_rx_buffer, &lsb);
_x_gyro = (msb << 8) | lsb;
buffer_get_item(&_mpu9520_rx_buffer, &msb);
buffer_get_item(&_mpu9520_rx_buffer, &lsb);
_y_gyro = (msb << 8) | lsb;
buffer_get_item(&_mpu9520_rx_buffer, &msb);
buffer_get_item(&_mpu9520_rx_buffer, &lsb);
SREG = _sreg;
_z_gyro = (msb << 8) | lsb;
state = _mpu9520_read_acc;
_poll_acc();
}
break;
}
default:
break;
}
}
// ----------------------------------------------------------------------------------------------------------------------
static void _mpu9250_call_back(spi_p spi_instance, uint8_t spi_last_received_byte)
{
uint8_t lsb = 0, msb = 0;
static _mpu9520_states_t state = _mpu9520_init_acc;
switch (state)
{
case _mpu9520_init_acc:
{
// Setup Acc
state = _mpu9520_init_gyro;
_mpu9250_write_2_reg(MPU9520_ACCEL_CONFIG_REG, ACC_FULL_SCALE_2_G);
break;
}
case _mpu9520_init_gyro:
{
if (fifo_get_used_size(_spi_rx_fifo_desc) == 2) {
// Setup Gyro
state = _mpu9520_poll_acc;
fifo_flush(_spi_rx_fifo_desc);
_mpu9250_write_2_reg(MPU9520_GYRO_CONFIG_REG, GYRO_FULL_SCALE_500_DPS);
}
break;
}
case _mpu9520_poll_acc:
{
if (fifo_get_used_size(_spi_rx_fifo_desc) == 2) {
state = _mpu9520_read_acc;
fifo_flush(_spi_rx_fifo_desc);
_poll_acc();
}
break;
}
case _mpu9520_read_acc:
{
if (fifo_get_used_size(_spi_rx_fifo_desc) == 7) {
fifo_pull_uint8(_spi_rx_fifo_desc, &lsb); // Throw away the command response
fifo_pull_uint8(_spi_rx_fifo_desc, &msb);
fifo_pull_uint8(_spi_rx_fifo_desc, &lsb);
_x_acc = (msb << 8) | lsb;
fifo_pull_uint8(_spi_rx_fifo_desc, &msb);
fifo_pull_uint8(_spi_rx_fifo_desc, &lsb);
_y_acc = (msb << 8) | lsb;
fifo_pull_uint8(_spi_rx_fifo_desc, &msb);
fifo_pull_uint8(_spi_rx_fifo_desc, &lsb);
_z_acc = (msb << 8) | lsb;
state = _mpu9520_read_gyro;
_poll_gyro();
}
break;
}
case _mpu9520_read_gyro:
{
if (fifo_get_used_size(_spi_rx_fifo_desc) == 7) {
fifo_pull_uint8(_spi_rx_fifo_desc, &lsb); // Throw away the command response
fifo_pull_uint8(_spi_rx_fifo_desc, &msb);
fifo_pull_uint8(_spi_rx_fifo_desc, &lsb);
_x_gyro = (msb << 8) | lsb;
fifo_pull_uint8(_spi_rx_fifo_desc, &msb);
fifo_pull_uint8(_spi_rx_fifo_desc, &lsb);
_y_gyro = (msb << 8) | lsb;
fifo_pull_uint8(_spi_rx_fifo_desc, &msb);
fifo_pull_uint8(_spi_rx_fifo_desc, &lsb);
_z_gyro = (msb << 8) | lsb;
state = _mpu9520_read_acc;
}
break;
}
default:
break;
}
}
