Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
imu.h File Reference

Inertial Measurement Unit interface. More...

#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_orientation_conversion.h"
#include "generated/airframe.h"
+ Include dependency graph for imu.h:

Go to the source code of this file.

Data Structures

struct  Imu
 abstract IMU interface providing fixed point interface More...
 

Macros

#define IMU_BODY_TO_IMU_PHI   0
 
#define IMU_BODY_TO_IMU_THETA   0
 
#define IMU_BODY_TO_IMU_PSI   0
 
#define IMU_GYRO_P_NEUTRAL   0
 
#define IMU_GYRO_Q_NEUTRAL   0
 
#define IMU_GYRO_R_NEUTRAL   0
 
#define IMU_ACCEL_X_NEUTRAL   0
 
#define IMU_ACCEL_Y_NEUTRAL   0
 
#define IMU_ACCEL_Z_NEUTRAL   0
 
#define IMU_GYRO_P_SIGN   1
 
#define IMU_GYRO_Q_SIGN   1
 
#define IMU_GYRO_R_SIGN   1
 
#define IMU_ACCEL_X_SIGN   1
 
#define IMU_ACCEL_Y_SIGN   1
 
#define IMU_ACCEL_Z_SIGN   1
 
#define IMU_MAG_X_SIGN   1
 
#define IMU_MAG_Y_SIGN   1
 
#define IMU_MAG_Z_SIGN   1
 

Functions

void imu_impl_init (void)
 must be defined by underlying hardware More...
 
void imu_periodic (void)
 optional. More...
 
void imu_init (void)
 
void imu_SetBodyToImuPhi (float phi)
 
void imu_SetBodyToImuTheta (float theta)
 
void imu_SetBodyToImuPsi (float psi)
 
void imu_SetBodyToImuCurrent (float set)
 
void imu_ResetBodyToImu (float reset)
 
void imu_scale_gyro (struct Imu *_imu)
 
void imu_scale_accel (struct Imu *_imu)
 
void imu_scale_mag (struct Imu *_imu)
 

Variables

struct Imu imu
 global IMU state More...
 

Detailed Description

Inertial Measurement Unit interface.

Definition in file imu.h.


Data Structure Documentation

struct Imu

abstract IMU interface providing fixed point interface

Definition at line 41 of file imu.h.

+ Collaboration diagram for Imu:
Data Fields
struct Int32Vect3 accel accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
struct Int32Vect3 accel_neutral accelerometer bias
struct Int32Vect3 accel_prev previous accelerometer measurements
struct Int32Vect3 accel_unscaled unscaled accelerometer measurements
bool_t b2i_set_current flag for adjusting body_to_imu via settings.

if FALSE, reset to airframe values, if TRUE set current roll/pitch

struct OrientationReps body_to_imu rotation from body to imu frame
struct Int32Rates gyro gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
struct Int32Rates gyro_neutral gyroscope bias
struct Int32Rates gyro_prev previous gyroscope measurements
struct Int32Rates gyro_unscaled unscaled gyroscope measurements
struct Int32Vect3 mag magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
struct Int32Vect3 mag_neutral magnetometer neutral readings (bias)
struct Int32Vect3 mag_unscaled unscaled magnetometer measurements

Macro Definition Documentation

#define IMU_ACCEL_X_NEUTRAL   0

Definition at line 94 of file imu.h.

#define IMU_ACCEL_X_SIGN   1

Definition at line 105 of file imu.h.

#define IMU_ACCEL_Y_NEUTRAL   0

Definition at line 95 of file imu.h.

#define IMU_ACCEL_Y_SIGN   1

Definition at line 106 of file imu.h.

#define IMU_ACCEL_Z_NEUTRAL   0

Definition at line 96 of file imu.h.

#define IMU_ACCEL_Z_SIGN   1

Definition at line 107 of file imu.h.

#define IMU_BODY_TO_IMU_PHI   0

Definition at line 82 of file imu.h.

Referenced by imu_init(), and imu_SetBodyToImuCurrent().

#define IMU_BODY_TO_IMU_PSI   0

Definition at line 84 of file imu.h.

