diff --git a/conf/abi.xml b/conf/abi.xml
index 7b8b360d41..be92c384d4 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -22,17 +22,17 @@
-
+
-
+
-
+
@@ -215,6 +215,35 @@
Pointer to a radio control structure
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/examples/cube_orange.xml b/conf/airframes/examples/cube_orange.xml
index f0c25f9852..0dfef3fc7f 100644
--- a/conf/airframes/examples/cube_orange.xml
+++ b/conf/airframes/examples/cube_orange.xml
@@ -87,7 +87,7 @@
-
+
-
-
-
-
-
-
-
+
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/modules/core/abi_sender_ids.h b/sw/airborne/modules/core/abi_sender_ids.h
index f5011d13b2..cf31eea861 100644
--- a/sw/airborne/modules/core/abi_sender_ids.h
+++ b/sw/airborne/modules/core/abi_sender_ids.h
@@ -372,7 +372,7 @@
#endif
#ifndef IMU_CUBE3_ID
-#define IMU_CUBE4_ID 22
+#define IMU_CUBE3_ID 22
#endif
// prefiltering with OneEuro filter
diff --git a/sw/airborne/modules/imu/imu.c b/sw/airborne/modules/imu/imu.c
index 0dd9099780..c575ffd3a6 100644
--- a/sw/airborne/modules/imu/imu.c
+++ b/sw/airborne/modules/imu/imu.c
@@ -31,13 +31,10 @@
#include "modules/imu/imu.h"
#include "state.h"
#include "modules/core/abi.h"
+#include "modules/energy/electrical.h"
-#ifdef IMU_POWER_GPIO
-#include "mcu_periph/gpio.h"
-
-#ifndef IMU_POWER_GPIO_ON
-#define IMU_POWER_GPIO_ON gpio_set
-#endif
+#ifndef IMU_INTEGRATION
+#define IMU_INTEGRATION true
#endif
#if PERIODIC_TELEMETRY
@@ -45,94 +42,154 @@
static void send_accel_raw(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID,
- &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z);
+ static uint8_t id = 0;
+ pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID, &imu.accels[id].abi_id,
+ &imu.accels[id].unscaled.x, &imu.accels[id].unscaled.y, &imu.accels[id].unscaled.z);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
+ id = 0;
}
static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID,
- &imu.accel.x, &imu.accel.y, &imu.accel.z);
+ static uint8_t id = 0;
+ pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID, &imu.accels[id].abi_id,
+ &imu.accels[id].scaled.x, &imu.accels[id].scaled.y, &imu.accels[id].scaled.z);
+ id++;
+ if(id >= IMU_MAX_SENSORS)
+ id = 0;
}
static void send_accel(struct transport_tx *trans, struct link_device *dev)
{
+ static uint8_t id = 0;
struct FloatVect3 accel_float;
- ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
- pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID,
+ ACCELS_FLOAT_OF_BFP(accel_float, imu.accels[id].scaled);
+ pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, &imu.accels[id].abi_id,
&accel_float.x, &accel_float.y, &accel_float.z);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.accels[id].abi_id == ABI_DISABLE)
+ id = 0;
}
static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID,
- &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r);
+ static uint8_t id = 0;
+ pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID, &imu.gyros[id].abi_id,
+ &imu.gyros[id].unscaled.p, &imu.gyros[id].unscaled.q, &imu.gyros[id].unscaled.r);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
+ id = 0;
}
static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID,
- &imu.gyro.p, &imu.gyro.q, &imu.gyro.r);
+ static uint8_t id = 0;
+ pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID, &imu.gyros[id].abi_id,
+ &imu.gyros[id].scaled.p, &imu.gyros[id].scaled.q, &imu.gyros[id].scaled.r);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
+ id = 0;
}
static void send_gyro(struct transport_tx *trans, struct link_device *dev)
{
+ static uint8_t id = 0;
struct FloatRates gyro_float;
- RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
- pprz_msg_send_IMU_GYRO(trans, dev, AC_ID,
+ RATES_FLOAT_OF_BFP(gyro_float, imu.gyros[id].scaled);
+ pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, &imu.gyros[id].abi_id,
&gyro_float.p, &gyro_float.q, &gyro_float.r);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.gyros[id].abi_id == ABI_DISABLE)
+ id = 0;
}
static void send_mag_raw(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID,
- &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z);
+ static uint8_t id = 0;
+ pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID, &imu.mags[id].abi_id,
+ &imu.mags[id].unscaled.x, &imu.mags[id].unscaled.y, &imu.mags[id].unscaled.z);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
+ id = 0;
}
static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev)
{
- pprz_msg_send_IMU_MAG_SCALED(trans, dev, AC_ID,
- &imu.mag.x, &imu.mag.y, &imu.mag.z);
+ static uint8_t id = 0;
+ pprz_msg_send_IMU_MAG_SCALED(trans, dev, AC_ID, &imu.mags[id].abi_id ,
+ &imu.mags[id].scaled.x, &imu.mags[id].scaled.y, &imu.mags[id].scaled.z);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
+ id = 0;
}
static void send_mag(struct transport_tx *trans, struct link_device *dev)
{
+ static uint8_t id = 0;
struct FloatVect3 mag_float;
- MAGS_FLOAT_OF_BFP(mag_float, imu.mag);
- pprz_msg_send_IMU_MAG(trans, dev, AC_ID,
+ MAGS_FLOAT_OF_BFP(mag_float, imu.mags[id].scaled);
+ pprz_msg_send_IMU_MAG(trans, dev, AC_ID, &imu.mags[id].abi_id,
&mag_float.x, &mag_float.y, &mag_float.z);
+ id++;
+ if(id >= IMU_MAX_SENSORS || imu.mags[id].abi_id == ABI_DISABLE)
+ id = 0;
}
#endif /* PERIODIC_TELEMETRY */
-struct Imu imu;
+struct Imu imu = {0};
+static abi_event imu_gyro_raw_ev, imu_accel_raw_ev, imu_mag_raw_ev;
+static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples);
+static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples);
+static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data);
+static struct imu_gyro_t *imu_get_gyro(uint8_t sender_id);
+static struct imu_accel_t *imu_get_accel(uint8_t sender_id);
+static struct imu_mag_t *imu_get_mag(uint8_t sender_id);
void imu_init(void)
{
-
-#ifdef IMU_POWER_GPIO
- gpio_setup_output(IMU_POWER_GPIO);
- IMU_POWER_GPIO_ON(IMU_POWER_GPIO);
-#endif
-
- /* initialises neutrals */
- RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL);
-
- VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);
-
-#if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL
- VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL);
-#else
-#if USE_MAGNETOMETER && (!defined MAG_CALIB_UKF_H)
- INFO("Magnetometer neutrals are set to zero, you should calibrate!")
-#endif
- INT_VECT3_ZERO(imu.mag_neutral);
-#endif
-
+ // Do not initialize twice
+ if(imu.initialized)
+ return;
+
+ // Set the Body to IMU rotation
struct FloatEulers body_to_imu_eulers =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers);
+ // Initialize the non-initialized sensors to default values
+ for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
+ if(imu.gyros[i].abi_id == ABI_DISABLE) {
+ imu.gyros[i].last_stamp = 0;
+ INT_RATES_ZERO(imu.gyros[i].neutral);
+ RATES_ASSIGN(imu.gyros[i].scale[0], 1, 1, 1);
+ RATES_ASSIGN(imu.gyros[i].scale[1], 1, 1, 1);
+ int32_rmat_identity(&imu.gyros[i].imu_to_sensor);
+ }
+
+ if(imu.accels[i].abi_id == ABI_DISABLE) {
+ imu.accels[i].last_stamp = 0;
+ INT_VECT3_ZERO(imu.accels[i].neutral);
+ VECT3_ASSIGN(imu.accels[i].scale[0], 1, 1, 1);
+ VECT3_ASSIGN(imu.accels[i].scale[1], 1, 1, 1);
+ int32_rmat_identity(&imu.accels[i].imu_to_sensor);
+ }
+
+ if(imu.mags[i].abi_id == ABI_DISABLE) {
+ INT_VECT3_ZERO(imu.mags[i].neutral);
+ VECT3_ASSIGN(imu.mags[i].scale[0], 1, 1, 1);
+ VECT3_ASSIGN(imu.mags[i].scale[1], 1, 1, 1);
+ INT_VECT3_ZERO(imu.mags[i].current_scale);
+ int32_rmat_identity(&imu.mags[i].imu_to_sensor);
+ }
+ }
+
+ // Bind to raw measurements
+ AbiBindMsgIMU_GYRO_RAW(ABI_BROADCAST, &imu_gyro_raw_ev, imu_gyro_raw_cb);
+ AbiBindMsgIMU_ACCEL_RAW(ABI_BROADCAST, &imu_accel_raw_ev, imu_accel_raw_cb);
+ AbiBindMsgIMU_MAG_RAW(ABI_BROADCAST, &imu_mag_raw_ev, imu_mag_raw_cb);
+
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_RAW, send_accel_raw);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_ACCEL_SCALED, send_accel_scaled);
@@ -145,8 +202,235 @@ void imu_init(void)
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IMU_MAG, send_mag);
#endif // DOWNLINK
+ imu.initialized = true;
}
+/**
+ * @brief Set the sensor rotation
+ *
+ * @param abi_id
+ * @param imu_to_sensor
+ */
+void imu_set_gyro_rmat(uint8_t abi_id, struct Int32RMat *imu_to_sensor)
+{
+ // Could be that we are not initialized
+ imu_init();
+
+ // Find the correct gyro
+ struct imu_gyro_t *gyro = imu_get_gyro(abi_id);
+ if(gyro == NULL)
+ return;
+
+ RMAT_COPY(gyro->imu_to_sensor, *imu_to_sensor);
+}
+
+/**
+ * @brief Set the sensor rotation
+ *
+ * @param abi_id
+ * @param imu_to_sensor
+ */
+void imu_set_accel_rmat(uint8_t abi_id, struct Int32RMat *imu_to_sensor)
+{
+ // Could be that we are not initialized
+ imu_init();
+
+ // Find the correct accel
+ struct imu_accel_t *accel = imu_get_accel(abi_id);
+ if(accel == NULL)
+ return;
+
+ RMAT_COPY(accel->imu_to_sensor, *imu_to_sensor);
+}
+
+static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples)
+{
+ // Find the correct gyro
+ struct imu_gyro_t *gyro = imu_get_gyro(sender_id);
+ if(gyro == NULL || samples < 1)
+ return;
+
+ // Copy last sample as unscaled
+ RATES_COPY(gyro->unscaled, data[samples-1]);
+
+ // Scale the gyro
+ struct Int32Rates scaled, scaled_rot;
+ scaled.p = (gyro->unscaled.p - gyro->neutral.p) * gyro->scale[0].p / gyro->scale[1].p;
+ scaled.q = (gyro->unscaled.q - gyro->neutral.q) * gyro->scale[0].q / gyro->scale[1].q;
+ scaled.r = (gyro->unscaled.r - gyro->neutral.r) * gyro->scale[0].r / gyro->scale[1].r;
+
+ // Rotate the sensor
+ int32_rmat_ratemult(&scaled_rot, &gyro->imu_to_sensor, &scaled);
+
+#if IMU_INTEGRATION
+ // Only integrate if we have gotten a previous measurement and didn't overflow the timer
+ if(gyro->last_stamp > 0 && stamp > gyro->last_stamp) {
+ struct FloatRates integrated;
+ uint16_t delta_dt = stamp - gyro->last_stamp;
+
+ // Trapezoidal integration (TODO: coning correction)
+ integrated.p = RATE_FLOAT_OF_BFP(gyro->scaled.p + scaled_rot.p) * 0.5f;
+ integrated.q = RATE_FLOAT_OF_BFP(gyro->scaled.q + scaled_rot.q) * 0.5f;
+ integrated.r = RATE_FLOAT_OF_BFP(gyro->scaled.r + scaled_rot.r) * 0.5f;
+
+ for(uint8_t i = 0; i < samples-1; i++) {
+ integrated.p += data[i].p;
+ integrated.q += data[i].q;
+ integrated.r += data[i].r;
+ }
+
+ integrated.p = integrated.p / samples * ((float)delta_dt * 1e-6f);
+ integrated.q = integrated.q / samples * ((float)delta_dt * 1e-6f);
+ integrated.r = integrated.r / samples * ((float)delta_dt * 1e-6f);
+
+ // Send the integrated values
+ AbiSendMsgIMU_GYRO_INT(sender_id, stamp, &integrated, delta_dt);
+ }
+#endif
+
+ // Copy and send
+ RATES_COPY(gyro->scaled, scaled_rot);
+ AbiSendMsgIMU_GYRO(sender_id, stamp, &gyro->scaled);
+ gyro->last_stamp = stamp;
+}
+
+static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples)
+{
+ // Find the correct accel
+ struct imu_accel_t *accel = imu_get_accel(sender_id);
+ if(accel == NULL || samples < 1)
+ return;
+
+ // Copy last sample as unscaled
+ VECT3_COPY(accel->unscaled, data[samples-1]);
+
+ // Scale the accel
+ struct Int32Vect3 scaled, scaled_rot;
+ scaled.x = (accel->unscaled.x - accel->neutral.x) * accel->scale[0].x / accel->scale[1].x;
+ scaled.y = (accel->unscaled.y - accel->neutral.y) * accel->scale[0].y / accel->scale[1].y;
+ scaled.z = (accel->unscaled.z - accel->neutral.z) * accel->scale[0].z / accel->scale[1].z;
+
+ // Rotate the sensor
+ int32_rmat_transp_vmult(&scaled_rot, &accel->imu_to_sensor, &scaled);
+
+#if IMU_INTEGRATION
+ // Only integrate if we have gotten a previous measurement and didn't overflow the timer
+ if(accel->last_stamp > 0 && stamp > accel->last_stamp) {
+ struct FloatVect3 integrated;
+ uint16_t delta_dt = stamp - accel->last_stamp;
+
+ // Trapezoidal integration
+ integrated.x = RATE_FLOAT_OF_BFP(accel->scaled.x + scaled_rot.x) * 0.5f;
+ integrated.y = RATE_FLOAT_OF_BFP(accel->scaled.y + scaled_rot.y) * 0.5f;
+ integrated.z = RATE_FLOAT_OF_BFP(accel->scaled.z + scaled_rot.z) * 0.5f;
+
+ for(uint8_t i = 0; i < samples-1; i++) {
+ integrated.x += data[i].x;
+ integrated.y += data[i].y;
+ integrated.z += data[i].z;
+ }
+
+ integrated.x = integrated.x / samples * ((float)delta_dt * 1e-6f);
+ integrated.y = integrated.y / samples * ((float)delta_dt * 1e-6f);
+ integrated.z = integrated.z / samples * ((float)delta_dt * 1e-6f);
+
+ // Send the integrated values
+ AbiSendMsgIMU_ACCEL_INT(sender_id, stamp, &integrated, delta_dt);
+ }
+#endif
+
+ // Copy and send
+ VECT3_COPY(accel->scaled, scaled_rot);
+ AbiSendMsgIMU_ACCEL(sender_id, stamp, &accel->scaled);
+ accel->last_stamp = stamp;
+}
+
+static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data)
+{
+ // Find the correct mag
+ struct imu_mag_t *mag = imu_get_mag(sender_id);
+ if(mag == NULL)
+ return;
+
+ // Calculate current compensation
+ struct Int32Vect3 mag_correction;
+ mag_correction.x = (int32_t)(mag->current_scale.x * (float) electrical.current);
+ mag_correction.y = (int32_t)(mag->current_scale.y * (float) electrical.current);
+ mag_correction.z = (int32_t)(mag->current_scale.z * (float) electrical.current);
+
+ // Copy last sample as unscaled
+ VECT3_COPY(mag->unscaled, *data);
+
+ // Scale the mag
+ struct Int32Vect3 scaled;
+ scaled.x = (mag->unscaled.x - mag_correction.x - mag->neutral.x) * mag->scale[0].x / mag->scale[1].x;
+ scaled.y = (mag->unscaled.y - mag_correction.y - mag->neutral.y) * mag->scale[0].y / mag->scale[1].y;
+ scaled.z = (mag->unscaled.z - mag_correction.z - mag->neutral.z) * mag->scale[0].z / mag->scale[1].z;
+
+ // Rotate the sensor
+ int32_rmat_transp_vmult(&mag->scaled, &mag->imu_to_sensor, &scaled);
+ AbiSendMsgIMU_MAG(sender_id, stamp, &mag->scaled);
+}
+
+/**
+ * @brief Find or create the gyro in the imu structure
+ *
+ * @param sender_id The ABI sender id to search for
+ * @return struct imu_gyro_t* The gyro structure if found/created else NULL
+ */
+static struct imu_gyro_t *imu_get_gyro(uint8_t sender_id) {
+ // Find the correct gyro or create index
+ struct imu_gyro_t *gyro = NULL;
+ for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
+ if(imu.gyros[i].abi_id == sender_id || imu.gyros[i].abi_id == ABI_DISABLE) {
+ gyro = &imu.gyros[i];
+ gyro->abi_id = sender_id;
+ break;
+ }
+ }
+
+ return gyro;
+}
+
+/**
+ * @brief Find or create the accel in the imu structure
+ *
+ * @param sender_id The ABI sender id to search for
+ * @return struct imu_accel_t* The accel structure if found/created else NULL
+ */
+static struct imu_accel_t *imu_get_accel(uint8_t sender_id) {
+ // Find the correct accel
+ struct imu_accel_t *accel = NULL;
+ for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
+ if(imu.accels[i].abi_id == sender_id || imu.accels[i].abi_id == ABI_DISABLE) {
+ accel = &imu.accels[i];
+ accel->abi_id = sender_id;
+ break;
+ }
+ }
+
+ return accel;
+}
+
+/**
+ * @brief Find or create the mag in the imu structure
+ *
+ * @param sender_id The ABI sender id to search for
+ * @return struct imu_mag_t* The mag structure if found/created else NULL
+ */
+static struct imu_mag_t *imu_get_mag(uint8_t sender_id) {
+ // Find the correct mag
+ struct imu_mag_t *mag = NULL;
+ for(uint8_t i = 0; i < IMU_MAX_SENSORS; i++) {
+ if(imu.mags[i].abi_id == sender_id || imu.mags[i].abi_id == ABI_DISABLE) {
+ mag = &imu.mags[i];
+ mag->abi_id = sender_id;
+ break;
+ }
+ }
+
+ return mag;
+}
void imu_SetBodyToImuPhi(float phi)
{
@@ -201,57 +485,3 @@ void imu_SetBodyToImuCurrent(float set)
AbiSendMsgBODY_TO_IMU_QUAT(1, orientationGetQuat_f(&imu.body_to_imu));
}
}
-
-
-// weak functions, used if not explicitly provided by implementation
-
-void WEAK imu_scale_gyro(struct Imu *_imu)
-{
- RATES_COPY(_imu->gyro_prev, _imu->gyro);
- _imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p) * IMU_GYRO_P_SIGN *
- IMU_GYRO_P_SENS_NUM) / IMU_GYRO_P_SENS_DEN;
- _imu->gyro.q = ((_imu->gyro_unscaled.q - _imu->gyro_neutral.q) * IMU_GYRO_Q_SIGN *
- IMU_GYRO_Q_SENS_NUM) / IMU_GYRO_Q_SENS_DEN;
- _imu->gyro.r = ((_imu->gyro_unscaled.r - _imu->gyro_neutral.r) * IMU_GYRO_R_SIGN *
- IMU_GYRO_R_SENS_NUM) / IMU_GYRO_R_SENS_DEN;
-}
-
-void WEAK imu_scale_accel(struct Imu *_imu)
-{
- VECT3_COPY(_imu->accel_prev, _imu->accel);
- _imu->accel.x = ((_imu->accel_unscaled.x - _imu->accel_neutral.x) * IMU_ACCEL_X_SIGN *
- IMU_ACCEL_X_SENS_NUM) / IMU_ACCEL_X_SENS_DEN;
- _imu->accel.y = ((_imu->accel_unscaled.y - _imu->accel_neutral.y) * IMU_ACCEL_Y_SIGN *
- IMU_ACCEL_Y_SENS_NUM) / IMU_ACCEL_Y_SENS_DEN;
- _imu->accel.z = ((_imu->accel_unscaled.z - _imu->accel_neutral.z) * IMU_ACCEL_Z_SIGN *
- IMU_ACCEL_Z_SENS_NUM) / IMU_ACCEL_Z_SENS_DEN;
-}
-
-#if !defined SITL && defined IMU_MAG_X_CURRENT_COEF && defined IMU_MAG_Y_CURRENT_COEF && defined IMU_MAG_Z_CURRENT_COEF
-#include "modules/energy/electrical.h"
-void WEAK imu_scale_mag(struct Imu *_imu)
-{
- struct Int32Vect3 mag_correction;
- mag_correction.x = (int32_t)(IMU_MAG_X_CURRENT_COEF * (float) electrical.current);
- mag_correction.y = (int32_t)(IMU_MAG_Y_CURRENT_COEF * (float) electrical.current);
- mag_correction.z = (int32_t)(IMU_MAG_Z_CURRENT_COEF * (float) electrical.current);
- _imu->mag.x = (((_imu->mag_unscaled.x - mag_correction.x) - _imu->mag_neutral.x) *
- IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN;
- _imu->mag.y = (((_imu->mag_unscaled.y - mag_correction.y) - _imu->mag_neutral.y) *
- IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN;
- _imu->mag.z = (((_imu->mag_unscaled.z - mag_correction.z) - _imu->mag_neutral.z) *
- IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN;
-}
-#elif USE_MAGNETOMETER
-void WEAK imu_scale_mag(struct Imu *_imu)
-{
- _imu->mag.x = ((_imu->mag_unscaled.x - _imu->mag_neutral.x) * IMU_MAG_X_SIGN *
- IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN;
- _imu->mag.y = ((_imu->mag_unscaled.y - _imu->mag_neutral.y) * IMU_MAG_Y_SIGN *
- IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN;
- _imu->mag.z = ((_imu->mag_unscaled.z - _imu->mag_neutral.z) * IMU_MAG_Z_SIGN *
- IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN;
-}
-#else
-void WEAK imu_scale_mag(struct Imu *_imu __attribute__((unused))) {}
-#endif /* MAG_x_CURRENT_COEF */
diff --git a/sw/airborne/modules/imu/imu.h b/sw/airborne/modules/imu/imu.h
index 026dd9af89..5edb9b3cbb 100644
--- a/sw/airborne/modules/imu/imu.h
+++ b/sw/airborne/modules/imu/imu.h
@@ -32,20 +32,47 @@
#include "math/pprz_orientation_conversion.h"
#include "generated/airframe.h"
+#ifndef IMU_MAX_SENSORS
+#define IMU_MAX_SENSORS 3
+#endif
+
+struct imu_gyro_t {
+ uint8_t abi_id;
+ uint32_t last_stamp;
+ struct Int32Rates scaled;
+ struct Int32Rates unscaled;
+ struct Int32Rates neutral;
+ struct Int32Rates scale[2];
+ struct Int32RMat imu_to_sensor; ///< Rotation from imu to sensor frame
+};
+
+struct imu_accel_t {
+ uint8_t abi_id;
+ uint32_t last_stamp;
+ struct Int32Vect3 scaled;
+ struct Int32Vect3 unscaled;
+ struct Int32Vect3 neutral;
+ struct Int32Vect3 scale[2];
+ struct Int32RMat imu_to_sensor; ///< Rotation from imu to sensor frame
+};
+
+struct imu_mag_t {
+ uint8_t abi_id;
+ struct Int32Vect3 scaled;
+ struct Int32Vect3 unscaled;
+ struct Int32Vect3 neutral;
+ struct Int32Vect3 scale[2];
+ struct FloatVect3 current_scale;
+ struct Int32RMat imu_to_sensor; ///< Rotation from imu to sensor frame
+};
+
/** abstract IMU interface providing fixed point interface */
struct Imu {
- struct Int32Rates gyro; ///< gyroscope measurements in rad/s in BFP with #INT32_RATE_FRAC
- struct Int32Vect3 accel; ///< accelerometer measurements in m/s^2 in BFP with #INT32_ACCEL_FRAC
- struct Int32Vect3 mag; ///< magnetometer measurements scaled to 1 in BFP with #INT32_MAG_FRAC
- struct Int32Rates gyro_prev; ///< previous gyroscope measurements
- struct Int32Vect3 accel_prev; ///< previous accelerometer measurements
- struct Int32Rates gyro_neutral; ///< static gyroscope bias from calibration in raw/unscaled units
- struct Int32Vect3 accel_neutral; ///< static accelerometer bias from calibration in raw/unscaled units
- struct Int32Vect3 mag_neutral; ///< magnetometer neutral readings (bias) in raw/unscaled units
- struct Int32Rates gyro_unscaled; ///< unscaled gyroscope measurements
- struct Int32Vect3 accel_unscaled; ///< unscaled accelerometer measurements
- struct Int32Vect3 mag_unscaled; ///< unscaled magnetometer measurements
+ bool initialized;
+ struct imu_gyro_t gyros[IMU_MAX_SENSORS];
+ struct imu_accel_t accels[IMU_MAX_SENSORS];
+ struct imu_mag_t mags[IMU_MAX_SENSORS];
struct OrientationReps body_to_imu; ///< rotation from body to imu frame
/** flag for adjusting body_to_imu via settings.
@@ -57,62 +84,22 @@ struct Imu {
/** global IMU state */
extern struct Imu imu;
-/* underlying hardware */
-#ifdef IMU_TYPE_H
-#include IMU_TYPE_H
-#endif
-
+/** External functions */
extern void imu_init(void);
+extern void imu_set_gyro_rmat(uint8_t abi_id, struct Int32RMat *imu_to_sensor);
+extern void imu_set_accel_rmat(uint8_t abi_id, struct Int32RMat *imu_to_sensor);
+
extern void imu_SetBodyToImuPhi(float phi);
extern void imu_SetBodyToImuTheta(float theta);
extern void imu_SetBodyToImuPsi(float psi);
extern void imu_SetBodyToImuCurrent(float set);
extern void imu_ResetBodyToImu(float reset);
-/* can be provided implementation */
-extern void imu_scale_gyro(struct Imu *_imu);
-extern void imu_scale_accel(struct Imu *_imu);
-extern void imu_scale_mag(struct Imu *_imu);
-
#if !defined IMU_BODY_TO_IMU_PHI && !defined IMU_BODY_TO_IMU_THETA && !defined IMU_BODY_TO_IMU_PSI
#define IMU_BODY_TO_IMU_PHI 0
#define IMU_BODY_TO_IMU_THETA 0
#define IMU_BODY_TO_IMU_PSI 0
#endif
-#if !defined IMU_GYRO_P_NEUTRAL && !defined IMU_GYRO_Q_NEUTRAL && !defined IMU_GYRO_R_NEUTRAL
-#define IMU_GYRO_P_NEUTRAL 0
-#define IMU_GYRO_Q_NEUTRAL 0
-#define IMU_GYRO_R_NEUTRAL 0
-#endif
-
-#if !defined IMU_ACCEL_X_NEUTRAL && !defined IMU_ACCEL_Y_NEUTRAL && !defined IMU_ACCEL_Z_NEUTRAL
-#define IMU_ACCEL_X_NEUTRAL 0
-#define IMU_ACCEL_Y_NEUTRAL 0
-#define IMU_ACCEL_Z_NEUTRAL 0
-#endif
-
-#if !defined IMU_MAG_X_NEUTRAL && !defined IMU_MAG_Y_NEUTRAL && !defined IMU_MAG_Z_NEUTRAL
-#define IMU_MAG_X_NEUTRAL 0
-#define IMU_MAG_Y_NEUTRAL 0
-#define IMU_MAG_Z_NEUTRAL 0
-#endif
-
-#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
-#define IMU_GYRO_P_SIGN 1
-#define IMU_GYRO_Q_SIGN 1
-#define IMU_GYRO_R_SIGN 1
-#endif
-#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
-#define IMU_ACCEL_X_SIGN 1
-#define IMU_ACCEL_Y_SIGN 1
-#define IMU_ACCEL_Z_SIGN 1
-#endif
-#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
-#define IMU_MAG_X_SIGN 1
-#define IMU_MAG_Y_SIGN 1
-#define IMU_MAG_Z_SIGN 1
-#endif
-
#endif /* IMU_H */
diff --git a/sw/airborne/modules/imu/imu_cube.c b/sw/airborne/modules/imu/imu_cube.c
index 0e3842be5f..062b91ce2d 100644
--- a/sw/airborne/modules/imu/imu_cube.c
+++ b/sw/airborne/modules/imu/imu_cube.c
@@ -30,7 +30,8 @@
#include "peripherals/invensense2.h"
-struct invensense2_t imu1;
+static struct invensense2_t imu2;
+static struct invensense2_t imu3;
void imu_cube_init(void)
{
@@ -78,10 +79,12 @@ void imu_cube_init(void)
void imu_cube_periodic(void)
{
- invensense2_periodic(&imu1);
+ invensense2_periodic(&imu2);
+ invensense2_periodic(&imu3);
}
void imu_cube_event(void)
{
- invensense2_event(&imu1);
+ invensense2_event(&imu2);
+ invensense2_event(&imu3);
}
diff --git a/sw/airborne/modules/ins/ins_ekf2.cpp b/sw/airborne/modules/ins/ins_ekf2.cpp
index fc394f997c..b7279b1503 100644
--- a/sw/airborne/modules/ins/ins_ekf2.cpp
+++ b/sw/airborne/modules/ins/ins_ekf2.cpp
@@ -565,9 +565,9 @@ void ins_ekf2_init(void)
AbiBindMsgBARO_ABS(INS_EKF2_BARO_ID, &baro_ev, baro_cb);
AbiBindMsgTEMPERATURE(INS_EKF2_TEMPERATURE_ID, &temperature_ev, temperature_cb);
AbiBindMsgAGL(INS_EKF2_AGL_ID, &agl_ev, agl_cb);
- AbiBindMsgIMU_GYRO_INT32(INS_EKF2_GYRO_ID, &gyro_ev, gyro_cb);
- AbiBindMsgIMU_ACCEL_INT32(INS_EKF2_ACCEL_ID, &accel_ev, accel_cb);
- AbiBindMsgIMU_MAG_INT32(INS_EKF2_MAG_ID, &mag_ev, mag_cb);
+ AbiBindMsgIMU_GYRO(INS_EKF2_GYRO_ID, &gyro_ev, gyro_cb);
+ AbiBindMsgIMU_ACCEL(INS_EKF2_ACCEL_ID, &accel_ev, accel_cb);
+ AbiBindMsgIMU_MAG(INS_EKF2_MAG_ID, &mag_ev, mag_cb);
AbiBindMsgGPS(INS_EKF2_GPS_ID, &gps_ev, gps_cb);
AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
AbiBindMsgOPTICAL_FLOW(INS_EKF2_OF_ID, &optical_flow_ev, optical_flow_cb);
diff --git a/sw/airborne/modules/sensors/mag_lis3mdl.c b/sw/airborne/modules/sensors/mag_lis3mdl.c
index 1716ba9db2..51b5095d8f 100644
--- a/sw/airborne/modules/sensors/mag_lis3mdl.c
+++ b/sw/airborne/modules/sensors/mag_lis3mdl.c
@@ -52,13 +52,6 @@
#if MODULE_LIS3MDL_UPDATE_AHRS
#include "modules/imu/imu.h"
#include "modules/core/abi.h"
-
-#if defined LIS3MDL_MAG_TO_IMU_PHI && defined LIS3MDL_MAG_TO_IMU_THETA && defined LIS3MDL_MAG_TO_IMU_PSI
-#define USE_MAG_TO_IMU 1
-static struct Int32RMat mag_to_imu; ///< rotation from mag to imu frame
-#else
-#define USE_MAG_TO_IMU 0
-#endif
#endif
struct Lis3mdl mag_lis3mdl;
@@ -70,15 +63,6 @@ void mag_lis3mdl_module_init(void)
LIS3MDL_SCALE_4_GAUSS,
LIS3MDL_MODE_CONTINUOUS,
LIS3MDL_PERFORMANCE_ULTRA_HIGH);
-
-#if MODULE_LIS3MDL_UPDATE_AHRS && USE_MAG_TO_IMU
- struct Int32Eulers mag_to_imu_eulers = {
- ANGLE_BFP_OF_REAL(LIS3MDL_MAG_TO_IMU_PHI),
- ANGLE_BFP_OF_REAL(LIS3MDL_MAG_TO_IMU_THETA),
- ANGLE_BFP_OF_REAL(LIS3MDL_MAG_TO_IMU_PSI)
- };
- int32_rmat_of_eulers(&mag_to_imu, &mag_to_imu_eulers);
-#endif
}
void mag_lis3mdl_module_periodic(void)
@@ -101,21 +85,8 @@ void mag_lis3mdl_module_event(void)
LIS3MDL_CHAN_Y_SIGN(int32_t)(mag_lis3mdl.data.value[LIS3MDL_CHAN_Y]),
LIS3MDL_CHAN_Z_SIGN(int32_t)(mag_lis3mdl.data.value[LIS3MDL_CHAN_Z])
};
- // only rotate if needed
-#if USE_MAG_TO_IMU
- struct Int32Vect3 imu_mag;
- // rotate data from mag frame to imu frame
- int32_rmat_vmult(&imu_mag, &mag_to_imu, &mag);
- // unscaled vector
- VECT3_COPY(imu.mag_unscaled, imu_mag);
-#else
- // unscaled vector
- VECT3_COPY(imu.mag_unscaled, mag);
-#endif
- // scale vector
- imu_scale_mag(&imu);
- AbiSendMsgIMU_MAG_INT32(MAG_LIS3MDL_SENDER_ID, now_ts, &imu.mag);
+ AbiSendMsgIMU_MAG_RAW(MAG_LIS3MDL_SENDER_ID, now_ts, &mag);
#endif
#if MODULE_LIS3MDL_SYNC_SEND
mag_lis3mdl_report();
@@ -128,10 +99,11 @@ void mag_lis3mdl_module_event(void)
void mag_lis3mdl_report(void)
{
+ uint8_t id = MAG_LIS3MDL_SENDER_ID;
struct Int32Vect3 mag = {
LIS3MDL_CHAN_X_SIGN(int32_t)(mag_lis3mdl.data.value[LIS3MDL_CHAN_X]),
LIS3MDL_CHAN_Y_SIGN(int32_t)(mag_lis3mdl.data.value[LIS3MDL_CHAN_Y]),
LIS3MDL_CHAN_Z_SIGN(int32_t)(mag_lis3mdl.data.value[LIS3MDL_CHAN_Z])
};
- DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z);
+ DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &id, &mag.x, &mag.y, &mag.z);
}
diff --git a/sw/airborne/peripherals/invensense2.c b/sw/airborne/peripherals/invensense2.c
index 8c4559c8b6..1130bc4d5d 100644
--- a/sw/airborne/peripherals/invensense2.c
+++ b/sw/airborne/peripherals/invensense2.c
@@ -221,7 +221,7 @@ void invensense2_event(struct invensense2_t *inv) {
invensense2_parse_data(inv, &rx_buffer[3], valid_bytes, (inv->timer != 0));
inv->timer -= valid_bytes;
} else {
- fifo_bytes -= fifo_bytes%14;
+ fifo_bytes -= fifo_bytes%INVENSENSE2_SAMPLE_SIZE;
inv->timer = fifo_bytes;
}
@@ -276,9 +276,12 @@ void invensense2_event(struct invensense2_t *inv) {
* @param last If the data was the last available data in the FIFO buffer or if we are expecting more
*/
static void invensense2_parse_data(struct invensense2_t *inv, volatile uint8_t *data, uint16_t len, bool last) {
- uint8_t samples = len / 14;
- static struct Int32Vect3 accel = {0};
- static struct Int32Rates gyro = {0};
+ uint8_t samples = len / INVENSENSE2_SAMPLE_SIZE;
+ static struct Int32Vect3 accel[INVENSENSE2_SAMPLE_CNT] = {0};
+ static struct Int32Rates gyro[INVENSENSE2_SAMPLE_CNT] = {0};
+
+ if(samples > INVENSENSE2_SAMPLE_CNT)
+ samples = INVENSENSE2_SAMPLE_CNT;
uint16_t gyro_samplerate = 9000;
if(inv->gyro_dlpf != INVENSENSE2_GYRO_DLPF_OFF)
diff --git a/sw/airborne/peripherals/invensense2.h b/sw/airborne/peripherals/invensense2.h
index 294d9ef8c6..80ab174dff 100644
--- a/sw/airborne/peripherals/invensense2.h
+++ b/sw/airborne/peripherals/invensense2.h
@@ -34,7 +34,9 @@
#include "mcu_periph/i2c.h"
// Hold 22 measurements and 3 for the register address and length
-#define INVENSENSE2_BUFFER_SIZE ((14*22) + 3)
+#define INVENSENSE2_SAMPLE_CNT 22
+#define INVENSENSE2_SAMPLE_SIZE 14
+#define INVENSENSE2_BUFFER_SIZE ((INVENSENSE2_SAMPLE_SIZE*INVENSENSE2_SAMPLE_CNT) + 3)
/* Invensense v2 SPI peripheral */
struct invensense2_spi_t {
diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink
index 20cf56bcd0..a796278507 160000
--- a/sw/ext/pprzlink
+++ b/sw/ext/pprzlink
@@ -1 +1 @@
-Subproject commit 20cf56bcd0d24896a81abc682ed4b082b7391d78
+Subproject commit a7962785072cbd78fb34e94e5265ce3ed4c1597e
diff --git a/sw/tools/calibration/calibrate.py b/sw/tools/calibration/calibrate.py
index 2ae5c22a30..295d2d702f 100755
--- a/sw/tools/calibration/calibrate.py
+++ b/sw/tools/calibration/calibrate.py
@@ -37,6 +37,9 @@ def main():
parser.add_option("-i", "--id", dest="ac_id",
action="store",
help="aircraft id to use")
+ parser.add_option("-j", "--sid", dest="sensor_id",
+ action="store",
+ help="sensor id to use")
parser.add_option("-s", "--sensor", dest="sensor",
type="choice", choices=["ACCEL", "MAG"],
help="sensor to calibrate (ACCEL, MAG)",
@@ -75,6 +78,15 @@ def main():
if options.verbose:
print("Using aircraft id "+options.ac_id)
+ sensor_ids = calibration_utils.get_ids_in_log(options.ac_id, filename, options.sensor)
+ if options.sensor_id is None:
+ if len(sensor_ids) == 1:
+ options.sensor_id = sensor_ids[0]
+ else:
+ parser.error("More than one sensor id found in log file. Specify the id to use.")
+ if options.verbose:
+ print("Using sensor id "+options.sensor_id)
+
if options.sensor == "ACCEL":
sensor_ref = 9.81
sensor_res = 10
@@ -87,10 +99,10 @@ def main():
noise_threshold = options.noise_threshold
if options.verbose:
- print("reading file "+filename+" for aircraft "+options.ac_id+" and sensor "+options.sensor)
+ print("reading file "+filename+" for aircraft "+options.ac_id+" and sensor "+options.sensor +" with sensor id "+options.sensor_id)
# read raw measurements from log file
- measurements = calibration_utils.read_log(options.ac_id, filename, options.sensor)
+ measurements = calibration_utils.read_log(options.ac_id, filename, options.sensor, options.sensor_id)
if len(measurements) == 0:
print("Error: found zero IMU_"+options.sensor+"_RAW measurements for aircraft with id "+options.ac_id+" in log file!")
sys.exit(1)
diff --git a/sw/tools/calibration/calibration_utils.py b/sw/tools/calibration/calibration_utils.py
index 2e36f8cd5b..121d93c14f 100644
--- a/sw/tools/calibration/calibration_utils.py
+++ b/sw/tools/calibration/calibration_utils.py
@@ -47,11 +47,25 @@ def get_ids_in_log(filename):
ids.append(ac_id)
return ids
+def get_sensor_ids(ac_id, filename, sensor):
+ f = open(filename, 'r')
+ ids = []
+ pattern = re.compile("\S+ "+ac_id+" IMU_"+sensor+"_RAW (\S+) \S+ \S+ \S+")
+ while True:
+ line = f.readline().strip()
+ if line == '':
+ break
+ m = re.match(pattern, line)
+ if m:
+ sensor_id = m.group(1)
+ if not sensor_id in ids:
+ ids.append(sensor_id)
+ return ids
-def read_log(ac_id, filename, sensor):
+def read_log(ac_id, filename, sensor, sensor_id):
"""Extracts raw sensor measurements from a log."""
f = open(filename, 'r')
- pattern = re.compile("(\S+) "+ac_id+" IMU_"+sensor+"_RAW (\S+) (\S+) (\S+)")
+ pattern = re.compile("(\S+) "+ac_id+" IMU_"+sensor+"_RAW "+sensor_id+" (\S+) (\S+) (\S+)")
list_meas = []
while True:
line = f.readline().strip()
@@ -63,10 +77,10 @@ def read_log(ac_id, filename, sensor):
return np.array(list_meas)
-def read_log_scaled(ac_id, filename, sensor, t_start, t_end):
+def read_log_scaled(ac_id, filename, sensor, sensor_id, t_start, t_end):
"""Extracts scaled sensor measurements from a log."""
f = open(filename, 'r')
- pattern = re.compile("(\S+) "+ac_id+" IMU_"+sensor+"_SCALED (\S+) (\S+) (\S+)")
+ pattern = re.compile("(\S+) "+ac_id+" IMU_"+sensor+"_SCALED "+sensor_id+" (\S+) (\S+) (\S+)")
list_meas = []
while True:
line = f.readline().strip()