static void mtpipe_flush_all(mtpipe_handle h)
{
struct list_head_t *p_head = &h->fifo_list;
struct __fifo_elem_t *p = NULL;
list_foreach(p_head, p) {
fifo_flush(p->fifo);
}
int bsreader_reset(int stream)
{
if (!G_ready) {
return -1;
}
printf("bsreader_reset\n");
fifo_flush(myFIFO[stream]);
return 0;
}
// ----------------------------------------------------------------------------------------------------------------------
static void _mpu9250_read_reg(uint8_t reg, uint8_t no_of_bytes)
{
uint8_t send[no_of_bytes+1];
send[0] = MPU9520_READ | reg;
for (uint8_t i = 1; i <= no_of_bytes; i++)
{
send[i] = 0;
}
fifo_flush(_spi_rx_fifo_desc);
spi_send_bytes(_spi_mpu9520, send , no_of_bytes+1);
}
void event_handler_on_ts_end(void)
{
fifo_flush(&g_async_evt_fifo_ts);
}
// ----------------------------------------------------------------------------------------------------------------------
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;
}
}
