Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
imu.c File Reference
+ Include dependency graph for imu.c:

Go to the source code of this file.

Macros

#define IMU_INTEGRATION   false
 By default disable IMU integration calculations. More...
 
#define IMU_GYRO_P_SIGN   1
 By default gyro signs are positive for single IMU with old format or defaults. More...
 
#define IMU_GYRO_Q_SIGN   1
 
#define IMU_GYRO_R_SIGN   1
 
#define IMU_GYRO_CALIB   {}
 Default gyro calibration is for single IMU with old format. More...
 
#define IMU_ACCEL_X_SIGN   1
 By default accel signs are positive for single IMU with old format and defaults. More...
 
#define IMU_ACCEL_Y_SIGN   1
 
#define IMU_ACCEL_Z_SIGN   1
 
#define IMU_ACCEL_CALIB   {}
 Default accel calibration is for single IMU with old format. More...
 
#define IMU_MAG_X_SIGN   1
 By default mag signs are positive for single IMU with old format and defaults. More...
 
#define IMU_MAG_Y_SIGN   1
 
#define IMU_MAG_Z_SIGN   1
 
#define IMU_MAG_CALIB   {}
 Default mag calibration is for single IMU with old format. More...
 
#define IMU_BODY_TO_IMU_PHI   0
 Default body to imu is 0 (radians) More...
 
#define IMU_BODY_TO_IMU_THETA   0
 
#define IMU_BODY_TO_IMU_PSI   0
 

Functions

static void send_accel_raw (struct transport_tx *trans, struct link_device *dev)
 
static void send_accel_scaled (struct transport_tx *trans, struct link_device *dev)
 
static void send_accel (struct transport_tx *trans, struct link_device *dev)
 
static void send_gyro_raw (struct transport_tx *trans, struct link_device *dev)
 
static void send_gyro_scaled (struct transport_tx *trans, struct link_device *dev)
 
static void send_gyro (struct transport_tx *trans, struct link_device *dev)
 
static void send_mag_raw (struct transport_tx *trans, struct link_device *dev)
 
static void send_mag_scaled (struct transport_tx *trans, struct link_device *dev)
 
static void send_mag (struct transport_tx *trans, struct link_device *dev)
 
static void send_mag_current (struct transport_tx *trans, struct link_device *dev)
 
static void imu_gyro_raw_cb (uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float temp)
 
static void imu_accel_raw_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float temp)
 
static void imu_mag_raw_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data)
 
static void imu_set_body_to_imu_eulers (struct FloatEulers *body_to_imu_eulers)
 Set the body to IMU rotation in eulers This will update all the sensor values. More...
 
void imu_init (void)
 External functions. More...
 
void imu_set_defaults_gyro (uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct Int32Rates *scale)
 Set the defaults for a gyro sensor WARNING: Should be called before sensor is publishing messages to ensure correct values. More...
 
void imu_set_defaults_accel (uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale)
 Set the defaults for a accel sensor WARNING: Should be called before sensor is publishing messages to ensure correct values. More...
 
void imu_set_defaults_mag (uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale)
 Set the defaults for a mag sensor WARNING: Should be called before sensor is publishing messages to ensure correct values. More...
 
struct imu_gyro_timu_get_gyro (uint8_t sender_id, bool create)
 Find or create the gyro in the imu structure. More...
 
struct imu_accel_timu_get_accel (uint8_t sender_id, bool create)
 Find or create the accel in the imu structure. More...
 
struct imu_mag_timu_get_mag (uint8_t sender_id, bool create)
 Find or create the mag in the imu structure. More...
 
void imu_SetBodyToImuPhi (float phi)
 
void imu_SetBodyToImuTheta (float theta)
 
void imu_SetBodyToImuPsi (float psi)
 
void imu_SetBodyToImuCurrent (float set)
 

Variables

struct Imu imu = {0}
 global IMU state More...
 
static abi_event imu_gyro_raw_ev
 
static abi_event imu_accel_raw_ev
 
static abi_event imu_mag_raw_ev
 

Detailed Description

Inertial Measurement Unit interface.

Definition in file imu.c.

Macro Definition Documentation

◆ IMU_ACCEL_CALIB

#define IMU_ACCEL_CALIB   {}

Default accel calibration is for single IMU with old format.

Definition at line 109 of file imu.c.

◆ IMU_ACCEL_X_SIGN

#define IMU_ACCEL_X_SIGN   1

