38 #ifndef IMU_INTEGRATION
39 #define IMU_INTEGRATION false
43 #if defined(IMU_GYRO_CALIB) && (defined(IMU_GYRO_P_SIGN) || defined(IMU_GYRO_Q_SIGN) || defined(IMU_GYRO_R_SIGN))
44 #warning "The IMU_GYRO_?_SIGN's aren't compatible with the IMU_GYRO_CALIB define in the airframe"
46 #ifndef IMU_GYRO_P_SIGN
47 #define IMU_GYRO_P_SIGN 1
49 #ifndef IMU_GYRO_Q_SIGN
50 #define IMU_GYRO_Q_SIGN 1
52 #ifndef IMU_GYRO_R_SIGN
53 #define IMU_GYRO_R_SIGN 1
57 #if defined(IMU_GYRO_P_NEUTRAL) || defined(IMU_GYRO_Q_NEUTRAL) || defined(IMU_GYRO_R_NEUTRAL)
58 #define GYRO_NEUTRAL {IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL}
62 #if defined(IMU_GYRO_P_SENS) || defined(IMU_GYRO_Q_SENS) || defined(IMU_GYRO_R_SENS)
63 #define GYRO_SCALE {{IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM, IMU_GYRO_Q_SIGN*IMU_GYRO_Q_SENS_NUM, IMU_GYRO_R_SIGN*IMU_GYRO_R_SENS_NUM}, {IMU_GYRO_P_SENS_DEN, IMU_GYRO_Q_SENS_DEN, IMU_GYRO_R_SENS_DEN}}
67 #if defined(GYRO_NEUTRAL) && defined(GYRO_SCALE)
68 #define IMU_GYRO_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=GYRO_NEUTRAL, .scale=GYRO_SCALE}}
69 #elif defined(GYRO_NEUTRAL)
70 #define IMU_GYRO_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=GYRO_NEUTRAL}}
71 #elif defined(GYRO_SCALE)
72 #define IMU_GYRO_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=GYRO_SCALE}}
73 #elif !defined(IMU_GYRO_CALIB)
74 #define IMU_GYRO_CALIB {}
78 #if defined(IMU_ACCEL_CALIB) && (defined(IMU_ACCEL_X_SIGN) || defined(IMU_ACCEL_Y_SIGN) || defined(IMU_ACCEL_Z_SIGN))
79 #warning "The IMU_ACCEL_?_SIGN's aren't compatible with the IMU_ACCEL_CALIB define in the airframe"
81 #ifndef IMU_ACCEL_X_SIGN
82 #define IMU_ACCEL_X_SIGN 1
84 #ifndef IMU_ACCEL_Y_SIGN
85 #define IMU_ACCEL_Y_SIGN 1
87 #ifndef IMU_ACCEL_Z_SIGN
88 #define IMU_ACCEL_Z_SIGN 1
92 #if defined(IMU_ACCEL_X_NEUTRAL) || defined(IMU_ACCEL_Y_NEUTRAL) || defined(IMU_ACCEL_Z_NEUTRAL)
93 #define ACCEL_NEUTRAL {IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL}
97 #if defined(IMU_ACCEL_X_SENS) || defined(IMU_ACCEL_Y_SENS) || defined(IMU_ACCEL_Z_SENS)
98 #define ACCEL_SCALE {{IMU_ACCEL_X_SIGN*IMU_ACCEL_X_SENS_NUM, IMU_ACCEL_Y_SIGN*IMU_ACCEL_Y_SENS_NUM, IMU_ACCEL_Z_SIGN*IMU_ACCEL_Z_SENS_NUM}, {IMU_ACCEL_X_SENS_DEN, IMU_ACCEL_Y_SENS_DEN, IMU_ACCEL_Z_SENS_DEN}}
102 #if defined(ACCEL_NEUTRAL) && defined(ACCEL_SCALE)
103 #define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=ACCEL_NEUTRAL, .scale=ACCEL_SCALE}}
104 #elif defined(ACCEL_NEUTRAL)
105 #define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=ACCEL_NEUTRAL}}
106 #elif defined(ACCEL_SCALE)
107 #define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=ACCEL_SCALE}}
108 #elif !defined(IMU_ACCEL_CALIB)
109 #define IMU_ACCEL_CALIB {}
113 #if defined(IMU_MAG_CALIB) && (defined(IMU_MAG_X_SIGN) || defined(IMU_MAG_Y_SIGN) || defined(IMU_MAG_Z_SIGN))
114 #warning "The IMU_MAG_?_SIGN's aren't compatible with the IMU_MAG_CALIB define in the airframe"
116 #ifndef IMU_MAG_X_SIGN
117 #define IMU_MAG_X_SIGN 1
119 #ifndef IMU_MAG_Y_SIGN
120 #define IMU_MAG_Y_SIGN 1
122 #ifndef IMU_MAG_Z_SIGN
123 #define IMU_MAG_Z_SIGN 1
127 #if defined(IMU_MAG_X_NEUTRAL) || defined(IMU_MAG_Y_NEUTRAL) || defined(IMU_MAG_Z_NEUTRAL)
128 #define MAG_NEUTRAL {IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL}
132 #if defined(IMU_MAG_X_SENS) || defined(IMU_MAG_Y_SENS) || defined(IMU_MAG_Z_SENS)
133 #define MAG_SCALE {{IMU_MAG_X_SIGN*IMU_MAG_X_SENS_NUM, IMU_MAG_Y_SIGN*IMU_MAG_Y_SENS_NUM, IMU_MAG_Z_SIGN*IMU_MAG_Z_SENS_NUM}, {IMU_MAG_X_SENS_DEN, IMU_MAG_Y_SENS_DEN, IMU_MAG_Z_SENS_DEN}}
137 #if defined(MAG_NEUTRAL) && defined(MAG_SCALE)
138 #define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=MAG_NEUTRAL, .scale=MAG_SCALE}}
139 #elif defined(MAG_NEUTRAL)
140 #define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=MAG_NEUTRAL}}
141 #elif defined(MAG_SCALE)
142 #define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=MAG_SCALE}}
143 #elif !defined(IMU_MAG_CALIB)
144 #define IMU_MAG_CALIB {}
149 #if !defined(IMU_BODY_TO_IMU_PHI) && !defined(IMU_BODY_TO_IMU_THETA) && !defined(IMU_BODY_TO_IMU_PSI)
150 #define IMU_BODY_TO_IMU_PHI 0
151 #define IMU_BODY_TO_IMU_THETA 0
152 #define IMU_BODY_TO_IMU_PSI 0
159 #ifndef IMU_GYRO_ABI_SEND_ID
160 #define IMU_GYRO_ABI_SEND_ID ABI_BROADCAST
165 #ifndef IMU_ACCEL_ABI_SEND_ID
166 #define IMU_ACCEL_ABI_SEND_ID ABI_BROADCAST
171 #ifndef IMU_MAG_ABI_SEND_ID
172 #define IMU_MAG_ABI_SEND_ID ABI_BROADCAST
177 #ifndef IMU_LOG_HIGHSPEED_DEVICE
178 #define IMU_LOG_HIGHSPEED_DEVICE flightrecorder_sdlog
182 #if PERIODIC_TELEMETRY
257 static void send_gyro(
struct transport_tx *trans,
struct link_device *
dev)
265 &gyro_float.
p, &gyro_float.
q, &gyro_float.
r);
301 static void send_mag(
struct transport_tx *trans,
struct link_device *
dev)
322 pprz_msg_send_IMU_MAG_CURRENT_CALIBRATION(trans,
dev, AC_ID,
364 if(i >= gyro_calib_len) {
397 if(i >= accel_calib_len) {
430 if(i >= mag_calib_len) {
463 #if PERIODIC_TELEMETRY
577 if(gyro == NULL || samples < 1)
583 for(
uint8_t i = 0; i < samples; i++) {
588 data = data_filtered;
622 for(
uint8_t i = 0; i < samples-1; i++) {
628 #if IMU_LOG_HIGHSPEED
632 integrated_sensor.
p += f_sample.
p;
633 integrated_sensor.
q += f_sample.
q;
634 integrated_sensor.
r += f_sample.
r;
642 integrated.
p = integrated.
p * (1.f / rate);
643 integrated.
q = integrated.
q * (1.f / rate);
644 integrated.
r = integrated.
r * (1.f / rate);
647 uint16_t dt = (1e6 / rate) * samples;
648 AbiSendMsgIMU_GYRO_INT(sender_id, stamp, &integrated, dt);
654 #if IMU_LOG_HIGHSPEED
663 AbiSendMsgIMU_GYRO(sender_id, stamp, &gyro->
scaled);
671 if(accel == NULL || samples < 1)
677 for(
uint8_t i = 0; i < samples; i++) {
682 data = data_filtered;
716 for(
uint8_t i = 0; i < samples-1; i++) {
722 #if IMU_LOG_HIGHSPEED
726 integrated_sensor.
x += f_sample.
x;
727 integrated_sensor.
y += f_sample.
y;
728 integrated_sensor.
z += f_sample.
z;
736 integrated.
x = integrated.
x * (1.f / rate);
737 integrated.
y = integrated.
y * (1.f / rate);
738 integrated.
z = integrated.
z * (1.f / rate);
741 uint16_t dt = (1e6 / rate) * samples;
742 AbiSendMsgIMU_ACCEL_INT(sender_id, stamp, &integrated, dt);
748 #if IMU_LOG_HIGHSPEED
757 AbiSendMsgIMU_ACCEL(sender_id, stamp, &accel->
scaled);
785 AbiSendMsgIMU_MAG(sender_id, stamp, &mag->
scaled);
825 accel->
abi_id = sender_id;
866 struct Int32RMat new_body_to_imu, diff_body_to_imu;
897 body_to_imu_eulers.
phi =
phi;
913 body_to_imu_eulers.
psi =
psi;
Main include for ABI (AirBorneInterface).
#define ABI_DISABLE
Reserved ABI ID to disable callback.
#define ABI_BROADCAST
Broadcast address.
Event structure to store callbacks in a linked list.
struct FloatVect3 accel_float
struct FloatVect3 mag_float
static const float scale[]
struct Electrical electrical
Interface for electrical status: supply voltage, current, battery status, etc.
float current
current in A
void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
rotate anglular rates by rotation matrix.
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
#define RATES_COPY(_a, _b)
#define RATES_ASSIGN(_ra, _p, _q, _r)
#define EULERS_BFP_OF_REAL(_ei, _ef)
#define RMAT_FLOAT_OF_BFP(_ef, _ei)
#define RMAT_COPY(_o, _i)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define VECT3_COPY(_a, _b)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
int32_t p
in rad/s with INT32_RATE_FRAC
int32_t r
in rad/s with INT32_RATE_FRAC
int32_t q
in rad/s with INT32_RATE_FRAC
#define INT_RATES_ZERO(_e)
#define int32_rmat_of_eulers
Rotation matrix from Euler angles.
#define RATE_FLOAT_OF_BFP(_ai)
void int32_rmat_comp_inv(struct Int32RMat *m_a2b, const struct Int32RMat *m_a2c, const struct Int32RMat *m_b2c)
Composition (multiplication) of two rotation matrices.
void int32_rmat_transp_ratemult(struct Int32Rates *rb, struct Int32RMat *m_b2a, struct Int32Rates *ra)
rotate anglular rates by transposed rotation matrix.
#define INT_VECT3_ZERO(_v)
void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va)
rotate 3D vector by transposed rotation matrix.
void int32_rmat_comp(struct Int32RMat *m_a2c, const struct Int32RMat *m_a2b, const struct Int32RMat *m_b2c)
Composition (multiplication) of two rotation matrices.
static void int32_rmat_identity(struct Int32RMat *rm)
initialises a rotation matrix to identity
#define ACCEL_FLOAT_OF_BFP(_ai)
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
static void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
#define IMU_MAG_ABI_SEND_ID
Which mag measurements to send over telemetry/logging.
#define IMU_GYRO_ABI_SEND_ID
Which gyro measurements to send over telemetry/logging.
#define IMU_MAG_CALIB
Default mag calibration is for single IMU with old format.
struct imu_accel_t * imu_get_accel(uint8_t sender_id, bool create)
Find or create the accel in the imu structure.
static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float rate, float temp)
#define IMU_BODY_TO_IMU_THETA
static void send_mag_raw(struct transport_tx *trans, struct link_device *dev)
#define IMU_GYRO_P_SIGN
By default gyro signs are positive for single IMU with old format or defaults.
static abi_event imu_accel_raw_ev
static abi_event imu_mag_raw_ev
struct imu_gyro_t * imu_get_gyro(uint8_t sender_id, bool create)
Find or create the gyro in the imu structure.
#define IMU_MAG_X_SIGN
By default mag signs are positive for single IMU with old format and defaults.
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 e...
void imu_SetBodyToImuTheta(float theta)
#define IMU_ACCEL_X_SIGN
By default accel signs are positive for single IMU with old format and defaults.
#define IMU_BODY_TO_IMU_PHI
Default body to imu is 0 (radians)
static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float rate, float temp)
#define IMU_ACCEL_CALIB
Default accel calibration is for single IMU with old format.
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...
#define IMU_LOG_HIGHSPEED_DEVICE
By default log highspeed on the flightrecorder.
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 void send_mag_scaled(struct transport_tx *trans, struct link_device *dev)
static void send_mag_current(struct transport_tx *trans, struct link_device *dev)
struct Imu imu
global IMU state
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 ...
#define IMU_BODY_TO_IMU_PSI
#define IMU_GYRO_CALIB
Default gyro calibration is for single IMU with old format.
void imu_init(void)
External functions.
void imu_SetBodyToImuPsi(float psi)
static abi_event imu_gyro_raw_ev
static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
void imu_SetBodyToImuCurrent(float set)
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)
void imu_SetBodyToImuPhi(float phi)
static void send_accel(struct transport_tx *trans, struct link_device *dev)
static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data)
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
static void send_mag(struct transport_tx *trans, struct link_device *dev)
struct imu_mag_t * imu_get_mag(uint8_t sender_id, bool create)
Find or create the mag in the imu structure.
static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
#define IMU_ACCEL_ABI_SEND_ID
Which accel measurements to send over telemetry/logging.
Inertial Measurement Unit interface.
uint32_t last_stamp
Last measurement timestamp for integration.
struct Int32Rates scaled
Last scaled values in body frame.
struct Int32RMat body_to_sensor
Rotation from body to sensor frame (body to imu combined with imu to sensor)
struct imu_calib_t calibrated
Calibration bitmask.
struct FloatVect3 current_scale
Current scaling multiplying.
struct imu_accel_t accels[IMU_MAX_SENSORS]
The accelerometer sensors.
uint8_t abi_id
ABI sensor ID.
bool scale
Scale calibrated.
float temperature
Temperature in degrees celcius.
Butterworth2LowPass filter[3]
Lowpass filter optional.
float filter_freq
Lowpass filter frequency (Hz)
uint32_t last_stamp
Last measurement timestamp for integration.
float filter_sample_freq
Lowpass filter sample frequency (Hz)
struct Int32Vect3 neutral
Neutral values, compensation on unscaled->scaled.
uint8_t mag_abi_send_id
Filter out and send only a specific ABI id in telemetry for the magnetometer.
struct OrientationReps body_to_imu
Rotation from body to imu (all sensors) frame.
bool filter
Enable the lowpass filter.
struct Int32Vect3 unscaled
Last unscaled values in sensor frame.
float temperature
Temperature in degrees celcius.
struct Int32Rates neutral
Neutral values, compensation on unscaled->scaled.
bool current
Current calibrated.
struct Int32RMat body_to_sensor
Rotation from body to sensor frame (body to imu combined with imu to sensor)
bool initialized
Check if the IMU is initialized.
Butterworth2LowPass filter[3]
Lowpass filter optional.
struct imu_calib_t calibrated
Calibration bitmask.
struct Int32Vect3 scale[2]
Scaling, first is numerator and second denominator.
struct Int32Rates scale[2]
Scaling, first is numerator and second denominator.
float filter_sample_freq
Lowpass filter sample frequency (Hz)
struct Int32Vect3 scaled
Last scaled values in body frame.
struct imu_gyro_t gyros[IMU_MAX_SENSORS]
The gyro sensors.
struct Int32RMat body_to_sensor
Rotation from body to sensor frame (body to imu combined with imu to sensor)
struct Int32Rates unscaled
Last unscaled values in sensor frame.
uint8_t abi_id
ABI sensor ID.
struct imu_mag_t mags[IMU_MAX_SENSORS]
The magnetometer sensors.
struct Int32Vect3 scale[2]
Scaling, first is numerator and second denominator.
bool neutral
Neutral values calibrated.
bool b2i_set_current
flag for adjusting body_to_imu via settings.
struct Int32Vect3 neutral
Neutral values, compensation on unscaled->scaled.
struct Int32Vect3 unscaled
Last unscaled values in sensor frame.
float filter_freq
Filter frequency.
struct imu_calib_t calibrated
Calibration bitmask.
uint8_t accel_abi_send_id
Filter out and send only a specific ABI id in telemetry for the accelerometer.
struct Int32Vect3 scaled
Last scaled values in body frame.
bool rotation
Rotation calibrated.
uint8_t gyro_abi_send_id
Filter out and send only a specific ABI id in telemetry for the gyro.
uint8_t abi_id
ABI sensor ID.
abstract IMU interface providing fixed point interface
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
static TRICAL_instance_t mag_calib
struct pprzlog_transport pprzlog_tp
PPRZLOG transport structure.
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.