Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
imu.c File Reference

Inertial Measurement Unit interface. More...

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.
#define IMU_RATE_EST_WINDOW   1000000
 Window to measure the sample rate for the integration (us).
#define IMU_RATE_EST_ALPHA   0.1f
 Low-pass factor of the measured sample rate per window (0.1: averaged over ~10 windows).
#define IMU_RATE_EST_BOUND   0.03f
 Maximum relative deviation of the measured sample rate from the calibrated sample_freq or driver rate.
#define IMU_RATE_EST_JITTER   2000
 Timestamp jitter (us) allowed before a callback interval is seen as a gap with lost samples.
#define IMU_RATE_EST_REPORT   30
 Number of accepted windows after which the measured sample rate is reported once (INFO_MSG).
#define IMU_GYRO_P_SIGN   1
 By default gyro signs are positive for single IMU with old format or defaults.
#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.
#define IMU_ACCEL_X_SIGN   1
 By default accel signs are positive for single IMU with old format and defaults.
#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.
#define IMU_MAG_X_SIGN   1
 By default mag signs are positive for single IMU with old format and defaults.
#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.
#define IMU_BODY_TO_IMU_PHI   0
 Default body to imu is 0 (radians).
#define IMU_BODY_TO_IMU_THETA   0
#define IMU_BODY_TO_IMU_PSI   0
#define IMU_GYRO_ABI_SEND_ID   ABI_BROADCAST
 Which gyro measurements to send over telemetry/logging.
#define IMU_ACCEL_ABI_SEND_ID   ABI_BROADCAST
 Which accel measurements to send over telemetry/logging.
#define IMU_MAG_ABI_SEND_ID   ABI_BROADCAST
 Which mag measurements to send over telemetry/logging.
#define IMU_LOG_HIGHSPEED_DEVICE   flightrecorder_sdlog
 By default log highspeed on the flightrecorder.

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 rate, float temp)
static void imu_accel_raw_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float rate, 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.
static bool imu_filter_init (Butterworth2LowPass filter[3], float freq, float sample_freq, const float init[3])
 Initialize the lowpass filters of a sensor on its first sample.
void imu_init (void)
 External functions.
void imu_set_defaults_gyro (uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct FloatRates *scale_f)
 Set the defaults for a gyro sensor WARNING: Should be called before sensor is publishing messages to ensure correct values.
