Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
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"
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 |
Inertial Measurement Unit interface.
Definition in file imu.c.
#define IMU_ACCEL_ABI_SEND_ID ABI_BROADCAST |
#define IMU_ACCEL_CALIB {} |
#define IMU_ACCEL_X_SIGN 1 |
#define IMU_BODY_TO_IMU_PHI 0 |
#define IMU_GYRO_ABI_SEND_ID ABI_BROADCAST |
#define IMU_GYRO_CALIB {} |
#define IMU_GYRO_P_SIGN 1 |
#define IMU_INTEGRATION false |
#define IMU_LOG_HIGHSPEED_DEVICE flightrecorder_sdlog |
#define IMU_MAG_ABI_SEND_ID ABI_BROADCAST |
#define IMU_MAG_CALIB {} |
#define IMU_MAG_X_SIGN 1 |
|
static |
Definition at line 756 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().
struct imu_accel_t* imu_get_accel | ( | uint8_t | sender_id, |
bool | create | ||
) |
Find or create the accel in the imu structure.
sender_id | The ABI sender id to search for |
create | Create a new index if not found |
Definition at line 907 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().
struct imu_gyro_t* imu_get_gyro | ( | uint8_t | sender_id, |
bool | create | ||
) |
Find or create the gyro in the imu structure.
sender_id | The ABI sender id to search for |
create | Create a new index if not found |
Definition at line 884 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().
Find or create the mag in the imu structure.
sender_id | The ABI sender id to search for |
create | Create a new index if not found |
Definition at line 931 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().
|
static |
Definition at line 662 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().
void imu_init | ( | void | ) |
External functions.
Definition at line 429 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(), shell_add_entry(), and VECT3_ASSIGN.
|
static |
Definition at line 850 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().
|
static |
Set the body to IMU rotation in eulers This will update all the sensor values.
body_to_imu_eulers | 321 Euler angles in radians |
Definition at line 953 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().
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.
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 610 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().
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.
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 580 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().
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.
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 640 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.
Referenced by imu_nps_init(), mag_ist8310_module_init(), mag_lis3mdl_module_init(), navdata_init(), and sensors_hitl_init().
void imu_SetBodyToImuCurrent | ( | float | set | ) |
Definition at line 1006 of file imu.c.
References Imu::b2i_set_current, Imu::body_to_imu, imu, IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_PSI, IMU_BODY_TO_IMU_THETA, imu_set_body_to_imu_eulers(), orientationGetEulers_f(), FloatEulers::phi, stateGetNedToBodyEulers_f(), stateIsAttitudeValid(), and FloatEulers::theta.
void imu_SetBodyToImuPhi | ( | float | phi | ) |
Definition at line 982 of file imu.c.
References Imu::body_to_imu, imu, imu_set_body_to_imu_eulers(), orientationGetEulers_f(), and FloatEulers::phi.
void imu_SetBodyToImuPsi | ( | float | psi | ) |
Definition at line 998 of file imu.c.
References Imu::body_to_imu, imu, imu_set_body_to_imu_eulers(), orientationGetEulers_f(), and FloatEulers::psi.
void imu_SetBodyToImuTheta | ( | float | theta | ) |
Definition at line 990 of file imu.c.
References Imu::body_to_imu, imu, imu_set_body_to_imu_eulers(), orientationGetEulers_f(), and FloatEulers::theta.
|
static |
Definition at line 213 of file imu.c.
References ABI_BROADCAST, ABI_DISABLE, imu_accel_t::abi_id, Imu::accel_abi_send_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().
|
static |
Definition at line 185 of file imu.c.
References ABI_BROADCAST, ABI_DISABLE, imu_accel_t::abi_id, Imu::accel_abi_send_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().
|
static |
Definition at line 199 of file imu.c.
References ABI_BROADCAST, ABI_DISABLE, imu_accel_t::abi_id, Imu::accel_abi_send_id, Imu::accels, dev, imu, IMU_MAX_SENSORS, imu_accel_t::scaled, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.
Referenced by imu_init().
|
static |
Definition at line 257 of file imu.c.
References ABI_BROADCAST, ABI_DISABLE, imu_gyro_t::abi_id, dev, Imu::gyro_abi_send_id, 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().
|
static |
Definition at line 229 of file imu.c.
References ABI_BROADCAST, ABI_DISABLE, imu_gyro_t::abi_id, dev, Imu::gyro_abi_send_id, 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().
|
static |
Definition at line 243 of file imu.c.
References ABI_BROADCAST, ABI_DISABLE, imu_gyro_t::abi_id, dev, Imu::gyro_abi_send_id, Imu::gyros, imu, IMU_MAX_SENSORS, Int32Rates::p, Int32Rates::q, Int32Rates::r, and imu_gyro_t::scaled.
Referenced by imu_init().
|
static |
Definition at line 301 of file imu.c.
References ABI_BROADCAST, ABI_DISABLE, imu_mag_t::abi_id, dev, imu, IMU_MAX_SENSORS, Imu::mag_abi_send_id, mag_float, Imu::mags, MAGS_FLOAT_OF_BFP, imu_mag_t::scaled, FloatVect3::x, FloatVect3::y, and FloatVect3::z.
Referenced by imu_init().
|
static |
Definition at line 317 of file imu.c.
Referenced by imu_init().
|
static |
Definition at line 273 of file imu.c.
References ABI_BROADCAST, ABI_DISABLE, imu_mag_t::abi_id, dev, imu, IMU_MAX_SENSORS, Imu::mag_abi_send_id, Imu::mags, imu_mag_t::unscaled, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.
Referenced by imu_init().
|
static |
Definition at line 287 of file imu.c.
References ABI_BROADCAST, ABI_DISABLE, imu_mag_t::abi_id, dev, imu, IMU_MAX_SENSORS, Imu::mag_abi_send_id, Imu::mags, imu_mag_t::scaled, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.
Referenced by imu_init().
struct Imu imu = {0} |
global IMU state
Definition at line 317 of file imu.c.
Referenced by direct_memory_logger_periodic(), imu_get_accel(), imu_get_gyro(), imu_get_mag(), imu_init(), imu_set_body_to_imu_eulers(), imu_set_defaults_accel(), imu_set_defaults_gyro(), imu_set_defaults_mag(), imu_SetBodyToImuCurrent(), imu_SetBodyToImuPhi(), imu_SetBodyToImuPsi(), imu_SetBodyToImuTheta(), jevois_mavlink_filter_init(), jevois_mavlink_filter_periodic(), logger_uart_periodic(), mag_calib_ukf_run(), send_accel(), send_accel_raw(), send_accel_scaled(), send_gyro(), send_gyro_raw(), send_gyro_scaled(), send_mag(), send_mag_raw(), and send_mag_scaled().
|
static |
Definition at line 423 of file imu.c.
Referenced by imu_init().
|
static |
Definition at line 423 of file imu.c.
Referenced by imu_init().
|
static |
Definition at line 423 of file imu.c.
Referenced by imu_init().