Referenced by imu_init(), and imu_SetBodyToImuCurrent().

#define IMU_BODY_TO_IMU_THETA   0

Definition at line 83 of file imu.h.

Referenced by imu_init(), and imu_SetBodyToImuCurrent().

#define IMU_GYRO_P_NEUTRAL   0

Definition at line 88 of file imu.h.

#define IMU_GYRO_P_SIGN   1

Definition at line 100 of file imu.h.

#define IMU_GYRO_Q_NEUTRAL   0

Definition at line 89 of file imu.h.

#define IMU_GYRO_Q_SIGN   1

Definition at line 101 of file imu.h.

#define IMU_GYRO_R_NEUTRAL   0

Definition at line 90 of file imu.h.

#define IMU_GYRO_R_SIGN   1

Definition at line 102 of file imu.h.

#define IMU_MAG_X_SIGN   1

Definition at line 110 of file imu.h.

#define IMU_MAG_Y_SIGN   1

Definition at line 111 of file imu.h.

#define IMU_MAG_Z_SIGN   1

Definition at line 112 of file imu.h.

Function Documentation

void imu_impl_init ( void  )

must be defined by underlying hardware

must be defined by underlying hardware

Todo:
drdy int handling for adxl345
Todo:
eoc interrupt for itg3200, polling for now (including status reg)
Todo:
drdy int handling for adxl345
Todo:
eoc interrupt for itg3200, polling for now (including status reg)
Todo:
drdy int handling for adxl345

Definition at line 79 of file imu_apogee.c.

