Skip to content

Commit

Permalink
Convert imu_aspirin event handler to static inline
Browse files Browse the repository at this point in the history
  • Loading branch information
Allen committed Feb 24, 2011
1 parent d52860b commit ed22614
Show file tree
Hide file tree
Showing 3 changed files with 66 additions and 47 deletions.
3 changes: 1 addition & 2 deletions sw/airborne/subsystems/imu.h
View file
Expand Up @@ -61,14 +61,13 @@ struct ImuFloat {
};

extern void imu_float_init(struct ImuFloat* imuf);
extern struct Imu imu;

/* underlying hardware */
#ifdef IMU_TYPE_H
#include IMU_TYPE_H
#endif

extern struct Imu imu;

extern void imu_init(void);

#define ImuScaleGyro(_imu) { \
Expand Down
6 changes: 6 additions & 0 deletions sw/airborne/subsystems/imu/imu_aspirin.c
View file
Expand Up @@ -32,6 +32,12 @@ void imu_impl_init(void) {
imu_aspirin.mag_available = FALSE;
imu_aspirin.accel_available = FALSE;

imu_aspirin.i2c_trans_gyro.type = I2CTransTxRx;
imu_aspirin.i2c_trans_gyro.buf[0] = ITG3200_REG_GYRO_XOUT_H;
imu_aspirin.i2c_trans_gyro.slave_addr = ITG3200_ADDR;
imu_aspirin.i2c_trans_gyro.len_w = 1;
imu_aspirin.i2c_trans_gyro.len_r = 6;

imu_aspirin_arch_init();
hmc5843_init();

Expand Down
104 changes: 59 additions & 45 deletions sw/airborne/subsystems/imu/imu_aspirin.h
View file
Expand Up @@ -90,57 +90,71 @@ extern struct ImuAspirin imu_aspirin;
} \
}

static void inline gyro_read_i2c(void)
/* underlying architecture */
#include "subsystems/imu/imu_aspirin_arch.h"
/* must be implemented by underlying architecture */
extern void imu_b2_arch_init(void);

static inline void gyro_read_i2c(void)
{
imu_aspirin.i2c_trans_gyro.buf[0] = ITG3200_REG_GYRO_XOUT_H;
i2c_submit(&i2c2,&imu_aspirin.i2c_trans_gyro);
}

#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
if (imu_aspirin.status != AspirinStatusUninit) { \
ImuMagEvent(_mag_handler); \
imu_aspirin_arch_int_disable(); \
if (imu_aspirin.status == AspirinStatusReadingGyro && \
imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess && i2c_idle(&i2c2)) { \
int16_t gp = imu_aspirin.i2c_trans_gyro.buf[0]<<8 | imu_aspirin.i2c_trans_gyro.buf[1]; \
int16_t gq = imu_aspirin.i2c_trans_gyro.buf[2]<<8 | imu_aspirin.i2c_trans_gyro.buf[3]; \
int16_t gr = imu_aspirin.i2c_trans_gyro.buf[4]<<8 | imu_aspirin.i2c_trans_gyro.buf[5]; \
RATES_ASSIGN(imu.gyro_unscaled, gp, gq, gr); \
imu_aspirin.status = AspirinStatusIdle; \
} \
if (imu_aspirin.gyro_eoc && i2c2.status == I2CIdle && i2c_idle(&i2c2)) { \
if (imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) { \
imu_aspirin.time_since_last_reading = 0; \
} \
imu_aspirin.gyro_eoc = FALSE; \
gyro_read_i2c(); \
} \
if (imu_aspirin.time_since_last_reading > ASPIRIN_GYRO_TIMEOUT) { \
imu_aspirin.gyro_eoc = FALSE; \
i2c2_er_irq_handler(); \
gyro_read_i2c(); \
imu_aspirin.time_since_last_reading = 0; \
} \
if (imu_aspirin.gyro_available_blaaa && imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) { \
imu_aspirin.gyro_available_blaaa = FALSE; \
_gyro_handler(); \
} \
if (imu_aspirin.accel_available) { \
imu_aspirin.accel_available = FALSE; \
const int16_t ax = imu_aspirin.accel_rx_buf[1] | (imu_aspirin.accel_rx_buf[2]<<8); \
const int16_t ay = imu_aspirin.accel_rx_buf[3] | (imu_aspirin.accel_rx_buf[4]<<8); \
const int16_t az = imu_aspirin.accel_rx_buf[5] | (imu_aspirin.accel_rx_buf[6]<<8); \
VECT3_ASSIGN(imu.accel_unscaled, ax, ay, az); \
_accel_handler(); \
} \
imu_aspirin_arch_int_enable(); \
} \
}
static inline void gyro_copy_i2c(void)
{
int16_t gp = imu_aspirin.i2c_trans_gyro.buf[0]<<8 | imu_aspirin.i2c_trans_gyro.buf[1];
int16_t gq = imu_aspirin.i2c_trans_gyro.buf[2]<<8 | imu_aspirin.i2c_trans_gyro.buf[3];
int16_t gr = imu_aspirin.i2c_trans_gyro.buf[4]<<8 | imu_aspirin.i2c_trans_gyro.buf[5];
RATES_ASSIGN(imu.gyro_unscaled, gp, gq, gr);
}

static inline void accel_copy_spi(void)
{
const int16_t ax = imu_aspirin.accel_rx_buf[1] | (imu_aspirin.accel_rx_buf[2]<<8);
const int16_t ay = imu_aspirin.accel_rx_buf[3] | (imu_aspirin.accel_rx_buf[4]<<8);
const int16_t az = imu_aspirin.accel_rx_buf[5] | (imu_aspirin.accel_rx_buf[6]<<8);
VECT3_ASSIGN(imu.accel_unscaled, ax, ay, az);
}

/* underlying architecture */
#include "subsystems/imu/imu_aspirin_arch.h"
/* must be implemented by underlying architecture */
extern void imu_b2_arch_init(void);
static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
{
if (imu_aspirin.status == AspirinStatusUninit) return;

imu_aspirin_arch_int_disable();
ImuMagEvent(_mag_handler);
if (imu_aspirin.status == AspirinStatusReadingGyro &&
imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess && i2c_idle(&i2c2)) {
gyro_copy_i2c();
imu_aspirin.status = AspirinStatusIdle;
}
if (imu_aspirin.gyro_eoc && i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
if (imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) {
imu_aspirin.time_since_last_reading = 0;
}
imu_aspirin.gyro_eoc = FALSE;
gyro_read_i2c();
}
if (imu_aspirin.time_since_last_reading > ASPIRIN_GYRO_TIMEOUT) {
imu_aspirin.gyro_eoc = FALSE;
i2c2_er_irq_handler();
gyro_read_i2c();
imu_aspirin.time_since_last_reading = 0;
}
if (imu_aspirin.gyro_available_blaaa && imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) {
imu_aspirin.gyro_available_blaaa = FALSE;
_gyro_handler();
}
if (imu_aspirin.accel_available) {
imu_aspirin.accel_available = FALSE;
accel_copy_spi();
_accel_handler();
}
imu_aspirin_arch_int_enable();
}

#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
imu_aspirin_event(_gyro_handler, _accel_handler, _mag_handler); \
}

#endif /* IMU_ASPIRIN_H */

0 comments on commit ed22614

Please sign in to comment.