// Configuration function called once before normal use
void mpu9250_i2c_start_configure(struct Mpu9250_I2c *mpu)
{
if (mpu->config.init_status == MPU9250_CONF_UNINIT) {
mpu->config.init_status++;
if (mpu->i2c_trans.status == I2CTransSuccess || mpu->i2c_trans.status == I2CTransDone) {
mpu9250_send_config(mpu9250_i2c_write_to_reg, (void *)mpu, &(mpu->config));
}
}
}
// Configuration function called once before normal use
void mpu9250_spi_start_configure(struct Mpu9250_Spi *mpu)
{
if (mpu->config.init_status == MPU9250_CONF_UNINIT) {
mpu->config.init_status++;
if (mpu->spi_trans.status == SPITransSuccess || mpu->spi_trans.status == SPITransDone) {
mpu9250_send_config(mpu9250_spi_write_to_reg, (void *)mpu, &(mpu->config));
}
}
}
void mpu9250_i2c_event(struct Mpu9250_I2c *mpu)
{
if (mpu->config.initialized) {
if (mpu->i2c_trans.status == I2CTransFailed) {
mpu->i2c_trans.status = I2CTransDone;
}
else if (mpu->i2c_trans.status == I2CTransSuccess) {
// Successfull reading
if (bit_is_set(mpu->i2c_trans.buf[0], 0)) {
// new data
mpu->data_accel.vect.x = Int16FromBuf(mpu->i2c_trans.buf, 1);
mpu->data_accel.vect.y = Int16FromBuf(mpu->i2c_trans.buf, 3);
mpu->data_accel.vect.z = Int16FromBuf(mpu->i2c_trans.buf, 5);
mpu->data_rates.rates.p = Int16FromBuf(mpu->i2c_trans.buf, 9);
mpu->data_rates.rates.q = Int16FromBuf(mpu->i2c_trans.buf, 11);
mpu->data_rates.rates.r = Int16FromBuf(mpu->i2c_trans.buf, 13);
// if we are reading slaves through the mpu, copy the ext_sens_data
if ((mpu->config.i2c_bypass == FALSE) && (mpu->config.nb_slaves > 0)) {
/* the buffer is volatile, since filled from ISR
* but we know it's ok to use it here so we silence the warning
*/
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-qual"
memcpy(mpu->data_ext, (uint8_t *) &(mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15);
#pragma GCC diagnostic pop
}
mpu->data_available = TRUE;
}
mpu->i2c_trans.status = I2CTransDone;
}
}
else if (mpu->config.init_status != MPU9250_CONF_UNINIT) { // Configuring but not yet initialized
switch (mpu->i2c_trans.status) {
case I2CTransFailed:
mpu->config.init_status--; // Retry config (TODO max retry)
case I2CTransSuccess:
case I2CTransDone:
mpu9250_send_config(mpu9250_i2c_write_to_reg, (void*)mpu, &(mpu->config));
if (mpu->config.initialized)
mpu->i2c_trans.status = I2CTransDone;
break;
default:
break;
}
}
// Ak8963 event function
ak8963_event(&mpu->akm);
}
void mpu9250_spi_event(struct Mpu9250_Spi *mpu)
{
if (mpu->config.initialized) {
if (mpu->spi_trans.status == SPITransFailed) {
mpu->spi_trans.status = SPITransDone;
} else if (mpu->spi_trans.status == SPITransSuccess) {
// Successfull reading
if (bit_is_set(mpu->rx_buf[1], 0)) {
// new data
mpu->data_accel.vect.x = Int16FromBuf(mpu->rx_buf, 2);
mpu->data_accel.vect.y = Int16FromBuf(mpu->rx_buf, 4);
mpu->data_accel.vect.z = Int16FromBuf(mpu->rx_buf, 6);
mpu->data_rates.rates.p = Int16FromBuf(mpu->rx_buf, 10);
mpu->data_rates.rates.q = Int16FromBuf(mpu->rx_buf, 12);
mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf, 14);
// if we are reading slaves, copy the ext_sens_data
if (mpu->config.nb_slaves > 0) {
/* the buffer is volatile, since filled from ISR
* but we know it's ok to use it here so we silence the warning
*/
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-qual"
memcpy(mpu->data_ext, (uint8_t *) & (mpu->rx_buf[16]), mpu->config.nb_bytes - 15);
#pragma GCC diagnostic pop
}
mpu->data_available = true;
}
mpu->spi_trans.status = SPITransDone;
}
} else if (mpu->config.init_status != MPU9250_CONF_UNINIT) { // Configuring but not yet initialized
switch (mpu->spi_trans.status) {
case SPITransFailed:
mpu->config.init_status--; // Retry config (TODO max retry)
case SPITransSuccess:
case SPITransDone:
mpu9250_send_config(mpu9250_spi_write_to_reg, (void *)mpu, &(mpu->config));
if (mpu->config.initialized) {
mpu->spi_trans.status = SPITransDone;
}
break;
default:
break;
}
}
}
// Configuration function called once before normal use
void mpu9250_spi_start_configure(struct Mpu9250_Spi *mpu)
{
if (mpu->config.init_status == MPU9250_CONF_UNINIT) {
// First check if we found the chip (succesfull WHO_AM_I response)
if (mpu->spi_trans.status == SPITransSuccess && mpu->rx_buf[1] == MPU9250_WHOAMI_REPLY) {
mpu->config.init_status++;
mpu->spi_trans.status = SPITransDone;
mpu9250_send_config(mpu9250_spi_write_to_reg, (void *)mpu, &(mpu->config));
}
// Send WHO_AM_I to check if chip is there
else if (mpu->spi_trans.status != SPITransRunning && mpu->spi_trans.status != SPITransPending) {
mpu->spi_trans.output_length = 1;
mpu->spi_trans.input_length = 2;
mpu->tx_buf[0] = MPU9250_REG_WHO_AM_I | MPU9250_SPI_READ;
spi_submit(mpu->spi_p, &(mpu->spi_trans));
}
}
}