By default accel signs are positive for single IMU with old format and defaults.

Definition at line 82 of file imu.c.

◆ IMU_ACCEL_Y_SIGN

#define IMU_ACCEL_Y_SIGN   1

Definition at line 85 of file imu.c.

◆ IMU_ACCEL_Z_SIGN

#define IMU_ACCEL_Z_SIGN   1

Definition at line 88 of file imu.c.

◆ IMU_BODY_TO_IMU_PHI

#define IMU_BODY_TO_IMU_PHI   0

Default body to imu is 0 (radians)

Definition at line 150 of file imu.c.

◆ IMU_BODY_TO_IMU_PSI

#define IMU_BODY_TO_IMU_PSI   0

Definition at line 152 of file imu.c.

◆ IMU_BODY_TO_IMU_THETA

#define IMU_BODY_TO_IMU_THETA   0

Definition at line 151 of file imu.c.

◆ IMU_GYRO_CALIB

#define IMU_GYRO_CALIB   {}

Default gyro calibration is for single IMU with old format.

Definition at line 74 of file imu.c.

◆ IMU_GYRO_P_SIGN

#define IMU_GYRO_P_SIGN   1

By default gyro signs are positive for single IMU with old format or defaults.

Definition at line 47 of file imu.c.

◆ IMU_GYRO_Q_SIGN

#define IMU_GYRO_Q_SIGN   1

Definition at line 50 of file imu.c.

◆ IMU_GYRO_R_SIGN

#define IMU_GYRO_R_SIGN   1

Definition at line 53 of file imu.c.

◆ IMU_INTEGRATION

#define IMU_INTEGRATION   false

By default disable IMU integration calculations.

Definition at line 39 of file imu.c.

◆ IMU_MAG_CALIB

#define IMU_MAG_CALIB   {}

Default mag calibration is for single IMU with old format.

Definition at line 144 of file imu.c.

◆ IMU_MAG_X_SIGN

#define IMU_MAG_X_SIGN   1

By default mag signs are positive for single IMU with old format and defaults.

Definition at line 117 of file imu.c.

◆ IMU_MAG_Y_SIGN

#define IMU_MAG_Y_SIGN   1

Definition at line 120 of file imu.c.

◆ IMU_MAG_Z_SIGN

#define IMU_MAG_Z_SIGN   1

Definition at line 123 of file imu.c.

Function Documentation

◆ imu_accel_raw_cb()

static void imu_accel_raw_cb ( uint8_t  sender_id,
uint32_t  stamp,
struct Int32Vect3 data,
uint8_t  samples,
float  temp 
)
static

◆ imu_get_accel()

struct imu_accel_t* imu_get_accel ( uint8_t  sender_id,
bool  create 
)

Find or create the accel in the imu structure.

Parameters
sender_idThe ABI sender id to search for
createCreate a new index if not found
Returns
struct imu_accel_t* The accel structure if found/created else NULL

Definition at line 681 of file imu.c.

References ABI_BROADCAST, ABI_DISABLE, imu_accel_t::abi_id, Imu::accels, imu, and IMU_MAX_SENSORS.

Referenced by ahrs_aligner_run(), high_speed_logger_spi_link_periodic(), imu_accel_raw_cb(), imu_quality_assessment_periodic(), and imu_set_defaults_accel().

+ Here is the caller graph for this function:

◆ imu_get_gyro()

struct imu_gyro_t* imu_get_gyro ( uint8_t  sender_id,
bool  create 
)

Find or create the gyro in the imu structure.

Parameters
sender_idThe ABI sender id to search for
createCreate a new index if not found
Returns
struct imu_gyro_t* The gyro structure if found/created else NULL

Definition at line 658 of file imu.c.

References ABI_BROADCAST, ABI_DISABLE, imu_gyro_t::abi_id, Imu::gyros, imu, and IMU_MAX_SENSORS.

Referenced by ahrs_aligner_run(), high_speed_logger_spi_link_periodic(), imu_gyro_raw_cb(), imu_set_defaults_gyro(), and send_aligner().

+ Here is the caller graph for this function:

◆ imu_get_mag()

struct imu_mag_t* imu_get_mag ( uint8_t  sender_id,
bool  create 
)

Find or create the mag in the imu structure.

Parameters
sender_idThe ABI sender id to search for
createCreate a new index if not found
Returns
struct imu_mag_t* The mag structure if found/created else NULL

