// ----------------------------------------------------------------------------------------------------------------------
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;
	}
}
示例#2
0
// ----------------------------------------------------------------------------------------------------------------------
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;
	}
}