References ImuAspirin::acc_adxl, ImuAspirinI2c::acc_adxl, ImuGL1I2c::acc_adxl, ImuPpzuav::acc_adxl, ImuXsens::accel_available, ImuNps::accel_available, Mpu60x0Config::accel_range, Mpu9250Config::accel_range, ImuKrooz::accel_sum, ImuAspirin::accel_valid, ImuKrooz::ad7689_spi_rx_buffer, ImuKrooz::ad7689_spi_tx_buffer, ImuKrooz::ad7689_trans, adc_buf_channel(), ADS8344_available, ImuNavgo::adxl, ImuUmarim::adxl, ADXL345_ADDR, ADXL345_ADDR_ALT, adxl345_i2c_init(), adxl345_spi_init(), spi_transaction::after_cb, ahrs_gx3, ahrs_gx3_align(), ImuBebop::ak, AK8963_ADDR, ak8963_init(), ami601_init(), analog_imu_adc_buf, APOGEE_ACCEL_RANGE, APOGEE_GYRO_RANGE, APOGEE_LOWPASS_FILTER, APOGEE_SMPLRT_DIV, aspirin2_mpu60x0, ASPIRIN_2_ACCEL_RANGE, ASPIRIN_2_GYRO_RANGE, ASPIRIN_2_SPI_DEV, ASPIRIN_2_SPI_SLAVE_IDX, ASPIRIN_ACCEL_RATE, ASPIRIN_GYRO_LOWPASS, ASPIRIN_GYRO_SMPLRT_DIV, ASPIRIN_I2C_DEV, ASPIRIN_SPI_DEV, ASPIRIN_SPI_SLAVE_IDX, BEBOP_ACCEL_RANGE, BEBOP_GYRO_RANGE, BEBOP_MAG_I2C_DEV, BEBOP_MPU_I2C_DEV, spi_transaction::before_cb, spi_transaction::bitorder, i2c_transaction::buf, buf_out, spi_transaction::cdiv, GX3Packet::chksm_error, UM6Packet::chksm_error, Adxl345_I2c::config, Adxl345_Spi::config, Mpu60x0_Spi::config, Mpu60x0_I2c::config, Mpu9250_I2c::config, L3g4200::config, Itg3200::config, Mpu60x0I2cSlave::configure, Mpu9250I2cSlave::configure, configure_baro_slave(), spi_transaction::cpha, spi_transaction::cpol, L3g4200Config::ctrl_reg1, L3g4200Config::ctrl_reg4, L3g4200Config::ctrl_reg5, data_chk, DefaultPeriodic, Mpu9250Config::dlpf_accel_cfg, Itg3200Config::dlpf_cfg, Mpu60x0Config::dlpf_cfg, Mpu9250Config::dlpf_gyro_cfg, Mpu60x0Config::drdy_int_enable, DROTEK_2_ACCEL_RANGE, DROTEK_2_GYRO_RANGE, DROTEK_2_HMC_I2C_ADDR, DROTEK_2_MPU_I2C_ADDR, spi_transaction::dss, FALSE, GL1_ACCEL_RATE, GL1_GYRO_LOWPASS, GL1_GYRO_SMPLRT, gpio_set(), gpio_setup_output(), GX3PacketWaiting, ImuXsens::gyro_available, ImuNps::gyro_available, ImuAspirin::gyro_itg, ImuAspirinI2c::gyro_itg, ImuPpzuav::gyro_itg, ImuGL1I2c::gyro_l3g, Mpu60x0Config::gyro_range, Mpu9250Config::gyro_range, ImuAspirin::gyro_valid, GX3Packet::hdr_error, UM6Packet::hdr_error, ImuPx4fmu::hmc, ImuMpu6000Hmc5883::hmc, ImuHbmini::hmc, ImuNavgo::hmc, ImuNavstik::hmc, ImuDrotek2::hmc, ImuKrooz::hmc, hmc5843_init(), HMC58XX_ADDR, hmc58xx_init(), ImuKrooz::hmc_eoc, HMC_TYPE_5843, HMC_TYPE_5883, Mpu60x0Config::i2c_bypass, Mpu60x0Config::i2c_mst_clk, Mpu9250Config::i2c_mst_clk, Mpu60x0Config::i2c_mst_delay, Mpu9250Config::i2c_mst_delay, i2c_transmit(), I2CTransPending, imu_apogee, imu_aspirin2, imu_aspirin2_configure_mag_slave(), imu_aspirin_arch_init(), imu_b2, imu_crista_arch_init(), imu_drotek2_configure_mag_slave(), IMU_GX3_LONG_DELAY, imu_hbmini, imu_krooz_sd_arch_init(), IMU_MPU60X0_ACCEL_RANGE, IMU_MPU60X0_GYRO_RANGE, IMU_MPU60X0_I2C_ADDR, imu_mpu9250, IMU_MPU9250_ACCEL_LOWPASS_FILTER, IMU_MPU9250_ACCEL_RANGE, imu_mpu9250_configure_mag_slave(), IMU_MPU9250_GYRO_LOWPASS_FILTER, IMU_MPU9250_GYRO_RANGE, IMU_MPU9250_I2C_ADDR, IMU_MPU9250_SMPLRT_DIV, IMU_MPU_ACCEL_RANGE, IMU_MPU_GYRO_RANGE, imu_mpu_i2c, imu_navgo, imu_nps, imu_overrun, IMU_PPZUAV_ACCEL_RATE, IMU_PPZUAV_GYRO_LOWPASS, IMU_PPZUAV_GYRO_SMPLRT_DIV, IMU_PPZUAV_I2C_DEV, IMU_UM6_COMMUNICATION_REG, IMU_UM6_MISC_CONFIG_REG, imu_xsens, InitMedianFilterRatesInt, InitMedianFilterVect3Int, spi_transaction::input_buf, spi_transaction::input_length, AhrsGX3::is_aligned, ImuNavgo::itg, ImuUmarim::itg, ITG3200_ADDR, ITG3200_ADDR_ALT, itg3200_init(), KROOZ_ACCEL_RANGE, KROOZ_GYRO_RANGE, KROOZ_LOWPASS_FILTER, KROOZ_SMPLRT_DIV, L3G4200_ADDR_ALT, l3g4200_init(), L3G4200_SCALE, ImuXsens::mag_available, ImuNps::mag_available, ImuAspirin::mag_hmc, ImuAspirinI2c::mag_hmc, ImuGL1I2c::mag_hmc, ImuPpzuav::mag_hmc, ImuAspirin::mag_valid, max1168_init(), ImuKrooz::meas_nb, median_mag, ImuMpu60x0::mpu, ImuMpu6000::mpu, ImuPx4fmu::mpu, ImuMpu6000Hmc5883::mpu, ImuAspirin2Spi::mpu, ImuBebop::mpu, ImuNavstik::mpu, ImuMpu9250::mpu, ImuDrotek2::mpu, ImuKrooz::mpu, ImuApogee::mpu, MPU60X0_ADDR, MPU60X0_ADDR_ALT, mpu60x0_i2c_init(), MPU60X0_MST_CLK_400KHZ, MPU60X0_REG_ACCEL_CONFIG, MPU60X0_REG_CONFIG, MPU60X0_REG_GYRO_CONFIG, MPU60X0_REG_PWR_MGMT_1, MPU60X0_REG_SMPLRT_DIV, mpu60x0_spi_init(), mpu9250_i2c_init(), MPU9250_MST_CLK_400KHZ, mpu9250_spi_init(), ImuKrooz::mpu_eoc, mpu_wait_slave4_ready_cb(), ms2100, ms2100_init(), GX3Packet::msg_available, UM6Packet::msg_available, GX3Packet::msg_idx, UM6Packet::msg_idx, navdata_init(), NAVGO_ACCEL_RATE, NAVGO_GYRO_LOWPASS, NAVGO_GYRO_SMPLRT_DIV, NAVSTIK_ACCEL_RANGE, NAVSTIK_GYRO_RANGE, NAVSTIK_MAG_I2C_DEV, NAVSTIK_MPU_I2C_DEV, Mpu60x0Config::nb_bytes, Mpu9250Config::nb_bytes, Mpu60x0Config::nb_slaves, Mpu9250Config::nb_slaves, spi_transaction::output_buf, spi_transaction::output_length, AhrsGX3::packet, PX4FMU_ACCEL_RANGE, PX4FMU_GYRO_RANGE, Adxl345Config::range, Adxl345Config::rate, RATES_ASSIGN, ImuKrooz::rates_sum, register_periodic_telemetry(), spi_transaction::select, ImuAspirin2Spi::slave4_ready, ImuMpu9250::slave4_ready, spi_transaction::slave_idx, Mpu60x0Config::slaves, Mpu9250Config::slaves, Itg3200Config::smplrt_div, Mpu60x0Config::smplrt_div, Mpu9250Config::smplrt_div, spi1, SPI_SLAVE2, SPICphaEdge1, SPICphaEdge2, SPICpolIdleHigh, SPICpolIdleLow, SPIDiv16, SPIDiv64, SPIDss8bit, SPIMSBFirst, SPISelectUnselect, SPITransDone, GX3Packet::status, UM6Packet::status, i2c_transaction::status, spi_transaction::status, TRUE, Hmc58xx::type, uart_put_byte(), UM6_calculate_checksum(), UM6_imu_align(), UM6_packet, UM6_send_packet(), UM6_status, UM6PacketWaiting, UM6Uninit, UMARIM_ACCEL_RANGE, UMARIM_ACCEL_RATE, UMARIM_GYRO_LOWPASS, UMARIM_GYRO_SMPLRT_DIV, VECT3_ASSIGN, ImuAspirin2Spi::wait_slave4_rx_buf, ImuMpu9250::wait_slave4_rx_buf, ImuAspirin2Spi::wait_slave4_trans, ImuMpu9250::wait_slave4_trans, ImuAspirin2Spi::wait_slave4_tx_buf, ImuMpu9250::wait_slave4_tx_buf, and xsens_init().