Definition at line 705 of file imu.c.

References ABI_BROADCAST, ABI_DISABLE, imu_mag_t::abi_id, imu, IMU_MAX_SENSORS, and Imu::mags.

Referenced by ahrs_aligner_run(), high_speed_logger_spi_link_periodic(), imu_mag_raw_cb(), and imu_set_defaults_mag().

+ Here is the caller graph for this function:

◆ imu_gyro_raw_cb()

static void imu_gyro_raw_cb ( uint8_t  sender_id,
uint32_t  stamp,
struct Int32Rates data,
uint8_t  samples,
float  temp 
)
static

◆ imu_init()

void imu_init ( void  )

External functions.

Definition at line 281 of file imu.c.

References ABI_BROADCAST, ABI_DISABLE, imu_gyro_t::abi_id, imu_accel_t::abi_id, imu_mag_t::abi_id, Imu::accels, Imu::body_to_imu, imu_gyro_t::body_to_sensor, imu_accel_t::body_to_sensor, imu_mag_t::body_to_sensor, imu_gyro_t::calibrated, imu_accel_t::calibrated, imu_mag_t::calibrated, imu_calib_t::current, imu_mag_t::current_scale, DefaultPeriodic, Imu::gyros, imu, IMU_ACCEL_CALIB, imu_accel_raw_cb(), imu_accel_raw_ev, IMU_ACCEL_X_SIGN, IMU_ACCEL_Y_SIGN, IMU_ACCEL_Z_SIGN, IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_PSI, IMU_BODY_TO_IMU_THETA, IMU_GYRO_CALIB, IMU_GYRO_P_SIGN, IMU_GYRO_Q_SIGN, IMU_GYRO_R_SIGN, imu_gyro_raw_cb(), imu_gyro_raw_ev, IMU_MAG_CALIB, imu_mag_raw_cb(), imu_mag_raw_ev, IMU_MAG_X_SIGN, IMU_MAG_Y_SIGN, IMU_MAG_Z_SIGN, IMU_MAX_SENSORS, Imu::initialized, int32_rmat_comp(), int32_rmat_identity(), INT_RATES_ZERO, INT_VECT3_ZERO, imu_gyro_t::last_stamp, imu_accel_t::last_stamp, mag_calib, Imu::mags, imu_calib_t::neutral, imu_gyro_t::neutral, imu_accel_t::neutral, imu_mag_t::neutral, orientationGetRMat_i(), orientationSetEulers_f(), RATES_ASSIGN, register_periodic_telemetry(), RMAT_COPY, imu_calib_t::rotation, imu_calib_t::scale, imu_gyro_t::scale, imu_accel_t::scale, imu_mag_t::scale, send_accel(), send_accel_raw(), send_accel_scaled(), send_gyro(), send_gyro_raw(), send_gyro_scaled(), send_mag(), send_mag_current(), send_mag_raw(), send_mag_scaled(), and VECT3_ASSIGN.

+ Here is the call graph for this function:

◆ imu_mag_raw_cb()

static void imu_mag_raw_cb ( uint8_t  sender_id,
uint32_t  stamp,
struct Int32Vect3 data 
)
static

◆ imu_set_body_to_imu_eulers()

static void imu_set_body_to_imu_eulers ( struct FloatEulers body_to_imu_eulers)
static

Set the body to IMU rotation in eulers This will update all the sensor values.

Parameters
body_to_imu_eulers321 Euler angles in radians

Definition at line 727 of file imu.c.

References Imu::accels, Imu::body_to_imu, imu_gyro_t::body_to_sensor, imu_accel_t::body_to_sensor, imu_mag_t::body_to_sensor, EULERS_BFP_OF_REAL, Imu::gyros, imu, IMU_MAX_SENSORS, int32_rmat_comp(), int32_rmat_comp_inv(), int32_rmat_of_eulers, Imu::mags, orientationGetRMat_i(), and orientationSetEulers_f().

Referenced by imu_SetBodyToImuCurrent(), imu_SetBodyToImuPhi(), imu_SetBodyToImuPsi(), and imu_SetBodyToImuTheta().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ imu_set_defaults_accel()

void imu_set_defaults_accel ( uint8_t  abi_id,
const struct Int32RMat imu_to_sensor,
const struct Int32Vect3 neutral,
const struct Int32Vect3 scale 
)

Set the defaults for a accel sensor WARNING: Should be called before sensor is publishing messages to ensure correct values.

