From 7c7cfbc395dc0c21a0b66b4c2740792430ed4f3a Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Mon, 16 May 2022 10:16:46 +0200 Subject: [PATCH] [imu] Multi IMU support --- conf/abi.xml | 35 +- conf/airframes/examples/cube_orange.xml | 2 +- conf/modules/imu_cube.xml | 29 +- sw/airborne/modules/core/abi_sender_ids.h | 2 +- sw/airborne/modules/imu/imu.c | 426 +++++++++++++++++----- sw/airborne/modules/imu/imu.h | 97 +++-- sw/airborne/modules/imu/imu_cube.c | 9 +- sw/airborne/modules/ins/ins_ekf2.cpp | 6 +- sw/airborne/modules/sensors/mag_lis3mdl.c | 34 +- sw/airborne/peripherals/invensense2.c | 11 +- sw/airborne/peripherals/invensense2.h | 4 +- sw/ext/pprzlink | 2 +- sw/tools/calibration/calibrate.py | 16 +- sw/tools/calibration/calibration_utils.py | 22 +- 14 files changed, 478 insertions(+), 217 deletions(-) diff --git a/conf/abi.xml b/conf/abi.xml index 7b8b360d41a..be92c384d4d 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 f0c25f98520..0dfef3fc7f5 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 f5011d13b23..cf31eea861b 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 0dd9099780f..c575ffd3a6e 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,93 +42,153 @@ 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) { + // 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); -#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); + // 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); + } - VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); + 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 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 + 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); + } + } - 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); + // 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); @@ -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 026dd9af89e..5edb9b3cbbd 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 0e3842be5fa..062b91ce2db 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 fc394f997c6..b7279b15038 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 1716ba9db27..51b5095d8fe 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 8c4559c8b6b..1130bc4d5dc 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 294d9ef8c62..80ab174dff8 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 20cf56bcd0d..a7962785072 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 2ae5c22a30f..295d2d702fc 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 2e36f8cd5b7..121d93c14f9 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()