void l3g4200_event(struct L3g4200 *l3g)
{
if (l3g->initialized) {
if (l3g->i2c_trans.status == I2CTransFailed) {
l3g->i2c_trans.status = I2CTransDone;
}
else if (l3g->i2c_trans.status == I2CTransSuccess) {
// Successfull reading and new data available
if (l3g->i2c_trans.buf[0] & 0x01) { // ver oque é o sinal antes do &
// New data available
l3g->data.rates.p = Int16FromBuf(l3g->i2c_trans.buf,3);
l3g->data.rates.q = Int16FromBuf(l3g->i2c_trans.buf,5);
l3g->data.rates.r = Int16FromBuf(l3g->i2c_trans.buf,7);
l3g->data_available = TRUE;
}
l3g->i2c_trans.status = I2CTransDone;
}
}
else if (l3g->init_status != L3G_CONF_UNINIT) { // Configuring but not yet initialized
if (l3g->i2c_trans.status == I2CTransSuccess || l3g->i2c_trans.status == I2CTransDone) {
l3g->i2c_trans.status = I2CTransDone;
l3g4200_send_config(l3g);
}
if (l3g->i2c_trans.status == I2CTransFailed) {
l3g->init_status--;
l3g->i2c_trans.status = I2CTransDone;
l3g4200_send_config(l3g); // Retry config (TODO max retry)
}
}
}
void itg3200_event(struct Itg3200 *itg)
{
if (itg->initialized) {
if (itg->i2c_trans.status == I2CTransFailed) {
itg->i2c_trans.status = I2CTransDone;
}
else if (itg->i2c_trans.status == I2CTransSuccess) {
// Successfull reading and new data available
if (itg->i2c_trans.buf[0] & 0x01) {
// New data available
itg->data.rates.p = Int16FromBuf(itg->i2c_trans.buf,3);
itg->data.rates.q = Int16FromBuf(itg->i2c_trans.buf,5);
itg->data.rates.r = Int16FromBuf(itg->i2c_trans.buf,7);
itg->data_available = TRUE;
}
itg->i2c_trans.status = I2CTransDone;
}
}
else if (itg->init_status != ITG_CONF_UNINIT) { // Configuring but not yet initialized
if (itg->i2c_trans.status == I2CTransSuccess || itg->i2c_trans.status == I2CTransDone) {
itg->i2c_trans.status = I2CTransDone;
itg3200_send_config(itg);
}
if (itg->i2c_trans.status == I2CTransFailed) {
itg->init_status--;
itg->i2c_trans.status = I2CTransDone;
itg3200_send_config(itg); // Retry config (TODO max retry)
}
}
}
void adxl345_i2c_event(struct Adxl345_I2c *adxl)
{
if (adxl->initialized) {
if (adxl->i2c_trans.status == I2CTransFailed) {
adxl->i2c_trans.status = I2CTransDone;
} else if (adxl->i2c_trans.status == I2CTransSuccess) {
// Successfull reading
adxl->data.vect.x = Int16FromBuf(adxl->i2c_trans.buf, 0);
adxl->data.vect.y = Int16FromBuf(adxl->i2c_trans.buf, 2);
adxl->data.vect.z = Int16FromBuf(adxl->i2c_trans.buf, 4);
adxl->data_available = true;
adxl->i2c_trans.status = I2CTransDone;
}
} else if (adxl->init_status != ADXL_CONF_UNINIT) { // Configuring but not yet initialized
if (adxl->i2c_trans.status == I2CTransSuccess || adxl->i2c_trans.status == I2CTransDone) {
adxl->i2c_trans.status = I2CTransDone;
adxl345_i2c_send_config(adxl);
}
if (adxl->i2c_trans.status == I2CTransFailed) {
adxl->init_status--;
adxl->i2c_trans.status = I2CTransDone;
adxl345_i2c_send_config(adxl); // Retry config (TODO max retry)
}
}
}
void imu_aspirin2_event(void)
{
mpu60x0_spi_event(&imu_aspirin2.mpu);
if (imu_aspirin2.mpu.data_available) {
/* HMC5883 has xzy order of axes in returned data */
struct Int32Vect3 mag;
mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0);
mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2);
mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4);
#ifdef LISA_M_LONGITUDINAL_X
RATES_ASSIGN(imu.gyro_unscaled,
imu_aspirin2.mpu.data_rates.rates.q,
-imu_aspirin2.mpu.data_rates.rates.p,
imu_aspirin2.mpu.data_rates.rates.r);
VECT3_ASSIGN(imu.accel_unscaled,
imu_aspirin2.mpu.data_accel.vect.y,
-imu_aspirin2.mpu.data_accel.vect.x,
imu_aspirin2.mpu.data_accel.vect.z);
VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z);
#else
#ifdef LISA_S
#ifdef LISA_S_UPSIDE_DOWN
RATES_ASSIGN(imu.gyro_unscaled,
imu_aspirin2.mpu.data_rates.rates.p,
-imu_aspirin2.mpu.data_rates.rates.q,
-imu_aspirin2.mpu.data_rates.rates.r);
VECT3_ASSIGN(imu.accel_unscaled,
imu_aspirin2.mpu.data_accel.vect.x,
-imu_aspirin2.mpu.data_accel.vect.y,
-imu_aspirin2.mpu.data_accel.vect.z);
VECT3_ASSIGN(imu.mag_unscaled, mag.x, -mag.y, -mag.z);
#else
RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
VECT3_COPY(imu.mag_unscaled, mag);
#endif
#else
RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z)
#endif
#endif
imu_aspirin2.mpu.data_available = FALSE;
imu_aspirin2.gyro_valid = TRUE;
imu_aspirin2.accel_valid = TRUE;
imu_aspirin2.mag_valid = TRUE;
}
}
void ezcurrent_read_event(void)
{
if (ezcurrent_i2c_trans.status == I2CTransSuccess) {
/* voltage of EzOSD sensor is provided in mV, convert to deciVolt */
electrical.vsupply = Uint16FromBuf(ezcurrent_i2c_trans.buf, 2) / 100;
/* consumed ? in mAh */
electrical.consumed = Int16FromBuf(ezcurrent_i2c_trans.buf, 6);
/* sensor provides current in 1e-1 Ampere, convert to mA */
electrical.current = Int16FromBuf(ezcurrent_i2c_trans.buf, 8) * 100;
// Transaction has been read
ezcurrent_i2c_trans.status = I2CTransDone;
} else if (ezcurrent_i2c_trans.status == I2CTransFailed) {
ezcurrent_i2c_trans.status = I2CTransDone;
// ezcurrent_i2c_trans.slave_addr++;
}
}
void imu_mpu9250_event(void)
{
uint32_t now_ts = get_sys_time_usec();
// If the MPU9250 SPI transaction has succeeded: convert the data
mpu9250_spi_event(&imu_mpu9250.mpu);
if (imu_mpu9250.mpu.data_available) {
// set channel order
struct Int32Vect3 accel = {
IMU_MPU9250_X_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_X]),
IMU_MPU9250_Y_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Y]),
IMU_MPU9250_Z_SIGN * (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Z])
};
struct Int32Rates rates = {
IMU_MPU9250_X_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_X]),
IMU_MPU9250_Y_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]),
IMU_MPU9250_Z_SIGN * (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z])
};
// unscaled vector
VECT3_COPY(imu.accel_unscaled, accel);
RATES_COPY(imu.gyro_unscaled, rates);
#if IMU_MPU9250_READ_MAG
if (!bit_is_set(imu_mpu9250.mpu.data_ext[6], 3)) { //mag valid just HOFL == 0
/** FIXME: assumes that we get new mag data each time instead of reading drdy bit */
struct Int32Vect3 mag;
mag.x = (IMU_MPU9250_X_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Y);
mag.y = (IMU_MPU9250_Y_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_X);
mag.z = -(IMU_MPU9250_Z_SIGN) * Int16FromBuf(imu_mpu9250.mpu.data_ext, 2 * IMU_MPU9250_CHAN_Z);
VECT3_COPY(imu.mag_unscaled, mag);
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_MPU9250_ID, now_ts, &imu.mag);
}
#endif
imu_mpu9250.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_MPU9250_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_MPU9250_ID, now_ts, &imu.accel);
}
}
void lsm303dlhc_i2c_event(struct Lsm303dlhc *lsm)
{
if (lsm->initialized) {
if (lsm->i2c_trans.status == I2CTransFailed) {
lsm->i2c_trans.status = I2CTransDone;
} else if (lsm->i2c_trans.status == I2CTransSuccess) {
lsm->data.vect.x = Int16FromBuf(lsm->i2c_trans.buf, 0);
lsm->data.vect.y = Int16FromBuf(lsm->i2c_trans.buf, 2);
lsm->data.vect.z = Int16FromBuf(lsm->i2c_trans.buf, 4);
lsm->data_available = true;
lsm->i2c_trans.status = I2CTransDone;
} else {
}
} else {
if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) {
if (lsm->init_status.acc != LSM_CONF_ACC_UNINIT) { // Configuring but not yet initialized
if (lsm->i2c_trans.status == I2CTransSuccess || lsm->i2c_trans.status == I2CTransDone) {
lsm->i2c_trans.status = I2CTransDone;
lsm303dlhc_send_config(lsm);
}
if (lsm->i2c_trans.status == I2CTransFailed) {
lsm->init_status.acc--;
lsm->i2c_trans.status = I2CTransDone;
lsm303dlhc_send_config(lsm); // Retry config (TODO max retry)
}
}
} else {
if (lsm->init_status.mag != LSM_CONF_MAG_UNINIT) { // Configuring but not yet initialized
if (lsm->i2c_trans.status == I2CTransSuccess || lsm->i2c_trans.status == I2CTransDone) {
lsm->i2c_trans.status = I2CTransDone;
lsm303dlhc_send_config(lsm);
}
if (lsm->i2c_trans.status == I2CTransFailed) {
lsm->init_status.mag--;
lsm->i2c_trans.status = I2CTransDone;
lsm303dlhc_send_config(lsm); // Retry config (TODO max retry)
}
}
}
}
}
void mpu60x0_spi_event(struct Mpu60x0_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);
int16_t temp_raw = Int16FromBuf(mpu->rx_buf, 8);
mpu->temp = (float)temp_raw / 340.0f + 36.53f;
// 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 != MPU60X0_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:
mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void *)mpu, &(mpu->config));
if (mpu->config.initialized) {
mpu->spi_trans.status = SPITransDone;
}
break;
default:
break;
}
}
}
void ms2100_event(struct Ms2100 *ms) {
// handle request transaction
if (ms->req_trans.status == SPITransDone) {
if (ms->status == MS2100_GOT_EOC) {
// eoc occurs, submit reading req
spi_submit(ms->spi_p, &(ms->read_trans));
ms->status = MS2100_READING_RES;
}
}
else if (ms->req_trans.status == SPITransSuccess) {
ms->req_trans.status = SPITransDone;
}
else if (ms->req_trans.status == SPITransFailed) {
ms->status = MS2100_IDLE;
ms->cur_axe = 0;
ms->req_trans.status = SPITransDone;
}
// handle reading transaction
if (ms->read_trans.status == SPITransSuccess) {
if (ms->status == MS2100_READING_RES) {
// store value
int16_t new_val = Int16FromBuf(ms->read_buf,0);
// what is this check about?
if (abs(new_val) < 2000) {
ms->data.value[ms->cur_axe] = new_val;
}
ms->cur_axe++;
if (ms->cur_axe > 2) {
ms->cur_axe = 0;
ms->status = MS2100_DATA_AVAILABLE;
}
else {
ms->status = MS2100_IDLE;
}
ms->read_trans.status = SPITransDone;
}
}
else if (ms->read_trans.status == SPITransFailed) {
ms->status = MS2100_IDLE;
ms->cur_axe = 0;
ms->read_trans.status = SPITransDone;
}
}
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 imu_aspirin2_event(void)
{
uint32_t now_ts = get_sys_time_usec();
mpu60x0_spi_event(&imu_aspirin2.mpu);
if (imu_aspirin2.mpu.data_available) {
#if !ASPIRIN_2_DISABLE_MAG
/* HMC5883 has xzy order of axes in returned data */
struct Int32Vect3 mag;
mag.x = Int16FromBuf(imu_aspirin2.mpu.data_ext, 0);
mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2);
mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4);
#endif
/* Handle axis assignement for Lisa/S integrated Aspirin like IMU. */
#ifdef LISA_S
#ifdef LISA_S_UPSIDE_DOWN
RATES_ASSIGN(imu.gyro_unscaled,
imu_aspirin2.mpu.data_rates.rates.p,
-imu_aspirin2.mpu.data_rates.rates.q,
-imu_aspirin2.mpu.data_rates.rates.r);
VECT3_ASSIGN(imu.accel_unscaled,
imu_aspirin2.mpu.data_accel.vect.x,
-imu_aspirin2.mpu.data_accel.vect.y,
-imu_aspirin2.mpu.data_accel.vect.z);
VECT3_ASSIGN(imu.mag_unscaled, mag.x, -mag.y, -mag.z);
#else
RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
#if !ASPIRIN_2_DISABLE_MAG
VECT3_COPY(imu.mag_unscaled, mag);
#endif
#endif
#else
/* Handle axis assignement for Lisa/M or Lisa/MX V2.1 integrated Aspirin like
* IMU.
*/
#ifdef LISA_M_OR_MX_21
RATES_ASSIGN(imu.gyro_unscaled,
-imu_aspirin2.mpu.data_rates.rates.q,
imu_aspirin2.mpu.data_rates.rates.p,
imu_aspirin2.mpu.data_rates.rates.r);
VECT3_ASSIGN(imu.accel_unscaled,
-imu_aspirin2.mpu.data_accel.vect.y,
imu_aspirin2.mpu.data_accel.vect.x,
imu_aspirin2.mpu.data_accel.vect.z);
#if !ASPIRIN_2_DISABLE_MAG
VECT3_ASSIGN(imu.mag_unscaled, -mag.y, mag.x, mag.z);
#endif
#else
/* Handle real Aspirin IMU axis assignement. */
#ifdef LISA_M_LONGITUDINAL_X
RATES_ASSIGN(imu.gyro_unscaled,
imu_aspirin2.mpu.data_rates.rates.q,
-imu_aspirin2.mpu.data_rates.rates.p,
imu_aspirin2.mpu.data_rates.rates.r);
VECT3_ASSIGN(imu.accel_unscaled,
imu_aspirin2.mpu.data_accel.vect.y,
-imu_aspirin2.mpu.data_accel.vect.x,
imu_aspirin2.mpu.data_accel.vect.z);
#if !ASPIRIN_2_DISABLE_MAG
VECT3_ASSIGN(imu.mag_unscaled, -mag.x, -mag.y, mag.z);
#endif
#else
RATES_COPY(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_aspirin2.mpu.data_accel.vect);
#if !ASPIRIN_2_DISABLE_MAG
VECT3_ASSIGN(imu.mag_unscaled, mag.y, -mag.x, mag.z)
#endif
#endif
#endif
#endif
imu_aspirin2.mpu.data_available = false;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.accel);
#if !ASPIRIN_2_DISABLE_MAG
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.mag);
#endif
}
}