Parameters
abi_idThe ABI sender id to set the defaults for
imu_to_sensorImu to sensor rotation matrix
neutralNeutral values
scaleScale values, 0 index is multiply and 1 index is divide

Definition at line 440 of file imu.c.

References imu_accel_t::abi_id, Imu::body_to_imu, imu_accel_t::body_to_sensor, imu_accel_t::calibrated, imu, IMU_ACCEL_X_SIGN, IMU_ACCEL_Y_SIGN, IMU_ACCEL_Z_SIGN, imu_get_accel(), int32_rmat_comp(), imu_calib_t::neutral, imu_accel_t::neutral, orientationGetRMat_i(), RMAT_COPY, imu_calib_t::rotation, imu_calib_t::scale, imu_accel_t::scale, scale, VECT3_ASSIGN, and VECT3_COPY.

Referenced by imu_apogee_init(), imu_aspirin2_init(), imu_aspirin_i2c_init(), imu_aspirin_init(), imu_bebop_init(), imu_bmi088_init(), imu_cube_init(), imu_disco_init(), imu_mpu9250_init(), imu_mpu_hmc_init(), imu_mpu_i2c_init(), imu_mpu_spi_init(), imu_nps_init(), imu_px4_init(), imu_px4fmu_init(), invensense2_fix_config(), and navdata_init().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ imu_set_defaults_gyro()

void imu_set_defaults_gyro ( uint8_t  abi_id,
const struct Int32RMat imu_to_sensor,
const struct Int32Rates neutral,
const struct Int32Rates scale 
)

Set the defaults for a gyro sensor WARNING: Should be called before sensor is publishing messages to ensure correct values.

Parameters
abi_idThe ABI sender id to set the defaults for
imu_to_sensorImu to sensor rotation matrix
neutralNeutral values
scaleScale values, 0 index is multiply and 1 index is divide

Definition at line 410 of file imu.c.

References imu_gyro_t::abi_id, Imu::body_to_imu, imu_gyro_t::body_to_sensor, imu_gyro_t::calibrated, imu, imu_get_gyro(), IMU_GYRO_P_SIGN, IMU_GYRO_Q_SIGN, IMU_GYRO_R_SIGN, int32_rmat_comp(), imu_calib_t::neutral, imu_gyro_t::neutral, orientationGetRMat_i(), RATES_ASSIGN, RATES_COPY, RMAT_COPY, imu_calib_t::rotation, imu_calib_t::scale, imu_gyro_t::scale, and scale.

Referenced by imu_apogee_init(), imu_aspirin2_init(), imu_aspirin_i2c_init(), imu_aspirin_init(), imu_bebop_init(), imu_bmi088_init(), imu_cube_init(), imu_disco_init(), imu_mpu9250_init(), imu_mpu_hmc_init(), imu_mpu_i2c_init(), imu_mpu_spi_init(), imu_nps_init(), imu_px4_init(), imu_px4fmu_init(), invensense2_fix_config(), and navdata_init().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ imu_set_defaults_mag()

void imu_set_defaults_mag ( uint8_t  abi_id,
const struct Int32RMat imu_to_sensor,
const struct Int32Vect3 neutral,
const struct Int32Vect3 scale 
)

Set the defaults for a mag sensor WARNING: Should be called before sensor is publishing messages to ensure correct values.

Parameters
abi_idThe ABI sender id to set the defaults for
imu_to_sensorImu to sensor rotation matrix
neutralNeutral values
scaleScale values, 0 index is multiply and 1 index is divide

Definition at line 470 of file imu.c.

References imu_mag_t::abi_id, Imu::body_to_imu, imu_mag_t::body_to_sensor, imu_mag_t::calibrated, imu, imu_get_mag(), IMU_MAG_X_SIGN, IMU_MAG_Y_SIGN, IMU_MAG_Z_SIGN, int32_rmat_comp(), imu_calib_t::neutral, imu_mag_t::neutral, orientationGetRMat_i(), RMAT_COPY, imu_calib_t::rotation, imu_calib_t::scale, imu_mag_t::scale, scale, VECT3_ASSIGN, and VECT3_COPY.

Referenced by imu_nps_init(), mag_ist8310_module_init(), and navdata_init().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ imu_SetBodyToImuCurrent()

◆ imu_SetBodyToImuPhi()

void imu_SetBodyToImuPhi ( float  phi)

Definition at line 756 of file imu.c.

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

