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 #if PERIODIC_TELEMETRY
214 static void send_gyro(
struct transport_tx *trans,
struct link_device *
dev)
220 &gyro_float.
p, &gyro_float.
q, &gyro_float.
r);
246 static void send_mag(
struct transport_tx *trans,
struct link_device *
dev)
261 pprz_msg_send_IMU_MAG_CURRENT_CALIBRATION(trans,
dev, AC_ID,
301 if(i >= gyro_calib_len) {
327 if(i >= accel_calib_len) {
353 if(i >= mag_calib_len) {
386 #if PERIODIC_TELEMETRY
496 if(gyro == NULL || samples < 1)
531 for(
uint8_t i = 0; i < samples-1; i++) {
542 integrated.
p = integrated.
p / samples * ((float)dt * 1e-6f);
543 integrated.
q = integrated.
q / samples * ((float)dt * 1e-6f);
544 integrated.
r = integrated.
r / samples * ((float)dt * 1e-6f);
547 AbiSendMsgIMU_GYRO_INT(sender_id, stamp, &integrated, dt);
554 AbiSendMsgIMU_GYRO(sender_id, stamp, &gyro->
scaled);
562 if(accel == NULL || samples < 1)
597 for(
uint8_t i = 0; i < samples-1; i++) {
608 integrated.
x = integrated.
x / samples * ((float)dt * 1e-6f);
609 integrated.
y = integrated.
y / samples * ((float)dt * 1e-6f);
610 integrated.
z = integrated.
z / samples * ((float)dt * 1e-6f);
613 AbiSendMsgIMU_ACCEL_INT(sender_id, stamp, &integrated, dt);
620 AbiSendMsgIMU_ACCEL(sender_id, stamp, &accel->
scaled);
648 AbiSendMsgIMU_MAG(sender_id, stamp, &mag->
scaled);
688 accel->
abi_id = sender_id;
729 struct Int32RMat new_body_to_imu, diff_body_to_imu;
760 body_to_imu_eulers.
phi =
phi;
776 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_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.
#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...
static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float temp)
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)
#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...
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 imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float temp)
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)
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.
uint32_t last_stamp
Last measurement timestamp for integration.
struct Int32Vect3 neutral
Neutral values, compensation on unscaled->scaled.
struct OrientationReps body_to_imu
Rotation from body to imu (all sensors) frame.
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.
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.
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.
struct imu_calib_t calibrated
Calibration bitmask.
struct Int32Vect3 scaled
Last scaled values in body frame.
bool rotation
Rotation calibrated.
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 TRICAL_instance_t mag_calib
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.