void imu_set_defaults_accel (uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
 Set the defaults for a accel sensor WARNING: Should be called before sensor is publishing messages to ensure correct values.
void imu_set_defaults_mag (uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
 Set the defaults for a mag sensor WARNING: Should be called before sensor is publishing messages to ensure correct values.
struct imu_gyro_t * imu_get_gyro (uint8_t sender_id, bool create)
 Find or create the gyro in the imu structure.
struct imu_accel_t * imu_get_accel (uint8_t sender_id, bool create)
 Find or create the accel in the imu structure.
struct imu_mag_t * imu_get_mag (uint8_t sender_id, bool create)
 Find or create the mag in the imu structure.
void imu_SetBodyToImuPhi (float phi)
void imu_SetBodyToImuTheta (float theta)
void imu_SetBodyToImuPsi (float psi)
void imu_SetBodyToImuCurrent (bool set)

Variables

struct Imu imu = {0}
 global IMU state
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_ABI_SEND_ID

#define IMU_ACCEL_ABI_SEND_ID   ABI_BROADCAST

Which accel measurements to send over telemetry/logging.

Definition at line 195 of file imu.c.

Referenced by imu_init().

◆ IMU_ACCEL_CALIB

#define IMU_ACCEL_CALIB   {}

Default accel calibration is for single IMU with old format.

Definition at line 138 of file imu.c.

Referenced by imu_init().

◆ 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 111 of file imu.c.

Referenced by imu_init(), and imu_set_defaults_accel().

◆ IMU_ACCEL_Y_SIGN

#define IMU_ACCEL_Y_SIGN   1

Definition at line 114 of file imu.c.

Referenced by imu_init(), and imu_set_defaults_accel().

◆ IMU_ACCEL_Z_SIGN

#define IMU_ACCEL_Z_SIGN   1

Definition at line 117 of file imu.c.

Referenced by imu_init(), and imu_set_defaults_accel().

◆ IMU_BODY_TO_IMU_PHI

#define IMU_BODY_TO_IMU_PHI   0

Default body to imu is 0 (radians).

Definition at line 179 of file imu.c.

Referenced by imu_init(), and imu_SetBodyToImuCurrent().

◆ IMU_BODY_TO_IMU_PSI

#define IMU_BODY_TO_IMU_PSI   0

Definition at line 181 of file imu.c.

Referenced by imu_init(), and imu_SetBodyToImuCurrent().

◆ IMU_BODY_TO_IMU_THETA

#define IMU_BODY_TO_IMU_THETA   0

Definition at line 180 of file imu.c.

Referenced by imu_init(), and imu_SetBodyToImuCurrent().

◆ IMU_GYRO_ABI_SEND_ID

#define IMU_GYRO_ABI_SEND_ID   ABI_BROADCAST

Which gyro measurements to send over telemetry/logging.

Definition at line 189 of file imu.c.

Referenced by imu_init().

◆ IMU_GYRO_CALIB

#define IMU_GYRO_CALIB   {}

Default gyro calibration is for single IMU with old format.

Definition at line 99 of file imu.c.

Referenced by imu_init().

◆ 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 72 of file imu.c.

Referenced by imu_init(), and imu_set_defaults_gyro().

◆ IMU_GYRO_Q_SIGN

#define IMU_GYRO_Q_SIGN   1

Definition at line 75 of file imu.c.

Referenced by imu_init(), and imu_set_defaults_gyro().

◆ IMU_GYRO_R_SIGN

#define IMU_GYRO_R_SIGN   1

Definition at line 78 of file imu.c.

Referenced by imu_init(), and imu_set_defaults_gyro().

◆ IMU_INTEGRATION

#define IMU_INTEGRATION   false

By default disable IMU integration calculations.

Definition at line 39 of file imu.c.

◆ IMU_LOG_HIGHSPEED_DEVICE

#define IMU_LOG_HIGHSPEED_DEVICE   flightrecorder_sdlog

By default log highspeed on the flightrecorder.

Definition at line 207 of file imu.c.

Referenced by imu_accel_raw_cb(), and imu_gyro_raw_cb().

◆ IMU_MAG_ABI_SEND_ID

#define IMU_MAG_ABI_SEND_ID   ABI_BROADCAST

Which mag measurements to send over telemetry/logging.

Definition at line 201 of file imu.c.

Referenced by imu_init().

◆ IMU_MAG_CALIB

#define IMU_MAG_CALIB   {}

Default mag calibration is for single IMU with old format.

Definition at line 173 of file imu.c.

Referenced by imu_init().

◆ 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 146 of file imu.c.

◆ IMU_MAG_Y_SIGN

#define IMU_MAG_Y_SIGN   1

Definition at line 149 of file imu.c.

Referenced by imu_init(), and imu_set_defaults_mag().

◆ IMU_MAG_Z_SIGN

#define IMU_MAG_Z_SIGN   1

Definition at line 152 of file imu.c.

Referenced by imu_init(), and imu_set_defaults_mag().

◆ IMU_RATE_EST_ALPHA

#define IMU_RATE_EST_ALPHA   0.1f

Low-pass factor of the measured sample rate per window (0.1: averaged over ~10 windows).

Definition at line 49 of file imu.c.

◆ IMU_RATE_EST_BOUND

#define IMU_RATE_EST_BOUND   0.03f

Maximum relative deviation of the measured sample rate from the calibrated sample_freq or driver rate.

Definition at line 54 of file imu.c.

◆ IMU_RATE_EST_JITTER

#define IMU_RATE_EST_JITTER   2000

Timestamp jitter (us) allowed before a callback interval is seen as a gap with lost samples.

Definition at line 59 of file imu.c.

◆ IMU_RATE_EST_REPORT

#define IMU_RATE_EST_REPORT   30

Number of accepted windows after which the measured sample rate is reported once (INFO_MSG).

Definition at line 64 of file imu.c.

◆ IMU_RATE_EST_WINDOW

#define IMU_RATE_EST_WINDOW   1000000

Window to measure the sample rate for the integration (us).

Definition at line 44 of file imu.c.

Function Documentation

◆ imu_accel_raw_cb()

◆ imu_filter_init()

bool imu_filter_init ( Butterworth2LowPass filter[3],
float freq,
float sample_freq,
const float init[3] )
static

Initialize the lowpass filters of a sensor on its first sample.

Parameters
filterThe 3 filters of the sensor
freqThe cutoff frequency (Hz)
sample_freqThe sample frequency (Hz)
initThe first sample to start the filters from
Returns
true if the filter is valid, false (filter disabled) if a frequency is not set or invalid

Definition at line 480 of file imu.c.

References init, and init_butterworth_2_low_pass().

Referenced by imu_accel_raw_cb(), and imu_gyro_raw_cb().

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

◆ 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 1193 of file imu.c.

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

Referenced by ahrs_aligner_run(), 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 1170 of file imu.c.

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

Referenced by ahrs_aligner_run(), 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 1217 of file imu.c.

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

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

Here is the caller graph for this function:

◆ imu_gyro_raw_cb()

◆ imu_init()

◆ imu_mag_raw_cb()

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

◆ imu_set_body_to_imu_eulers()

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 1239 of file imu.c.

References EULERS_BFP_OF_REAL, imu, IMU_MAX_SENSORS, int32_rmat_comp(), int32_rmat_comp_inv(), int32_rmat_of_eulers, 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 FloatVect3 * scale_f )

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
scale_fScale values

Definition at line 871 of file imu.c.

References imu_accel_t::abi_id, 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_accel_t::neutral, imu_calib_t::neutral, orientationGetRMat_i(), RMAT_COPY, imu_calib_t::rotation, imu_accel_t::scale_f, imu_calib_t::scale_f, VECT3_ASSIGN, VECT3_COPY, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

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_pixhawk6x_init(), imu_px4_init(), imu_px4fmu_init(), invensense2_fix_config(), invensense3_456_set_scalings(), invensense3_fix_config(), and sensors_hitl_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 FloatRates * scale_f )

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
scale_fScale values