+ Here is the call graph for this function:

◆ imu_SetBodyToImuPsi()

void imu_SetBodyToImuPsi ( float  psi)

Definition at line 772 of file imu.c.

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

+ Here is the call graph for this function:

◆ imu_SetBodyToImuTheta()

void imu_SetBodyToImuTheta ( float  theta)

Definition at line 764 of file imu.c.

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

+ Here is the call graph for this function:

◆ send_accel()

static void send_accel ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 182 of file imu.c.

References ABI_DISABLE, imu_accel_t::abi_id, accel_float, Imu::accels, ACCELS_FLOAT_OF_BFP, dev, imu, IMU_MAX_SENSORS, imu_accel_t::scaled, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by imu_init().

+ Here is the caller graph for this function:

◆ send_accel_raw()

static void send_accel_raw ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 162 of file imu.c.

References ABI_DISABLE, imu_accel_t::abi_id, Imu::accels, dev, imu, IMU_MAX_SENSORS, imu_accel_t::temperature, imu_accel_t::unscaled, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by imu_init().

+ Here is the caller graph for this function:

◆ send_accel_scaled()

static void send_accel_scaled ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 172 of file imu.c.

References ABI_DISABLE, imu_accel_t::abi_id, Imu::accels, dev, imu, IMU_MAX_SENSORS, imu_accel_t::scaled, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by imu_init().

+ Here is the caller graph for this function:

◆ send_gyro()

static void send_gyro ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 214 of file imu.c.

References ABI_DISABLE, imu_gyro_t::abi_id, dev, Imu::gyros, imu, IMU_MAX_SENSORS, FloatRates::p, FloatRates::q, FloatRates::r, RATES_FLOAT_OF_BFP, and imu_gyro_t::scaled.

Referenced by imu_init().

+ Here is the caller graph for this function:

◆ send_gyro_raw()

static void send_gyro_raw ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 194 of file imu.c.

References ABI_DISABLE, imu_gyro_t::abi_id, dev, Imu::gyros, imu, IMU_MAX_SENSORS, Int32Rates::p, Int32Rates::q, Int32Rates::r, imu_gyro_t::temperature, and imu_gyro_t::unscaled.

Referenced by imu_init().

+ Here is the caller graph for this function:

◆ send_gyro_scaled()

static void send_gyro_scaled ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 204 of file imu.c.

References ABI_DISABLE, imu_gyro_t::abi_id, dev, Imu::gyros, imu, IMU_MAX_SENSORS, Int32Rates::p, Int32Rates::q, Int32Rates::r, and imu_gyro_t::scaled.

Referenced by imu_init().

+ Here is the caller graph for this function:

◆ send_mag()

static void send_mag ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 246 of file imu.c.

References ABI_DISABLE, imu_mag_t::abi_id, dev, imu, IMU_MAX_SENSORS, mag_float, Imu::mags, MAGS_FLOAT_OF_BFP, imu_mag_t::scaled, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by imu_init().

+ Here is the caller graph for this function:

◆ send_mag_current()

static void send_mag_current ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 258 of file imu.c.

References ABI_DISABLE, imu_mag_t::abi_id, Electrical::current, dev, electrical, imu, IMU_MAX_SENSORS, Imu::mags, imu_mag_t::unscaled, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by imu_init().

+ Here is the caller graph for this function:

◆ send_mag_raw()

static void send_mag_raw ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 226 of file imu.c.

References ABI_DISABLE, imu_mag_t::abi_id, dev, imu, IMU_MAX_SENSORS, Imu::mags, imu_mag_t::unscaled, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by imu_init().

+ Here is the caller graph for this function:

◆ send_mag_scaled()

static void send_mag_scaled ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 236 of file imu.c.

References ABI_DISABLE, imu_mag_t::abi_id, dev, imu, IMU_MAX_SENSORS, Imu::mags, imu_mag_t::scaled, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by imu_init().

+ Here is the caller graph for this function:

Variable Documentation

◆ imu

◆ imu_accel_raw_ev

abi_event imu_accel_raw_ev
static

Definition at line 275 of file imu.c.

Referenced by imu_init().

◆ imu_gyro_raw_ev

abi_event imu_gyro_raw_ev
static

Definition at line 275 of file imu.c.

Referenced by imu_init().

◆ imu_mag_raw_ev

abi_event imu_mag_raw_ev
static

Definition at line 275 of file imu.c.

Referenced by imu_init().