38#ifndef IMU_INTEGRATION
39#define IMU_INTEGRATION false
43#ifndef IMU_RATE_EST_WINDOW
44#define IMU_RATE_EST_WINDOW 1000000
48#ifndef IMU_RATE_EST_ALPHA
49#define IMU_RATE_EST_ALPHA 0.1f
53#ifndef IMU_RATE_EST_BOUND
54#define IMU_RATE_EST_BOUND 0.03f
58#ifndef IMU_RATE_EST_JITTER
59#define IMU_RATE_EST_JITTER 2000
63#ifndef IMU_RATE_EST_REPORT
64#define IMU_RATE_EST_REPORT 30
68#if defined(IMU_GYRO_CALIB) && (defined(IMU_GYRO_P_SIGN) || defined(IMU_GYRO_Q_SIGN) || defined(IMU_GYRO_R_SIGN))
69#warning "The IMU_GYRO_?_SIGN's aren't compatible with the IMU_GYRO_CALIB define in the airframe"
71#ifndef IMU_GYRO_P_SIGN
72#define IMU_GYRO_P_SIGN 1
74#ifndef IMU_GYRO_Q_SIGN
75#define IMU_GYRO_Q_SIGN 1
77#ifndef IMU_GYRO_R_SIGN
78#define IMU_GYRO_R_SIGN 1
82#if defined(IMU_GYRO_P_NEUTRAL) || defined(IMU_GYRO_Q_NEUTRAL) || defined(IMU_GYRO_R_NEUTRAL)
83#define GYRO_NEUTRAL {IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL}
87#if defined(IMU_GYRO_P_SENS) || defined(IMU_GYRO_Q_SENS) || defined(IMU_GYRO_R_SENS)
88#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}}
92#if defined(GYRO_NEUTRAL) && defined(GYRO_SCALE)
93#define IMU_GYRO_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=GYRO_NEUTRAL, .scale=GYRO_SCALE}}
94#elif defined(GYRO_NEUTRAL)
95#define IMU_GYRO_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=GYRO_NEUTRAL}}
96#elif defined(GYRO_SCALE)
97#define IMU_GYRO_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=GYRO_SCALE}}
98#elif !defined(IMU_GYRO_CALIB)
99#define IMU_GYRO_CALIB {}
102#if defined(IMU_IMU_ACCEL_CALIB)
103#error IMU_IMU_ACCEL_CALIB defined. Rename it "ACCEL_CALIB" if your calibration is in a section with the "IMU_" prefix.
107#if defined(IMU_ACCEL_CALIB) && (defined(IMU_ACCEL_X_SIGN) || defined(IMU_ACCEL_Y_SIGN) || defined(IMU_ACCEL_Z_SIGN))
108#warning "The IMU_ACCEL_?_SIGN's aren't compatible with the IMU_ACCEL_CALIB define in the airframe"
110#ifndef IMU_ACCEL_X_SIGN
111#define IMU_ACCEL_X_SIGN 1
113#ifndef IMU_ACCEL_Y_SIGN
114#define IMU_ACCEL_Y_SIGN 1
116#ifndef IMU_ACCEL_Z_SIGN
117#define IMU_ACCEL_Z_SIGN 1
121#if defined(IMU_ACCEL_X_NEUTRAL) || defined(IMU_ACCEL_Y_NEUTRAL) || defined(IMU_ACCEL_Z_NEUTRAL)
122#define ACCEL_NEUTRAL {IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL}
126#if defined(IMU_ACCEL_X_SENS) || defined(IMU_ACCEL_Y_SENS) || defined(IMU_ACCEL_Z_SENS)
127#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}}
131#if defined(ACCEL_NEUTRAL) && defined(ACCEL_SCALE)
132#define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=ACCEL_NEUTRAL, .scale=ACCEL_SCALE}}
133#elif defined(ACCEL_NEUTRAL)
134#define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=ACCEL_NEUTRAL}}
135#elif defined(ACCEL_SCALE)
136#define IMU_ACCEL_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=ACCEL_SCALE}}
137#elif !defined(IMU_ACCEL_CALIB)
138#define IMU_ACCEL_CALIB {}
142#if defined(IMU_MAG_CALIB) && (defined(IMU_MAG_X_SIGN) || defined(IMU_MAG_Y_SIGN) || defined(IMU_MAG_Z_SIGN))
143#warning "The IMU_MAG_?_SIGN's aren't compatible with the IMU_MAG_CALIB define in the airframe"
145#ifndef IMU_MAG_X_SIGN
146#define IMU_MAG_X_SIGN 1
148#ifndef IMU_MAG_Y_SIGN
149#define IMU_MAG_Y_SIGN 1
151#ifndef IMU_MAG_Z_SIGN
152#define IMU_MAG_Z_SIGN 1
156#if defined(IMU_MAG_X_NEUTRAL) || defined(IMU_MAG_Y_NEUTRAL) || defined(IMU_MAG_Z_NEUTRAL)
157#define MAG_NEUTRAL {IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL}
161#if defined(IMU_MAG_X_SENS) || defined(IMU_MAG_Y_SENS) || defined(IMU_MAG_Z_SENS)
162#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}}
166#if defined(MAG_NEUTRAL) && defined(MAG_SCALE)
167#define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true, .scale=true}, .neutral=MAG_NEUTRAL, .scale=MAG_SCALE}}
168#elif defined(MAG_NEUTRAL)
169#define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.neutral=true}, .neutral=MAG_NEUTRAL}}
170#elif defined(MAG_SCALE)
171#define IMU_MAG_CALIB {{.abi_id=ABI_BROADCAST, .calibrated={.scale=true}, .scale=MAG_SCALE}}
172#elif !defined(IMU_MAG_CALIB)
173#define IMU_MAG_CALIB {}
178#if !defined(IMU_BODY_TO_IMU_PHI) && !defined(IMU_BODY_TO_IMU_THETA) && !defined(IMU_BODY_TO_IMU_PSI)
179#define IMU_BODY_TO_IMU_PHI 0
180#define IMU_BODY_TO_IMU_THETA 0
181#define IMU_BODY_TO_IMU_PSI 0
188#ifndef IMU_GYRO_ABI_SEND_ID
189#define IMU_GYRO_ABI_SEND_ID ABI_BROADCAST
194#ifndef IMU_ACCEL_ABI_SEND_ID
195#define IMU_ACCEL_ABI_SEND_ID ABI_BROADCAST
200#ifndef IMU_MAG_ABI_SEND_ID
201#define IMU_MAG_ABI_SEND_ID ABI_BROADCAST
206#ifndef IMU_LOG_HIGHSPEED_DEVICE
207#define IMU_LOG_HIGHSPEED_DEVICE flightrecorder_sdlog
211#if PERIODIC_TELEMETRY
219 pprz_msg_send_IMU_ACCEL_RAW(trans,
dev, AC_ID, &
imu.accels[
id].abi_id, &
imu.accels[
id].temperature,
220 &
imu.accels[
id].unscaled.x, &
imu.accels[
id].unscaled.y, &
imu.accels[
id].unscaled.z);
233 pprz_msg_send_IMU_ACCEL_SCALED(trans,
dev, AC_ID, &
imu.accels[
id].abi_id,
234 &
imu.accels[
id].scaled.x, &
imu.accels[
id].scaled.y, &
imu.accels[
id].scaled.z);
249 pprz_msg_send_IMU_ACCEL(trans,
dev, AC_ID, &
imu.accels[
id].abi_id,
263 pprz_msg_send_IMU_GYRO_RAW(trans,
dev, AC_ID, &
imu.gyros[
id].abi_id, &
imu.gyros[
id].temperature,
264 &
imu.gyros[
id].unscaled.p, &
imu.gyros[
id].unscaled.q, &
imu.gyros[
id].unscaled.r);
277 pprz_msg_send_IMU_GYRO_SCALED(trans,
dev, AC_ID, &
imu.gyros[
id].abi_id,
278 &
imu.gyros[
id].scaled.p, &
imu.gyros[
id].scaled.q, &
imu.gyros[
id].scaled.r);
286static void send_gyro(
struct transport_tx *trans,
struct link_device *
dev)
293 pprz_msg_send_IMU_GYRO(trans,
dev, AC_ID, &
imu.gyros[
id].abi_id,
294 &gyro_float.
p, &gyro_float.
q, &gyro_float.
r);
307 pprz_msg_send_IMU_MAG_RAW(trans,
dev, AC_ID, &
imu.mags[
id].abi_id,
308 &
imu.mags[
id].unscaled.x, &
imu.mags[
id].unscaled.y, &
imu.mags[
id].unscaled.z);
321 pprz_msg_send_IMU_MAG_SCALED(trans,
dev, AC_ID, &
imu.mags[
id].abi_id ,
322 &
imu.mags[
id].scaled.x, &
imu.mags[
id].scaled.y, &
imu.mags[
id].scaled.z);
330static void send_mag(
struct transport_tx *trans,
struct link_device *
dev)
337 pprz_msg_send_IMU_MAG(trans,
dev, AC_ID, &
imu.mags[
id].abi_id,
351 pprz_msg_send_IMU_MAG_CURRENT_CALIBRATION(trans,
dev, AC_ID,
352 &
imu.mags[
id].abi_id,
353 &
imu.mags[
id].unscaled.x,
354 &
imu.mags[
id].unscaled.y,
355 &
imu.mags[
id].unscaled.z,
372 chprintf(sh,
" calibrated: neutral %d, scale %d, scale_f %d, rotation %d, current %d, filter %d\r\n",
377 chprintf(sh,
" %s: %ld, %ld, %ld\r\n", name, v->
x, v->
y, v->
z);
381 chprintf(sh,
" %s: %f, %f, %f\r\n", name, v->
x, v->
y, v->
z);
385 chprintf(sh,
" %s: %ld, %ld, %ld\r\n", name, r->
p, r->
q, r->
r);
389 chprintf(sh,
" %s: %f, %f, %f\r\n", name, r->
p, r->
q, r->
r);
395 chprintf(sh,
" %ld, %ld, %ld\r\n",
MAT33_ELMT(*m, 0, 0),
MAT33_ELMT(*m, 0, 1),
MAT33_ELMT(*m, 0, 2));
396 chprintf(sh,
" %ld, %ld, %ld\r\n",
MAT33_ELMT(*m, 1, 0),
MAT33_ELMT(*m, 1, 1),
MAT33_ELMT(*m, 1, 2));
397 chprintf(sh,
" %ld, %ld, %ld\r\n",
MAT33_ELMT(*m, 2, 0),
MAT33_ELMT(*m, 2, 1),
MAT33_ELMT(*m, 2, 2));
400static void cmd_imu(
shell_stream_t *sh,
int argc,
const char *
const argv[])
410 if (
imu.gyros[i].abi_id != 0) {
412 chprintf(sh,
" Gyro id: %u, time %lu, measured rate %.2f Hz (0: not measured yet)\r\n",
imu.gyros[i].abi_id,
imu.gyros[i].last_stamp,
imu.gyros[i].rate_est.rate);
414 chprintf(sh,
" Gyro id: %u, time %lu\r\n",
imu.gyros[i].abi_id,
imu.gyros[i].last_stamp);
416 show_calibrated(sh, &
imu.gyros[i].calibrated);
417 show_rates(sh,
"neutral", &
imu.gyros[i].neutral);
418 show_ratesf(sh,
"scale", &
imu.gyros[i].scale_f);
419 show_matrix(sh,
"body_to_sensor", &
imu.gyros[i].body_to_sensor);
420 show_rates(sh,
"unscaled", &
imu.gyros[i].unscaled);
421 show_rates(sh,
"scaled", &
imu.gyros[i].scaled);
424 chprintf(sh,
" -> gyro (rad/s): %.3f, %.3f, %.3f\r\n", gf.p, gf.q, gf.r);
429 if (
imu.accels[i].abi_id != 0) {
431 chprintf(sh,
" Accel id: %u, time %lu, measured rate %.2f Hz (0: not measured yet)\r\n",
imu.accels[i].abi_id,
imu.accels[i].last_stamp,
imu.accels[i].rate_est.rate);
433 chprintf(sh,
" Accel id: %u, time %lu\r\n",
imu.accels[i].abi_id,
imu.accels[i].last_stamp);
435 show_calibrated(sh, &
imu.accels[i].calibrated);
436 show_vect3(sh,
"neutral", &
imu.accels[i].neutral);
437 show_vect3f(sh,
"scale", &
imu.accels[i].scale_f);
438 show_matrix(sh,
"body_to_sensor", &
imu.accels[i].body_to_sensor);
439 show_vect3(sh,
"unscaled", &
imu.accels[i].unscaled);
440 show_vect3(sh,
"scaled", &
imu.accels[i].scaled);
443 chprintf(sh,
" -> accel (m/s2): %.3f, %.3f, %.3f\r\n", af.x, af.y, af.z);
448 if (
imu.mags[i].abi_id != 0) {
449 chprintf(sh,
" Mag id: %u\r\n",
imu.mags[i].abi_id);
450 show_calibrated(sh, &
imu.mags[i].calibrated);
451 show_vect3(sh,
"neutral", &
imu.mags[i].neutral);
452 show_vect3f(sh,
"scale", &
imu.mags[i].scale_f);
453 show_matrix(sh,
"body_to_sensor", &
imu.mags[i].body_to_sensor);
454 show_vect3(sh,
"unscaled", &
imu.mags[i].unscaled);
455 show_vect3(sh,
"scaled", &
imu.mags[i].scaled);
458 chprintf(sh,
" -> mag (unit): %.3f, %.3f, %.3f\r\n", mf.x, mf.y, mf.z);
482 if (!(freq > 0.f) || !(sample_freq > 0.f) || !isfinite(sample_freq)) {
485 float tau = 1.0 / (2.0 * M_PI * freq);
486 float sample_time = 1.0 / sample_freq;
487 for (
uint8_t j = 0; j < 3; j++) {
506static void imu_rate_est_info(
const char *name,
uint8_t id,
float rate,
float ref,
bool rejected)
511 len = snprintf(msg,
sizeof(msg),
"IMU %s %u: measured rate %.2f Hz rejected, too far from %.2f Hz (check sample_freq)",
512 name,
id, rate,
ref);
514 len = snprintf(msg,
sizeof(msg),
"IMU %s %u: measured rate %.2f Hz (ref %.2f Hz)", name,
id, rate,
ref);
519 len =
Min(len, (
int)
sizeof(msg) - 1);
521#if FLIGHTRECORDER_SDLOG
522 pprz_msg_send_INFO_MSG(&
pprzlog_tp.trans_tx, &flightrecorder_sdlog.device, AC_ID, len, msg);
546 if(!isfinite(
ref) || !(
ref > 0.f)) {
551 if(last_stamp == 0 || stamp <= last_stamp) {
559 if((
float)(stamp - last_stamp) > max_interval) {
567 const float window_rate = (float)est->
samples * 1e6f / (
float)
window;
568 if(fabsf(window_rate -
ref) <= bound) {
571 imu_rate_est_info(name,
id, est->
rate,
ref,
false);
575 imu_rate_est_info(name,
id, window_rate,
ref,
true);
610 *last_sample = *sample;
631 && !(
imu.accels[i].calibrated.neutral &&
imu.accels[i].calibrated.scale_f)) {
635 && !(
imu.mags[i].calibrated.neutral &&
imu.mags[i].calibrated.scale_f)) {
665 if(i >= gyro_calib_len) {
667 imu.gyros[i].calibrated.neutral =
false;
668 imu.gyros[i].calibrated.scale =
false;
669 imu.gyros[i].calibrated.scale_f =
false;
670 imu.gyros[i].calibrated.rotation =
false;
671 imu.gyros[i].calibrated.rot_euler =
false;
672 imu.gyros[i].calibrated.filter =
false;
674 imu.gyros[i] = gyro_calib[i];
678 if(!
imu.gyros[i].calibrated.neutral) {
682 if(!
imu.gyros[i].calibrated.scale_f) {
683 if(
imu.gyros[i].calibrated.scale) {
684 imu.gyros[i].scale_f.p = (float)
imu.gyros[i].scale[0].p / (
float)
imu.gyros[i].scale[1].p;
685 imu.gyros[i].scale_f.q = (float)
imu.gyros[i].scale[0].q / (
float)
imu.gyros[i].scale[1].q;
686 imu.gyros[i].scale_f.r = (float)
imu.gyros[i].scale[0].r / (
float)
imu.gyros[i].scale[1].r;
687 imu.gyros[i].calibrated.scale_f =
true;
693 if(!
imu.gyros[i].calibrated.rotation) {
694 if (
imu.gyros[i].calibrated.rot_euler) {
698 imu.gyros[i].calibrated.rotation =
true;
703 imu.gyros[i].last_stamp = 0;
711 if(i >= accel_calib_len) {
713 imu.accels[i].calibrated.neutral =
false;
714 imu.accels[i].calibrated.scale =
false;
715 imu.accels[i].calibrated.scale_f =
false;
716 imu.accels[i].calibrated.rotation =
false;
717 imu.accels[i].calibrated.rot_euler =
false;
718 imu.accels[i].calibrated.filter =
false;
720 imu.accels[i] = accel_calib[i];
724 if(!
imu.accels[i].calibrated.neutral) {
728 if(!
imu.accels[i].calibrated.scale_f) {
729 if(
imu.accels[i].calibrated.scale) {
730 imu.accels[i].scale_f.x = (float)
imu.accels[i].scale[0].x / (
float)
imu.accels[i].scale[1].x;
731 imu.accels[i].scale_f.y = (float)
imu.accels[i].scale[0].y / (
float)
imu.accels[i].scale[1].y;
732 imu.accels[i].scale_f.z = (float)
imu.accels[i].scale[0].z / (
float)
imu.accels[i].scale[1].z;
733 imu.accels[i].calibrated.scale_f =
true;
739 if(!
imu.accels[i].calibrated.rotation) {
740 if (
imu.accels[i].calibrated.rot_euler) {
744 imu.accels[i].calibrated.rotation =
true;
749 imu.accels[i].last_stamp = 0;
753 RMAT_COPY(
imu.accels[i].body_to_sensor, body_to_sensor);
756 if(i >= mag_calib_len) {
758 imu.mags[i].calibrated.neutral =
false;
759 imu.mags[i].calibrated.scale =
false;
760 imu.mags[i].calibrated.scale_f =
false;
761 imu.mags[i].calibrated.rotation =
false;
762 imu.mags[i].calibrated.rot_euler =
false;
763 imu.mags[i].calibrated.current =
false;
764 imu.mags[i].strength = 0;
770 if(!
imu.mags[i].calibrated.neutral) {
774 if(!
imu.mags[i].calibrated.scale_f) {
775 if(
imu.mags[i].calibrated.scale) {
776 imu.mags[i].scale_f.x = (float)
imu.mags[i].scale[0].x / (
float)
imu.mags[i].scale[1].x;
777 imu.mags[i].scale_f.y = (float)
imu.mags[i].scale[0].y / (
float)
imu.mags[i].scale[1].y;
778 imu.mags[i].scale_f.z = (float)
imu.mags[i].scale[0].z / (
float)
imu.mags[i].scale[1].z;
779 imu.mags[i].calibrated.scale_f =
true;
784 if(!
imu.mags[i].calibrated.rotation) {
785 if (
imu.mags[i].calibrated.rot_euler) {
789 imu.mags[i].calibrated.rotation =
true;
794 if(!
imu.mags[i].calibrated.current) {
806#if PERIODIC_TELEMETRY
827 imu.initialized =
true;
925 if(gyro == NULL || samples < 1)
936 const float init[3] = {data[0].
p, data[0].
q, data[0].
r};
941 struct Int32Rates data_filtered[gyro->calibrated.filter ? samples : 1];
942 if(gyro->calibrated.filter) {
943 for(
uint8_t i = 0; i < samples; i++) {
948 data = data_filtered;
967 const float int_rate = imu_rate_est_update(&gyro->
rate_est,
"gyro", sender_id, rate, stamp, gyro->
last_stamp, samples);
968 const bool integrate = isfinite(int_rate) && int_rate > 0.f;
972 const float sample_dt = integrate ? 1.f / int_rate : 0.f;
973 for(
uint8_t i = 0; i < samples; i++) {
982 if(i < samples - 1) {
999 struct FloatRates integrated, integrated_sensor;
1006 float dt = (1e6f / int_rate) * samples + 0.5f;
1007 if(dt < UINT16_MAX + 1.f) {
1008 AbiSendMsgIMU_GYRO_INT(sender_id, stamp, &integrated, (
uint16_t)dt);
1015#if IMU_LOG_HIGHSPEED
1025 AbiSendMsgIMU_GYRO(sender_id, stamp, &gyro->
scaled);
1033 if(accel == NULL || samples < 1)
1044 const float init[3] = {data[0].
x, data[0].
y, data[0].
z};
1049 struct Int32Vect3 data_filtered[accel->calibrated.filter ? samples : 1];
1050 if(accel->calibrated.filter) {
1051 for(
uint8_t i = 0; i < samples; i++) {
1056 data = data_filtered;
1076 const float int_rate = imu_rate_est_update(&accel->
rate_est,
"accel", sender_id, rate, stamp, accel->
last_stamp, samples);
1077 const bool integrate = isfinite(int_rate) && int_rate > 0.f;
1081 const float sample_dt = integrate ? 1.f / int_rate : 0.f;
1082 for(
uint8_t i = 0; i < samples; i++) {
1088#if IMU_LOG_HIGHSPEED
1091 if(i < samples - 1) {
1097 imu_integrate_sample(&
alpha, NULL, &accel->
last_sample, NULL, &f_sample, sample_dt);
1112 float dt = (1e6f / int_rate) * samples + 0.5f;
1113 if(dt < UINT16_MAX + 1.f) {
1114 AbiSendMsgIMU_ACCEL_INT(sender_id, stamp, &integrated, (
uint16_t)dt);
1121#if IMU_LOG_HIGHSPEED
1131 AbiSendMsgIMU_ACCEL(sender_id, stamp, &accel->
scaled);
1160 AbiSendMsgIMU_MAG(sender_id, stamp, &mag->
scaled);
1176 gyro = &
imu.gyros[i];
1177 gyro->
abi_id = sender_id;
1180 gyro = &
imu.gyros[i];
1199 accel = &
imu.accels[i];
1200 accel->
abi_id = sender_id;
1203 accel = &
imu.accels[i];
1241 struct Int32RMat new_body_to_imu, diff_body_to_imu;
1254 old_rmat =
imu.gyros[i].body_to_sensor;
1257 old_rmat =
imu.accels[i].body_to_sensor;
1260 old_rmat =
imu.mags[i].body_to_sensor;
1272 body_to_imu_eulers.
phi =
phi;
1288 body_to_imu_eulers.
psi =
psi;
1294 imu.b2i_set_current = set;
1296 if (
imu.b2i_set_current) {
1307 imu.b2i_set_current =
false;
Main include for ABI (AirBorneInterface).
#define ABI_DISABLE
Reserved ABI ID to disable callback.
struct abi_struct abi_event
#define ABI_BROADCAST
Broadcast address.
struct FloatVect3 accel_float
struct FloatVect3 mag_float
Common code for AP and FBW telemetry.
if(GpsFixValid() &&e_identification_started)
static struct uart_periph * dev
struct Electrical electrical
Interface for electrical status: supply voltage, current, battery status, etc.
void chprintf(BaseSequentialStream *lchp, const char *fmt,...)
void float_rmat_transp_ratemult(struct FloatRates *rb, const struct FloatRMat *m_b2a, const struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
#define FLOAT_VECT3_ZERO(_v)
void float_rmat_transp_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_b2a, const struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
#define VECT3_SUM(_c, _a, _b)
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
#define RATES_COPY(_a, _b)
#define RATES_ASSIGN(_ra, _p, _q, _r)
#define VECT3_CROSS_PRODUCT(_vo, _v1, _v2)
#define EULERS_BFP_OF_REAL(_ei, _ef)
#define RMAT_FLOAT_OF_BFP(_ef, _ei)
#define VECT3_SMUL(_vo, _vi, _s)
#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 VECT3_ADD(_a, _b)
#define VECT3_SUM_SCALED(_c, _a, _b, _s)
#define MAT33_ELMT(_m, _row, _col)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
#define VECT3_ADD_SCALED(_a, _b, _s)
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 void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers)
Set vehicle body attitude from euler angles (float).
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
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_RATE_EST_BOUND
Maximum relative deviation of the measured sample rate from the calibrated sample_freq or driver rate...
#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.
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.
#define IMU_RATE_EST_REPORT
Number of accepted windows after which the measured sample rate is reported once (INFO_MSG).
#define IMU_RATE_EST_WINDOW
Window to measure the sample rate for the integration (us).
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 ...
struct imu_gyro_t * imu_get_gyro(uint8_t sender_id, bool create)
Find or create the gyro in the imu structure.
static abi_event imu_accel_raw_ev
static abi_event imu_mag_raw_ev
void imu_SetBodyToImuCurrent(bool set)
struct imu_accel_t * imu_get_accel(uint8_t sender_id, bool create)
Find or create the accel in the imu structure.
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)
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...
#define IMU_ACCEL_CALIB
Default accel calibration is for single IMU with old format.
#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
#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)
#define IMU_RATE_EST_JITTER
Timestamp jitter (us) allowed before a callback interval is seen as a gap with lost samples.
#define IMU_RATE_EST_ALPHA
Low-pass factor of the measured sample rate per window (0.1: averaged over ~10 windows).
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_scaled(struct transport_tx *trans, struct link_device *dev)
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
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_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)
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.
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 e...
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, set as imu to sensor in the airframe calibration and combined wit...
struct imu_calib_t calibrated
Calibration bitmask.
float sample_freq
Calibrated sample frequency (Hz) for the filter, and reference of the measured integration rate (the ...
struct FloatVect3 current_scale
Current scaling multiplying.
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 cutoff frequency (Hz).
struct FloatVect3 last_delta_alpha
Last integrated sample delta angle in sensor frame for the coning correction (IMU_INTEGRATION).
struct imu_rate_est_t rate_est
Measured sample rate for the integration (IMU_INTEGRATION).
struct FloatRates scale_f
Scaling.
uint32_t last_stamp
Last measurement timestamp for integration.
float rate
Filtered measured sample rate (Hz), 0 until the first window.
struct FloatVect3 scale_f
Scaling.
struct Int32Vect3 neutral
Neutral values, compensation on unscaled->scaled.
bool filter
Enable the lowpass filter (gyro and accel only, ignored for the mag).
struct FloatVect3 last_sample
Last scaled sample in sensor frame for the integration (IMU_INTEGRATION).
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, set as imu to sensor in the airframe calibration and combined wit...
Butterworth2LowPass filter[3]
Lowpass filter optional.
struct imu_calib_t calibrated
Calibration bitmask.
struct FloatVect3 scale_f
Scaling.
uint32_t samples
Samples received since the window start.
bool scale_f
Scale calibrated with floating point.
bool rejected
A window was rejected (out of bounds), only warned once.
uint16_t windows
Accepted windows, up to IMU_RATE_EST_REPORT when the rate is reported.
struct Int32Vect3 scaled
Last scaled values in body frame.
struct Int32RMat body_to_sensor
Rotation from body to sensor frame, set as imu to sensor in the airframe calibration and combined wit...
struct Int32Rates unscaled
Last unscaled values in sensor frame.
uint8_t abi_id
ABI sensor ID.
float sample_freq
Calibrated sample frequency (Hz) for the filter, and reference of the measured integration rate (the ...
bool neutral
Neutral values calibrated.
struct Int32Vect3 neutral
Neutral values, compensation on unscaled->scaled.
struct Int32Vect3 unscaled
Last unscaled values in sensor frame.
float filter_freq
Lowpass filter cutoff frequency (Hz).
struct FloatVect3 last_sample
Last scaled sample in sensor frame for the integration (IMU_INTEGRATION).
struct imu_rate_est_t rate_est
Measured sample rate for the integration (IMU_INTEGRATION).
struct imu_calib_t calibrated
Calibration bitmask.
uint32_t start
Timestamp of the current window start (us).
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
Measured sample rate of a sensor, counted against the timestamps.
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
struct SecondOrderLowPass Butterworth2LowPass
Second order Butterworth low pass filter.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, const float tau, const float sample_time, const float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, const float value)
Update second order Butterworth low pass filter state with a new value.
static TRICAL_instance_t mag_calib
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
struct pprzlog_transport pprzlog_tp
PPRZLOG transport structure.
void preflight_success(struct preflight_result_t *result, const char *fmt,...)
Register a preflight success used inside the preflight checking functions.
void preflight_warning(struct preflight_result_t *result, const char *fmt,...)
Register a preflight warning used inside the preflight checking functions.
void preflight_check_register(struct preflight_check_t *check, preflight_check_f func)
Register a preflight check and add it to the linked list.
Mini printf-like functionality.
void shell_add_entry(char *cmd_name, shell_cmd_t *cmd)
Add dynamic entry.
BaseSequentialStream shell_stream_t
API to get/set the generic vehicle states.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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.