Definition at line 842 of file imu.c.

References imu_gyro_t::abi_id, 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(), FloatRates::p, FloatRates::q, FloatRates::r, RATES_ASSIGN, RATES_COPY, RMAT_COPY, imu_calib_t::rotation, imu_calib_t::scale_f, and imu_gyro_t::scale_f.

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_pixhawk6x_init(), imu_px4_init(), imu_px4fmu_init(), invensense2_fix_config(), invensense3_456_set_scalings(), invensense3_fix_config(), and sensors_hitl_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 FloatVect3 * scale_f )

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
scale_fScale values

Definition at line 900 of file imu.c.

References imu_mag_t::abi_id, 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_f, imu_mag_t::scale_f, VECT3_ASSIGN, VECT3_COPY, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by imu_nps_init(), mag_ist8310_module_init(), mag_lis3mdl_module_init(), mag_uavcan_init(), and sensors_hitl_init().

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

◆ imu_SetBodyToImuCurrent()

void imu_SetBodyToImuCurrent ( bool set)

◆ imu_SetBodyToImuPhi()

void imu_SetBodyToImuPhi ( float phi)

Definition at line 1268 of file imu.c.

References 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 1284 of file imu.c.

References 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 1276 of file imu.c.

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

Here is the call graph for this function:

◆ send_accel()

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

Definition at line 242 of file imu.c.

References ABI_BROADCAST, ABI_DISABLE, accel_float, ACCELS_FLOAT_OF_BFP, dev, imu, and IMU_MAX_SENSORS.

Referenced by imu_init(), and ins_vectornav_init().

Here is the caller graph for this function:

◆ send_accel_raw()

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

Definition at line 214 of file imu.c.

References ABI_BROADCAST, ABI_DISABLE, dev, imu, and IMU_MAX_SENSORS.

Referenced by imu_init().

Here is the caller graph for this function:

◆ send_accel_scaled()

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

Definition at line 228 of file imu.c.

References ABI_BROADCAST, ABI_DISABLE, dev, imu, and IMU_MAX_SENSORS.

Referenced by imu_init(), and ins_vectornav_init().

Here is the caller graph for this function:

◆ send_gyro()

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

Definition at line 286 of file imu.c.

References ABI_BROADCAST, ABI_DISABLE, dev, imu, IMU_MAX_SENSORS, FloatRates::p, FloatRates::q, FloatRates::r, and RATES_FLOAT_OF_BFP.

Referenced by imu_init(), and ins_vectornav_init().

Here is the caller graph for this function:

◆ send_gyro_raw()

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

Definition at line 258 of file imu.c.

References ABI_BROADCAST, ABI_DISABLE, dev, imu, and IMU_MAX_SENSORS.

Referenced by imu_init().

Here is the caller graph for this function:

◆ send_gyro_scaled()

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

Definition at line 272 of file imu.c.

References ABI_BROADCAST, ABI_DISABLE, dev, imu, and IMU_MAX_SENSORS.

Referenced by imu_init(), and ins_vectornav_init().

Here is the caller graph for this function:

◆ send_mag()

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

Definition at line 330 of file imu.c.

References ABI_BROADCAST, ABI_DISABLE, dev, imu, IMU_MAX_SENSORS, mag_float, and MAGS_FLOAT_OF_BFP.

Referenced by imu_init().

Here is the caller graph for this function:

◆ send_mag_current()

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

Definition at line 346 of file imu.c.

References ABI_BROADCAST, ABI_DISABLE, dev, electrical, imu, and IMU_MAX_SENSORS.

Referenced by imu_init().

Here is the caller graph for this function:

◆ send_mag_raw()

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

Definition at line 302 of file imu.c.

References ABI_BROADCAST, ABI_DISABLE, dev, imu, and IMU_MAX_SENSORS.

Referenced by imu_init().

Here is the caller graph for this function:

◆ send_mag_scaled()

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

Definition at line 316 of file imu.c.

References ABI_BROADCAST, ABI_DISABLE, dev, imu, and IMU_MAX_SENSORS.

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 466 of file imu.c.

Referenced by imu_init().

◆ imu_gyro_raw_ev

abi_event imu_gyro_raw_ev
static

Definition at line 466 of file imu.c.

Referenced by imu_init().

◆ imu_mag_raw_ev

abi_event imu_mag_raw_ev
static

Definition at line 466 of file imu.c.

Referenced by imu_init().