Referenced by imu_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void imu_periodic ( void  )

optional.

If not provided by implementation, empty function is used

optional.

Read the MPU60x0 every periodic call and the HMC58XX every 10th call.

Definition at line 95 of file imu_apogee.c.

References ImuAspirin::acc_adxl, ImuAspirinI2c::acc_adxl, ImuGL1I2c::acc_adxl, ImuPpzuav::acc_adxl, Imu::accel, ImuKrooz::accel_filtered, ImuKrooz::accel_sum, Imu::accel_unscaled, ImuNavgo::adxl, ImuUmarim::adxl, adxl345_i2c_periodic(), adxl345_spi_periodic(), ImuBebop::ak, ak8963_periodic(), ami601_read(), analog_imu_adc_buf, aspirin2_mpu60x0, i2c_transaction::buf, Mpu60x0_I2c::config, get_sys_time_usec(), Imu::gyro, ImuAspirin::gyro_itg, ImuAspirinI2c::gyro_itg, ImuPpzuav::gyro_itg, ImuGL1I2c::gyro_l3g, Imu::gyro_unscaled, adc_buf::head, ImuPx4fmu::hmc, ImuMpu6000Hmc5883::hmc, ImuHbmini::hmc, ImuNavgo::hmc, ImuNavstik::hmc, ImuDrotek2::hmc, ImuKrooz::hmc, hmc5843_periodic(), hmc58xx_periodic(), hmc58xx_read(), hmc58xx_start_configure(), i2c_transceive(), imu, IMU_ANALOG_ID, imu_apogee, imu_aspirin, imu_aspirin2, imu_b2, imu_bebop, IMU_BOARD_ID, imu_drotek2, imu_gl1, imu_hbmini, IMU_KROOZ_ACCEL_AVG_FILTER, imu_mpu9250, imu_mpu_hmc, imu_mpu_i2c, imu_mpu_spi, imu_navgo, imu_navstik, imu_overrun, imu_ppzuav, imu_px4fmu, imu_scale_accel(), imu_scale_gyro(), imu_umarim, ImuCristaArchPeriodic, Hmc58xx::initialized, Mpu60x0Config::initialized, INT_VECT3_ZERO, ImuNavgo::itg, ImuUmarim::itg, itg3200_periodic(), l3g4200_periodic(), ImuAspirin::mag_hmc, ImuGL1I2c::mag_hmc, ImuAspirinI2c::mag_hmc, ImuPpzuav::mag_hmc, Max1168Periodic, ImuKrooz::meas_nb, ImuKrooz::meas_nb_acc, ImuMpu6000::mpu, ImuMpu60x0::mpu, ImuPx4fmu::mpu, ImuMpu6000Hmc5883::mpu, ImuAspirin2Spi::mpu, ImuBebop::mpu, ImuNavstik::mpu, ImuMpu9250::mpu, ImuDrotek2::mpu, ImuKrooz::mpu, ImuApogee::mpu, MPU60X0_ADDR, mpu60x0_i2c_periodic(), mpu60x0_i2c_start_configure(), MPU60X0_REG_INT_STATUS, mpu60x0_spi_periodic(), mpu9250_i2c_periodic(), mpu9250_spi_periodic(), ms2100, ms2100_periodic(), NAVGO_ACCEL_RATE, Int32Rates::p, PERIODIC_FREQUENCY, Int32Rates::q, Int32Rates::r, RATES_ASSIGN, ImuKrooz::rates_sum, adc_buf::sum, UpdateMedianFilterVect3Int, VECT3_ADD, VECT3_ASSIGN, VECT3_COPY, VECT3_SDIV, VECT3_SMUL, Uint8Vect3::x, Int32Vect3::x, xsens_periodic(), Uint8Vect3::y, Int32Vect3::y, Uint8Vect3::z, and Int32Vect3::z.

Referenced by main_periodic(), main_periodic_task(), and sensors_task().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void imu_ResetBodyToImu ( float  reset)
void imu_SetBodyToImuPhi ( float  phi)

Definition at line 152 of file imu.c.

References Imu::body_to_imu, imu, orientationGetEulers_f(), orientationGetQuat_f(), orientationSetEulers_f(), and FloatEulers::phi.

+ Here is the call graph for this function:

void imu_SetBodyToImuPsi ( float  psi)

Definition at line 170 of file imu.c.

References Imu::body_to_imu, imu, orientationGetEulers_f(), orientationGetQuat_f(), orientationSetEulers_f(), and FloatEulers::psi.

+ Here is the call graph for this function:

void imu_SetBodyToImuTheta ( float  theta)

Definition at line 161 of file imu.c.

References Imu::body_to_imu, imu, orientationGetEulers_f(), orientationGetQuat_f(), orientationSetEulers_f(), and FloatEulers::theta.

+ Here is the call graph for this function:

Variable Documentation