Inertial Measurement Unit interface. More...

#include "modules/imu/imu.h"
#include "state.h"
#include "modules/core/abi.h"
#include "modules/energy/electrical.h"
#include "modules/datalink/telemetry.h"

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

#define	IMU_GYRO_ABI_SEND_ID ABI_BROADCAST
	Which gyro measurements to send over telemetry/logging. More...

#define	IMU_ACCEL_ABI_SEND_ID ABI_BROADCAST
	Which accel measurements to send over telemetry/logging. More...

#define	IMU_MAG_ABI_SEND_ID ABI_BROADCAST
	Which mag measurements to send over telemetry/logging. More...

#define	IMU_LOG_HIGHSPEED_DEVICE flightrecorder_sdlog
	By default log highspeed on the flightrecorder. More...

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. 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_t *	imu_get_gyro (uint8_t sender_id, bool create)
	Find or create the gyro in the imu structure. More...

struct imu_accel_t *	imu_get_accel (uint8_t sender_id, bool create)
	Find or create the accel in the imu structure. More...

struct imu_mag_t *	imu_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_ABI_SEND_ID

#define IMU_ACCEL_ABI_SEND_ID ABI_BROADCAST

Which accel measurements to send over telemetry/logging.

Definition at line 166 of file imu.c.

◆ 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_ABI_SEND_ID

#define IMU_GYRO_ABI_SEND_ID ABI_BROADCAST

Which gyro measurements to send over telemetry/logging.

Definition at line 160 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_LOG_HIGHSPEED_DEVICE

#define IMU_LOG_HIGHSPEED_DEVICE flightrecorder_sdlog

By default log highspeed on the flightrecorder.

Definition at line 178 of file imu.c.

◆ IMU_MAG_ABI_SEND_ID

#define IMU_MAG_ABI_SEND_ID ABI_BROADCAST

Which mag measurements to send over telemetry/logging.

Definition at line 172 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	rate,
		float	temp
	)

static

Definition at line 667 of file imu.c.

References ACCEL_FLOAT_OF_BFP, ACCELS_FLOAT_OF_BFP, imu_accel_t::body_to_sensor, imu_accel_t::calibrated, imu_calib_t::filter, imu_accel_t::filter, float_rmat_transp_vmult(), float_rmat_vmult(), imu_get_accel(), IMU_LOG_HIGHSPEED_DEVICE, int32_rmat_transp_vmult(), imu_accel_t::last_stamp, imu_accel_t::neutral, pprzlog_tp, RMAT_FLOAT_OF_BFP, imu_accel_t::scale, imu_accel_t::scaled, imu_accel_t::temperature, imu_accel_t::unscaled, update_butterworth_2_low_pass(), VECT3_COPY, FloatVect3::x, Int32Vect3::x, FloatVect3::y, Int32Vect3::y, FloatVect3::z, and Int32Vect3::z.

Referenced by imu_init().

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_id	The ABI sender id to search for
create	Create a new index if not found

Returns: struct imu_accel_t* The accel structure if found/created else NULL

Definition at line 818 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_id	The ABI sender id to search for
create	Create a new index if not found

Returns: struct imu_gyro_t* The gyro structure if found/created else NULL

Definition at line 795 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_id	The ABI sender id to search for
create	Create a new index if not found

Returns: struct imu_mag_t* The mag structure if found/created else NULL

Definition at line 842 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	rate,
		float	temp
	)

static

Definition at line 573 of file imu.c.

References imu_gyro_t::body_to_sensor, imu_gyro_t::calibrated, imu_calib_t::filter, imu_gyro_t::filter, float_rmat_ratemult(), float_rmat_transp_ratemult(), imu_get_gyro(), IMU_LOG_HIGHSPEED_DEVICE, int32_rmat_transp_ratemult(), imu_gyro_t::last_stamp, imu_gyro_t::neutral, FloatRates::p, Int32Rates::p, p, pprzlog_tp, FloatRates::q, Int32Rates::q, FloatRates::r, Int32Rates::r, RATE_FLOAT_OF_BFP, RATES_COPY, RATES_FLOAT_OF_BFP, RMAT_FLOAT_OF_BFP, imu_gyro_t::scale, imu_gyro_t::scaled, imu_gyro_t::temperature, imu_gyro_t::unscaled, and update_butterworth_2_low_pass().

Referenced by imu_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ imu_init()

void imu_init ( void )

External functions.

Definition at line 344 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::accel_abi_send_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_calib_t::filter, imu_gyro_t::filter, imu_accel_t::filter, imu_gyro_t::filter_freq, imu_accel_t::filter_freq, imu_gyro_t::filter_sample_freq, imu_accel_t::filter_sample_freq, Imu::gyro_abi_send_id, Imu::gyros, imu, IMU_ACCEL_ABI_SEND_ID, 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_ABI_SEND_ID, 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_ABI_SEND_ID, 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, init_butterworth_2_low_pass(), Imu::initialized, int32_rmat_comp(), int32_rmat_identity(), INT_RATES_ZERO, INT_VECT3_ZERO, imu_gyro_t::last_stamp, imu_accel_t::last_stamp, Imu::mag_abi_send_id, 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

Definition at line 761 of file imu.c.

References imu_mag_t::body_to_sensor, Electrical::current, imu_mag_t::current_scale, electrical, imu_get_mag(), int32_rmat_transp_vmult(), imu_mag_t::neutral, imu_mag_t::scale, imu_mag_t::scaled, imu_mag_t::unscaled, VECT3_COPY, FloatVect3::x, Int32Vect3::x, FloatVect3::y, Int32Vect3::y, FloatVect3::z, and Int32Vect3::z.

Referenced by imu_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ 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_eulers 321 Euler angles in radians

Definition at line 864 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_id	The ABI sender id to set the defaults for
imu_to_sensor	Imu to sensor rotation matrix
neutral	Neutral values
scale	Scale values, 0 index is multiply and 1 index is divide

Definition at line 521 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_accel_t::scale, scale, imu_calib_t::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_pixhawk6x_init(), imu_px4_init(), imu_px4fmu_init(), invensense2_fix_config(), invensense3_fix_config(), navdata_init(), 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 Int32Rates *	scale
	)

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

Parameters

abi_id	The ABI sender id to set the defaults for
imu_to_sensor	Imu to sensor rotation matrix
neutral	Neutral values
scale	Scale values, 0 index is multiply and 1 index is divide

Definition at line 491 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, scale, imu_calib_t::scale, and imu_gyro_t::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_pixhawk6x_init(), imu_px4_init(), imu_px4fmu_init(), invensense2_fix_config(), invensense3_fix_config(), navdata_init(), 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 Int32Vect3 *	scale
	)

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

Parameters

abi_id	The ABI sender id to set the defaults for
imu_to_sensor	Imu to sensor rotation matrix
neutral	Neutral values
scale	Scale values, 0 index is multiply and 1 index is divide

Definition at line 551 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_mag_t::scale, scale, imu_calib_t::scale, VECT3_ASSIGN, and VECT3_COPY.