From 4df4c8436b58f6575524788b251fe83ec4564303 Mon Sep 17 00:00:00 2001 From: Fabien-B Date: Wed, 25 Jun 2025 13:27:53 +0200 Subject: [PATCH] Imu scale float (#3477) * [imu] Use floats for scaling * [imu] default scales float * [imu] update comments * Fix print function * [imu] fix calibration * [imu] calibration in float * [imu] remove old calibration generation. compilation error if IMU_IMU_ACCEL_CALIB is defined. * remove static operations Co-authored-by: Gautier Hattenberger * clarify scales --------- Co-authored-by: Fabien-B Co-authored-by: Gautier Hattenberger --- .../simulator/nps/nps_sensors_params_common.h | 36 ----- sw/airborne/boards/apogee/imu_apogee.c | 4 +- sw/airborne/boards/ardrone/navdata.c | 22 +-- sw/airborne/modules/imu/imu.c | 125 +++++++++++------- sw/airborne/modules/imu/imu.h | 10 +- sw/airborne/modules/imu/imu_aspirin.c | 19 +-- sw/airborne/modules/imu/imu_aspirin_2_spi.c | 4 +- sw/airborne/modules/imu/imu_aspirin_i2c.c | 19 +-- sw/airborne/modules/imu/imu_bebop.c | 4 +- sw/airborne/modules/imu/imu_bmi088_i2c.c | 4 +- sw/airborne/modules/imu/imu_cube.c | 8 +- sw/airborne/modules/imu/imu_disco.c | 4 +- sw/airborne/modules/imu/imu_mpu6000.c | 4 +- sw/airborne/modules/imu/imu_mpu6000_hmc5883.c | 4 +- sw/airborne/modules/imu/imu_mpu60x0_i2c.c | 4 +- sw/airborne/modules/imu/imu_mpu9250_i2c.c | 4 +- sw/airborne/modules/imu/imu_mpu9250_spi.c | 4 +- sw/airborne/modules/imu/imu_nps.c | 33 ++--- sw/airborne/modules/imu/imu_px4fmu.c | 4 +- sw/airborne/modules/imu/imu_px4fmu_v2.4.c | 14 +- sw/airborne/modules/sensors/sensors_hitl.c | 34 ++--- sw/airborne/peripherals/bmi088.c | 31 ++--- sw/airborne/peripherals/bmi088.h | 5 +- sw/airborne/peripherals/invensense2.c | 38 ++---- sw/airborne/peripherals/invensense3.c | 48 +++---- sw/airborne/peripherals/l3gd20.h | 3 +- sw/airborne/peripherals/lsm303d.h | 3 +- sw/airborne/peripherals/mpu60x0.c | 30 ++--- sw/airborne/peripherals/mpu60x0.h | 23 +--- sw/airborne/peripherals/mpu9250.c | 28 ++-- sw/airborne/peripherals/mpu9250.h | 21 +-- sw/tools/calibration/calibration_utils.py | 40 +----- 32 files changed, 233 insertions(+), 401 deletions(-) diff --git a/conf/simulator/nps/nps_sensors_params_common.h b/conf/simulator/nps/nps_sensors_params_common.h index 4afecc4eb4..98b09be739 100644 --- a/conf/simulator/nps/nps_sensors_params_common.h +++ b/conf/simulator/nps/nps_sensors_params_common.h @@ -60,30 +60,18 @@ #ifdef IMU_ACCEL_X_SENS #define NPS_ACCEL_SENSITIVITY_XX ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS) -#define NPS_ACCEL_SENSITIVITY_XX_NUM ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS_NUM) -#define NPS_ACCEL_SENSITIVITY_XX_DEN ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS_DEN) #else #define NPS_ACCEL_SENSITIVITY_XX ACCEL_BFP_OF_REAL(1.) -#define NPS_ACCEL_SENSITIVITY_XX_NUM 1 -#define NPS_ACCEL_SENSITIVITY_XX_DEN 1 #endif #ifdef IMU_ACCEL_Y_SENS #define NPS_ACCEL_SENSITIVITY_YY ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS) -#define NPS_ACCEL_SENSITIVITY_YY_NUM ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS_NUM) -#define NPS_ACCEL_SENSITIVITY_YY_DEN ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS_DEN) #else #define NPS_ACCEL_SENSITIVITY_YY ACCEL_BFP_OF_REAL(1.) -#define NPS_ACCEL_SENSITIVITY_YY_NUM 1 -#define NPS_ACCEL_SENSITIVITY_YY_DEN 1 #endif #ifdef IMU_ACCEL_Z_SENS #define NPS_ACCEL_SENSITIVITY_ZZ ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS) -#define NPS_ACCEL_SENSITIVITY_ZZ_NUM ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS_NUM) -#define NPS_ACCEL_SENSITIVITY_ZZ_DEN ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS_DEN) #else #define NPS_ACCEL_SENSITIVITY_ZZ ACCEL_BFP_OF_REAL(1.) -#define NPS_ACCEL_SENSITIVITY_ZZ_NUM 1 -#define NPS_ACCEL_SENSITIVITY_ZZ_DEN 1 #endif #ifdef IMU_ACCEL_X_NEUTRAL @@ -142,30 +130,18 @@ #ifdef IMU_GYRO_P_SENS #define NPS_GYRO_SENSITIVITY_PP RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS) -#define NPS_GYRO_SENSITIVITY_PP_NUM RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS_NUM) -#define NPS_GYRO_SENSITIVITY_PP_DEN RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS_DEN) #else #define NPS_GYRO_SENSITIVITY_PP RATE_BFP_OF_REAL(1.) -#define NPS_GYRO_SENSITIVITY_PP_NUM 1 -#define NPS_GYRO_SENSITIVITY_PP_DEN 1 #endif #ifdef IMU_GYRO_Q_SENS #define NPS_GYRO_SENSITIVITY_QQ RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS) -#define NPS_GYRO_SENSITIVITY_QQ_NUM RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS_NUM) -#define NPS_GYRO_SENSITIVITY_QQ_DEN RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS_DEN) #else #define NPS_GYRO_SENSITIVITY_QQ RATE_BFP_OF_REAL(1.) -#define NPS_GYRO_SENSITIVITY_QQ_NUM 1 -#define NPS_GYRO_SENSITIVITY_QQ_DEN 1 #endif #ifdef IMU_GYRO_R_SENS #define NPS_GYRO_SENSITIVITY_RR RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS) -#define NPS_GYRO_SENSITIVITY_RR_NUM RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS_NUM) -#define NPS_GYRO_SENSITIVITY_RR_DEN RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS_DEN) #else #define NPS_GYRO_SENSITIVITY_RR RATE_BFP_OF_REAL(1.) -#define NPS_GYRO_SENSITIVITY_RR_NUM 1 -#define NPS_GYRO_SENSITIVITY_RR_DEN 1 #endif #ifdef IMU_GYRO_P_NEUTRAL @@ -232,30 +208,18 @@ #ifdef IMU_MAG_X_SENS #define NPS_MAG_SENSITIVITY_XX MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS) -#define NPS_MAG_SENSITIVITY_XX_NUM MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS_NUM) -#define NPS_MAG_SENSITIVITY_XX_DEN MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS_DEN) #else #define NPS_MAG_SENSITIVITY_XX MAG_BFP_OF_REAL(1.) -#define NPS_MAG_SENSITIVITY_XX_NUM 1 -#define NPS_MAG_SENSITIVITY_XX_DEN 1 #endif #ifdef IMU_MAG_Y_SENS #define NPS_MAG_SENSITIVITY_YY MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS) -#define NPS_MAG_SENSITIVITY_YY_NUM MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS_NUM) -#define NPS_MAG_SENSITIVITY_YY_DEN MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS_DEN) #else #define NPS_MAG_SENSITIVITY_YY MAG_BFP_OF_REAL(1.) -#define NPS_MAG_SENSITIVITY_YY_NUM 1 -#define NPS_MAG_SENSITIVITY_YY_DEN 1 #endif #ifdef IMU_MAG_Z_SENS #define NPS_MAG_SENSITIVITY_ZZ MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS) -#define NPS_MAG_SENSITIVITY_ZZ_NUM MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS_NUM) -#define NPS_MAG_SENSITIVITY_ZZ_DEN MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS_DEN) #else #define NPS_MAG_SENSITIVITY_ZZ MAG_BFP_OF_REAL(1.) -#define NPS_MAG_SENSITIVITY_ZZ_NUM 1 -#define NPS_MAG_SENSITIVITY_ZZ_DEN 1 #endif #ifdef IMU_MAG_X_NEUTRAL diff --git a/sw/airborne/boards/apogee/imu_apogee.c b/sw/airborne/boards/apogee/imu_apogee.c index c1f2a00851..1acdbcd3ad 100644 --- a/sw/airborne/boards/apogee/imu_apogee.c +++ b/sw/airborne/boards/apogee/imu_apogee.c @@ -107,8 +107,8 @@ void imu_apogee_init(void) imu_apogee.mpu.config.slaves[0].configure = &configure_baro_slave; imu_apogee.mpu.config.i2c_bypass = true; // set the default values - imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[APOGEE_GYRO_RANGE]); - imu_set_defaults_accel(IMU_BOARD_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[APOGEE_ACCEL_RANGE]); + imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, &MPU60X0_GYRO_SENS_F[APOGEE_GYRO_RANGE]); + imu_set_defaults_accel(IMU_BOARD_ID, NULL, NULL, &MPU60X0_ACCEL_SENS_F[APOGEE_ACCEL_RANGE]); #if APOGEE_USE_MPU9150 // if using MPU9150, internal mag needs to be configured diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index cabbb57c11..fec8f6da50 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -89,18 +89,13 @@ static pthread_cond_t navdata_cond = PTHREAD_COND_INITIALIZER; /** * Default gyro scale */ -static const struct Int32Rates gyro_scale[2] = { - {4359, 4359, 4359}, - {1000, 1000, 1000} -}; +static const struct FloatVect3 gyro_scale_f = { 4.359f, 4.359f, 4.359f }; /** * Default accel scale/neutral */ -static const struct Int32Vect3 accel_scale[2] = { - {195, 195, 195}, - {10, 10, 10} -}; +static const struct FloatVect3 accel_scale_f = { 19.5f, 19.5f, 19.5f }; + static const struct Int32Vect3 accel_neutral = { 2048, 2048, 2048 }; @@ -108,10 +103,7 @@ static const struct Int32Vect3 accel_neutral = { /** * Default mag scale */ -static const struct Int32Vect3 mag_scale[2] = { - {16, 16, 16}, - {1, 1, 1} -}; +static const struct FloatVect3 mag_scale_f = {16, 16, 16}; /** * Write to fd even while being interrupted @@ -255,9 +247,9 @@ bool navdata_init() navdata.baro_calibrated = true; /* Set the default scalings/neutrals */ - imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, gyro_scale); - imu_set_defaults_accel(IMU_BOARD_ID, NULL, &accel_neutral, accel_scale); - imu_set_defaults_mag(IMU_BOARD_ID, NULL, NULL, mag_scale); + imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, &gyro_scale_f); + imu_set_defaults_accel(IMU_BOARD_ID, NULL, &accel_neutral, &accel_scale_f); + imu_set_defaults_mag(IMU_BOARD_ID, NULL, NULL, &mag_scale_f); /* Start acquisition */ navdata_cmd_send(NAVDATA_CMD_START); diff --git a/sw/airborne/modules/imu/imu.c b/sw/airborne/modules/imu/imu.c index 36299d9acb..83bbb61090 100644 --- a/sw/airborne/modules/imu/imu.c +++ b/sw/airborne/modules/imu/imu.c @@ -74,6 +74,10 @@ PRINT_CONFIG_MSG("Using single IMU gyro sensitivity calibration") #define IMU_GYRO_CALIB {} #endif +#if defined(IMU_IMU_ACCEL_CALIB) +#error IMU_IMU_ACCEL_CALIB defined. Rename it "ACCEL_CALIB" if your calibration is in a section with the "IMU_" prefix. +#endif + /** By default accel signs are positive for single IMU with old format and defaults */ #if defined(IMU_ACCEL_CALIB) && (defined(IMU_ACCEL_X_SIGN) || defined(IMU_ACCEL_Y_SIGN) || defined(IMU_ACCEL_Z_SIGN)) #warning "The IMU_ACCEL_?_SIGN's aren't compatible with the IMU_ACCEL_CALIB define in the airframe" @@ -348,10 +352,15 @@ static void show_vect3(shell_stream_t *sh, char* name, struct Int32Vect3 *v) { chprintf(sh, " %s: %ld, %ld, %ld\r\n", name, v->x, v->y, v->z); } +static void show_vect3f(shell_stream_t *sh, char* name, struct FloatVect3 *v) { + chprintf(sh, " %s: %f, %f, %f\r\n", name, v->x, v->y, v->z); +} + static void show_rates(shell_stream_t *sh, char* name, struct Int32Rates *r) { chprintf(sh, " %s: %ld, %ld, %ld\r\n", name, r->p, r->q, r->r); } + static void show_matrix(shell_stream_t *sh, char* name, struct Int32RMat *m) { chprintf(sh, " %s:\r\n", name); chprintf(sh, " %ld, %ld, %ld\r\n", MAT33_ELMT(*m, 0, 0), MAT33_ELMT(*m, 0, 1), MAT33_ELMT(*m, 0, 2)); @@ -373,8 +382,7 @@ static void cmd_imu(shell_stream_t *sh, int argc, const char *const argv[]) chprintf(sh, " Gyro id: %u, time %lu\r\n", imu.gyros[i].abi_id, imu.gyros[i].last_stamp); show_calibrated(sh, &imu.gyros[i].calibrated); show_rates(sh, "neutral", &imu.gyros[i].neutral); - show_rates(sh, "scale_num", &imu.gyros[i].scale[0]); - show_rates(sh, "scale_den", &imu.gyros[i].scale[1]); + show_vect3f(sh, "scale", &imu.gyros[i].scale_f); show_matrix(sh, "body_to_sensor", &imu.gyros[i].body_to_sensor); show_rates(sh, "unscaled", &imu.gyros[i].unscaled); show_rates(sh, "scaled", &imu.gyros[i].scaled); @@ -389,8 +397,7 @@ static void cmd_imu(shell_stream_t *sh, int argc, const char *const argv[]) chprintf(sh, " Accel id: %u, time %lu\r\n", imu.accels[i].abi_id, imu.accels[i].last_stamp); show_calibrated(sh, &imu.accels[i].calibrated); show_vect3(sh, "neutral", &imu.accels[i].neutral); - show_vect3(sh, "scale_num", &imu.accels[i].scale[0]); - show_vect3(sh, "scale_den", &imu.accels[i].scale[1]); + show_vect3f(sh, "scale", &imu.accels[i].scale_f); show_matrix(sh, "body_to_sensor", &imu.accels[i].body_to_sensor); show_vect3(sh, "unscaled", &imu.accels[i].unscaled); show_vect3(sh, "scaled", &imu.accels[i].scaled); @@ -404,9 +411,8 @@ static void cmd_imu(shell_stream_t *sh, int argc, const char *const argv[]) if (imu.mags[i].abi_id != 0) { chprintf(sh, " Mag id: %u\r\n", imu.mags[i].abi_id); show_calibrated(sh, &imu.mags[i].calibrated); - show_vect3(sh, "neutral", &imu.mags[i].neutral); - show_vect3(sh, "scale_num", &imu.mags[i].scale[0]); - show_vect3(sh, "scale_den", &imu.mags[i].scale[1]); + show_vect3(sh, "neutral", &imu.mags[i].neutral); + show_vect3f(sh, "scale", &imu.mags[i].scale_f); show_matrix(sh, "body_to_sensor", &imu.mags[i].body_to_sensor); show_vect3(sh, "unscaled", &imu.mags[i].unscaled); show_vect3(sh, "scaled", &imu.mags[i].scaled); @@ -450,6 +456,7 @@ void imu_init(void) imu.gyros[i].abi_id = ABI_DISABLE; imu.gyros[i].calibrated.neutral = false; imu.gyros[i].calibrated.scale = false; + imu.gyros[i].calibrated.scale_f = false; imu.gyros[i].calibrated.rotation = false; imu.gyros[i].calibrated.filter = false; } else { @@ -460,10 +467,18 @@ void imu_init(void) if(!imu.gyros[i].calibrated.neutral) { INT_RATES_ZERO(imu.gyros[i].neutral); } - if(!imu.gyros[i].calibrated.scale) { - RATES_ASSIGN(imu.gyros[i].scale[0], IMU_GYRO_P_SIGN, IMU_GYRO_Q_SIGN, IMU_GYRO_R_SIGN); - RATES_ASSIGN(imu.gyros[i].scale[1], 1, 1, 1); + + if(!imu.gyros[i].calibrated.scale_f) { + if(imu.gyros[i].calibrated.scale) { + imu.gyros[i].scale_f.x = (float)imu.gyros[i].scale[0].p / (float)imu.gyros[i].scale[1].p; + imu.gyros[i].scale_f.y = (float)imu.gyros[i].scale[0].q / (float)imu.gyros[i].scale[1].q; + imu.gyros[i].scale_f.z = (float)imu.gyros[i].scale[0].r / (float)imu.gyros[i].scale[1].r; + imu.gyros[i].calibrated.scale_f = true; + } else { + VECT3_ASSIGN(imu.accels[i].scale_f, IMU_GYRO_P_SIGN, IMU_GYRO_Q_SIGN, IMU_GYRO_R_SIGN); + } } + if(!imu.gyros[i].calibrated.rotation) { int32_rmat_identity(&imu.gyros[i].body_to_sensor); } @@ -483,6 +498,7 @@ void imu_init(void) imu.accels[i].abi_id = ABI_DISABLE; imu.accels[i].calibrated.neutral = false; imu.accels[i].calibrated.scale = false; + imu.accels[i].calibrated.scale_f = false; imu.accels[i].calibrated.rotation = false; imu.accels[i].calibrated.filter = false; } else { @@ -493,10 +509,18 @@ void imu_init(void) if(!imu.accels[i].calibrated.neutral) { INT_VECT3_ZERO(imu.accels[i].neutral); } - if(!imu.accels[i].calibrated.scale) { - VECT3_ASSIGN(imu.accels[i].scale[0], IMU_ACCEL_X_SIGN, IMU_ACCEL_Y_SIGN, IMU_ACCEL_Z_SIGN); - VECT3_ASSIGN(imu.accels[i].scale[1], 1, 1, 1); + + if(!imu.accels[i].calibrated.scale_f) { + if(imu.accels[i].calibrated.scale) { + imu.accels[i].scale_f.x = (float)imu.accels[i].scale[0].x / (float)imu.accels[i].scale[1].x; + imu.accels[i].scale_f.y = (float)imu.accels[i].scale[0].y / (float)imu.accels[i].scale[1].y; + imu.accels[i].scale_f.z = (float)imu.accels[i].scale[0].z / (float)imu.accels[i].scale[1].z; + imu.accels[i].calibrated.scale_f = true; + } else { + VECT3_ASSIGN(imu.accels[i].scale_f, IMU_ACCEL_X_SIGN, IMU_ACCEL_Y_SIGN, IMU_ACCEL_Z_SIGN); + } } + if(!imu.accels[i].calibrated.rotation) { int32_rmat_identity(&imu.accels[i].body_to_sensor); } @@ -516,6 +540,7 @@ void imu_init(void) imu.mags[i].abi_id = ABI_DISABLE; imu.mags[i].calibrated.neutral = false; imu.mags[i].calibrated.scale = false; + imu.mags[i].calibrated.scale_f = false; imu.mags[i].calibrated.rotation = false; imu.mags[i].calibrated.current = false; } else { @@ -526,9 +551,16 @@ void imu_init(void) if(!imu.mags[i].calibrated.neutral) { INT_VECT3_ZERO(imu.mags[i].neutral); } - if(!imu.mags[i].calibrated.scale) { - VECT3_ASSIGN(imu.mags[i].scale[0], IMU_MAG_X_SIGN, IMU_MAG_Y_SIGN, IMU_MAG_Z_SIGN); - VECT3_ASSIGN(imu.mags[i].scale[1], 1, 1, 1); + + if(!imu.mags[i].calibrated.scale_f) { + if(imu.mags[i].calibrated.scale) { + imu.mags[i].scale_f.x = (float)imu.mags[i].scale[0].x / (float)imu.mags[i].scale[1].x; + imu.mags[i].scale_f.y = (float)imu.mags[i].scale[0].y / (float)imu.mags[i].scale[1].y; + imu.mags[i].scale_f.z = (float)imu.mags[i].scale[0].z / (float)imu.mags[i].scale[1].z; + imu.mags[i].calibrated.scale_f = true; + }else { + VECT3_ASSIGN(imu.mags[i].scale_f, IMU_MAG_X_SIGN, IMU_MAG_Y_SIGN, IMU_MAG_Z_SIGN); + } } if(!imu.mags[i].calibrated.rotation) { int32_rmat_identity(&imu.mags[i].body_to_sensor); @@ -575,9 +607,9 @@ void imu_init(void) * @param abi_id The ABI sender id to set the defaults for * @param imu_to_sensor Imu to sensor rotation matrix * @param neutral Neutral values - * @param scale Scale values, 0 index is multiply and 1 index is divide + * @param scale_f Scale values */ -void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct Int32Rates *scale) +void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct FloatVect3 *scale_f) { // Find the correct gyro struct imu_gyro_t *gyro = imu_get_gyro(abi_id, true); @@ -593,9 +625,8 @@ void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor } if(neutral != NULL && !gyro->calibrated.neutral) RATES_COPY(gyro->neutral, *neutral); - if(scale != NULL && !gyro->calibrated.scale) { - RATES_ASSIGN(gyro->scale[0], IMU_GYRO_P_SIGN*scale[0].p, IMU_GYRO_Q_SIGN*scale[0].q, IMU_GYRO_R_SIGN*scale[0].r); - RATES_COPY(gyro->scale[1], scale[1]); + if(scale_f != NULL && !gyro->calibrated.scale_f) { + VECT3_ASSIGN(gyro->scale_f, IMU_MAG_X_SIGN*scale_f->x, IMU_MAG_Y_SIGN*scale_f->y, IMU_MAG_Z_SIGN*scale_f->z); } } @@ -605,9 +636,9 @@ void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor * @param abi_id The ABI sender id to set the defaults for * @param imu_to_sensor Imu to sensor rotation matrix * @param neutral Neutral values - * @param scale Scale values, 0 index is multiply and 1 index is divide + * @param scale_f Scale values */ -void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale) +void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f) { // Find the correct accel struct imu_accel_t *accel = imu_get_accel(abi_id, true); @@ -623,9 +654,8 @@ void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_senso } if(neutral != NULL && !accel->calibrated.neutral) VECT3_COPY(accel->neutral, *neutral); - if(scale != NULL && !accel->calibrated.scale) { - VECT3_ASSIGN(accel->scale[0], IMU_ACCEL_X_SIGN*scale[0].x, IMU_ACCEL_Y_SIGN*scale[0].y, IMU_ACCEL_Z_SIGN*scale[0].z); - VECT3_COPY(accel->scale[1], scale[1]); + if(scale_f != NULL && !accel->calibrated.scale_f) { + VECT3_ASSIGN(accel->scale_f, IMU_MAG_X_SIGN*scale_f->x, IMU_MAG_Y_SIGN*scale_f->y, IMU_MAG_Z_SIGN*scale_f->z); } } @@ -635,9 +665,9 @@ void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_senso * @param abi_id The ABI sender id to set the defaults for * @param imu_to_sensor Imu to sensor rotation matrix * @param neutral Neutral values - * @param scale Scale values, 0 index is multiply and 1 index is divide + * @param scale_f Scale values */ -void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale) +void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f) { // Find the correct mag struct imu_mag_t *mag = imu_get_mag(abi_id, true); @@ -653,9 +683,8 @@ void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, } if(neutral != NULL && !mag->calibrated.neutral) VECT3_COPY(mag->neutral, *neutral); - if(scale != NULL && !mag->calibrated.scale) { - VECT3_ASSIGN(mag->scale[0], IMU_MAG_X_SIGN*scale[0].x, IMU_MAG_Y_SIGN*scale[0].y, IMU_MAG_Z_SIGN*scale[0].z); - VECT3_COPY(mag->scale[1], scale[1]); + if(scale_f != NULL && !mag->calibrated.scale_f) { + VECT3_ASSIGN(mag->scale_f, IMU_MAG_X_SIGN*scale_f->x, IMU_MAG_Y_SIGN*scale_f->y, IMU_MAG_Z_SIGN*scale_f->z); } } @@ -682,9 +711,9 @@ static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates // 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; + scaled.p = (gyro->unscaled.p - gyro->neutral.p) * gyro->scale_f.x; + scaled.q = (gyro->unscaled.q - gyro->neutral.q) * gyro->scale_f.y; + scaled.r = (gyro->unscaled.r - gyro->neutral.r) * gyro->scale_f.z; // Rotate the sensor int32_rmat_transp_ratemult(&scaled_rot, &gyro->body_to_sensor, &scaled); @@ -710,9 +739,10 @@ static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates // Add all the other samples for(uint8_t i = 0; i < samples-1; i++) { struct FloatRates f_sample; - f_sample.p = RATE_FLOAT_OF_BFP((data[i].p - gyro->neutral.p) * gyro->scale[0].p / gyro->scale[1].p); - f_sample.q = RATE_FLOAT_OF_BFP((data[i].q - gyro->neutral.q) * gyro->scale[0].q / gyro->scale[1].q); - f_sample.r = RATE_FLOAT_OF_BFP((data[i].r - gyro->neutral.r) * gyro->scale[0].r / gyro->scale[1].r); + f_sample.p = RATE_FLOAT_OF_BFP((data[i].p - gyro->neutral.p) * gyro->scale_f.x); + f_sample.q = RATE_FLOAT_OF_BFP((data[i].q - gyro->neutral.q) * gyro->scale_f.y); + f_sample.r = RATE_FLOAT_OF_BFP((data[i].r - gyro->neutral.r) * gyro->scale_f.z); + #if IMU_LOG_HIGHSPEED pprz_msg_send_IMU_GYRO(&pprzlog_tp.trans_tx, &(IMU_LOG_HIGHSPEED_DEVICE).device, AC_ID, &sender_id, &f_sample.p, &f_sample.q, &f_sample.r); @@ -776,9 +806,10 @@ static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect // 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; + scaled.x = (accel->unscaled.x - accel->neutral.x) * accel->scale_f.x; + scaled.y = (accel->unscaled.y - accel->neutral.y) * accel->scale_f.y; + scaled.z = (accel->unscaled.z - accel->neutral.z) * accel->scale_f.z; + // Rotate the sensor int32_rmat_transp_vmult(&scaled_rot, &accel->body_to_sensor, &scaled); @@ -804,9 +835,10 @@ static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect // Add all the other samples for(uint8_t i = 0; i < samples-1; i++) { struct FloatVect3 f_sample; - f_sample.x = ACCEL_FLOAT_OF_BFP((data[i].x - accel->neutral.x) * accel->scale[0].x / accel->scale[1].x); - f_sample.y = ACCEL_FLOAT_OF_BFP((data[i].y - accel->neutral.y) * accel->scale[0].y / accel->scale[1].y); - f_sample.z = ACCEL_FLOAT_OF_BFP((data[i].z - accel->neutral.z) * accel->scale[0].z / accel->scale[1].z); + f_sample.x = ACCEL_FLOAT_OF_BFP((data[i].x - accel->neutral.x) * accel->scale_f.x); + f_sample.y = ACCEL_FLOAT_OF_BFP((data[i].y - accel->neutral.y) * accel->scale_f.y); + f_sample.z = ACCEL_FLOAT_OF_BFP((data[i].z - accel->neutral.z) * accel->scale_f.z); + #if IMU_LOG_HIGHSPEED pprz_msg_send_IMU_ACCEL(&pprzlog_tp.trans_tx, &(IMU_LOG_HIGHSPEED_DEVICE).device, AC_ID, &sender_id, &f_sample.x, &f_sample.y, &f_sample.z); @@ -865,9 +897,10 @@ static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 // 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; + scaled.x = (mag->unscaled.x - mag_correction.x - mag->neutral.x) * mag->scale_f.x; + scaled.y = (mag->unscaled.y - mag_correction.y - mag->neutral.y) * mag->scale_f.y; + scaled.z = (mag->unscaled.z - mag_correction.z - mag->neutral.z) * mag->scale_f.z; + // Rotate the sensor int32_rmat_transp_vmult(&mag->scaled, &mag->body_to_sensor, &scaled); diff --git a/sw/airborne/modules/imu/imu.h b/sw/airborne/modules/imu/imu.h index 759e5eb3ff..00510d406d 100644 --- a/sw/airborne/modules/imu/imu.h +++ b/sw/airborne/modules/imu/imu.h @@ -41,6 +41,7 @@ struct imu_calib_t { bool neutral: 1; ///< Neutral values calibrated bool scale: 1; ///< Scale calibrated + bool scale_f: 1; ///< Scale calibrated with floating point bool rotation: 1; ///< Rotation calibrated bool current: 1; ///< Current calibrated bool filter: 1; ///< Enable the lowpass filter @@ -55,6 +56,7 @@ struct imu_gyro_t { float temperature; ///< Temperature in degrees celcius struct Int32Rates neutral; ///< Neutral values, compensation on unscaled->scaled struct Int32Rates scale[2]; ///< Scaling, first is numerator and second denominator + struct FloatVect3 scale_f; ///< Scaling struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame (body to imu combined with imu to sensor) float filter_freq; ///< Filter frequency float filter_sample_freq; ///< Lowpass filter sample frequency (Hz) @@ -70,6 +72,7 @@ struct imu_accel_t { float temperature; ///< Temperature in degrees celcius struct Int32Vect3 neutral; ///< Neutral values, compensation on unscaled->scaled struct Int32Vect3 scale[2]; ///< Scaling, first is numerator and second denominator + struct FloatVect3 scale_f; ///< Scaling struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame (body to imu combined with imu to sensor) float filter_freq; ///< Lowpass filter frequency (Hz) float filter_sample_freq; ///< Lowpass filter sample frequency (Hz) @@ -83,6 +86,7 @@ struct imu_mag_t { struct Int32Vect3 unscaled; ///< Last unscaled values in sensor frame struct Int32Vect3 neutral; ///< Neutral values, compensation on unscaled->scaled struct Int32Vect3 scale[2]; ///< Scaling, first is numerator and second denominator + struct FloatVect3 scale_f; ///< Scaling struct FloatVect3 current_scale; ///< Current scaling multiplying struct Int32RMat body_to_sensor; ///< Rotation from body to sensor frame (body to imu combined with imu to sensor) }; @@ -111,9 +115,9 @@ extern struct Imu imu; /** External functions */ extern void imu_init(void); -extern void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct Int32Rates *scale); -extern void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale); -extern void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale); +extern void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct FloatVect3 *scale_f); +extern void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f); +extern void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f); extern struct imu_gyro_t *imu_get_gyro(uint8_t sender_id, bool create); extern struct imu_accel_t *imu_get_accel(uint8_t sender_id, bool create); diff --git a/sw/airborne/modules/imu/imu_aspirin.c b/sw/airborne/modules/imu/imu_aspirin.c index 5980d59e6e..a2896a56fd 100644 --- a/sw/airborne/modules/imu/imu_aspirin.c +++ b/sw/airborne/modules/imu/imu_aspirin.c @@ -90,24 +90,15 @@ void imu_aspirin_init(void) // Default scaling values #ifdef IMU_ASPIRIN_VERSION_1_5 - const struct Int32Rates gyro_scale[2] = { - {4359, 4359, 4359}, - {1000, 1000, 1000} - }; + const struct FloatVect3 gyro_scale_f = { 4.359f, 4.359f, 4.359f }; #else - const struct Int32Rates gyro_scale[2] = { - {4973, 4973, 4973}, - {1000, 1000, 1000} - }; + const struct FloatVect3 gyro_scale_f = { 4.973f, 4.973f, 4.973f }; #endif - const struct Int32Vect3 accel_scale[2] = { - {3791, 3791, 3791}, - {100, 100, 100} - }; + const struct FloatVect3 accel_scale_f = { 37.91f, 37.91f, 37.91f }; // Set the default scaling - imu_set_defaults_gyro(IMU_ASPIRIN_ID, NULL, NULL, gyro_scale); - imu_set_defaults_accel(IMU_ASPIRIN_ID, NULL, NULL, accel_scale); + imu_set_defaults_gyro(IMU_ASPIRIN_ID, NULL, NULL, &gyro_scale_f); + imu_set_defaults_accel(IMU_ASPIRIN_ID, NULL, NULL, &accel_scale_f); /* initialize mag and set default options */ hmc58xx_init(&imu_aspirin.mag_hmc, &(ASPIRIN_I2C_DEV), HMC58XX_ADDR); diff --git a/sw/airborne/modules/imu/imu_aspirin_2_spi.c b/sw/airborne/modules/imu/imu_aspirin_2_spi.c index 053f44910f..57460332df 100644 --- a/sw/airborne/modules/imu/imu_aspirin_2_spi.c +++ b/sw/airborne/modules/imu/imu_aspirin_2_spi.c @@ -108,8 +108,8 @@ void imu_aspirin2_init(void) imu_aspirin2.mpu.config.accel_range = ASPIRIN_2_ACCEL_RANGE; // Set the default scaling - imu_set_defaults_gyro(IMU_ASPIRIN2_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[ASPIRIN_2_GYRO_RANGE]); - imu_set_defaults_accel(IMU_ASPIRIN2_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[ASPIRIN_2_ACCEL_RANGE]); + imu_set_defaults_gyro(IMU_ASPIRIN2_ID, NULL, NULL, &MPU60X0_GYRO_SENS_F[ASPIRIN_2_GYRO_RANGE]); + imu_set_defaults_accel(IMU_ASPIRIN2_ID, NULL, NULL, &MPU60X0_ACCEL_SENS_F[ASPIRIN_2_ACCEL_RANGE]); /* read 15 bytes for status, accel, gyro + 6 bytes for mag slave */ imu_aspirin2.mpu.config.nb_bytes = 21; diff --git a/sw/airborne/modules/imu/imu_aspirin_i2c.c b/sw/airborne/modules/imu/imu_aspirin_i2c.c index 42ceea35af..bb5167c934 100644 --- a/sw/airborne/modules/imu/imu_aspirin_i2c.c +++ b/sw/airborne/modules/imu/imu_aspirin_i2c.c @@ -111,24 +111,15 @@ void imu_aspirin_i2c_init(void) // Default scaling values #ifdef IMU_ASPIRIN_VERSION_1_5 - const struct Int32Rates gyro_scale[2] = { - {4359, 4359, 4359}, - {1000, 1000, 1000} - }; + const struct FloatVect3 gyro_scale_f = { 4.359f, 4.359f, 4.359f }; #else - const struct Int32Rates gyro_scale[2] = { - {4973, 4973, 4973}, - {1000, 1000, 1000} - }; + const struct FloatVect3 gyro_scale_f = { 4.973f, 4.973f, 4.973f }; #endif - const struct Int32Vect3 accel_scale[2] = { - {3791, 3791, 3791}, - {100, 100, 100} - }; + const struct FloatVect3 accel_scale_f = { 37.91f, 37.91f, 37.91f }; // Set the default scaling - imu_set_defaults_gyro(IMU_ASPIRIN_ID, NULL, NULL, gyro_scale); - imu_set_defaults_accel(IMU_ASPIRIN_ID, NULL, NULL, accel_scale); + imu_set_defaults_gyro(IMU_ASPIRIN_ID, NULL, NULL, &gyro_scale_f); + imu_set_defaults_accel(IMU_ASPIRIN_ID, NULL, NULL, &accel_scale_f); /* initialize mag and set default options */ hmc58xx_init(&imu_aspirin.mag_hmc, &(ASPIRIN_I2C_DEV), HMC58XX_ADDR); diff --git a/sw/airborne/modules/imu/imu_bebop.c b/sw/airborne/modules/imu/imu_bebop.c index f8e748bb77..71a9c78fb8 100644 --- a/sw/airborne/modules/imu/imu_bebop.c +++ b/sw/airborne/modules/imu/imu_bebop.c @@ -83,8 +83,8 @@ void imu_bebop_init(void) imu_bebop.mpu.config.accel_range = BEBOP_ACCEL_RANGE; // Set the default scaling - imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[BEBOP_GYRO_RANGE]); - imu_set_defaults_accel(IMU_BOARD_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[BEBOP_ACCEL_RANGE]); + imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, &MPU60X0_GYRO_SENS_F[BEBOP_GYRO_RANGE]); + imu_set_defaults_accel(IMU_BOARD_ID, NULL, NULL, &MPU60X0_ACCEL_SENS_F[BEBOP_ACCEL_RANGE]); /* AKM8963 */ ak8963_init(&imu_bebop.ak, &(BEBOP_MAG_I2C_DEV), AK8963_ADDR); diff --git a/sw/airborne/modules/imu/imu_bmi088_i2c.c b/sw/airborne/modules/imu/imu_bmi088_i2c.c index d8d03d9f66..3deb9427dc 100644 --- a/sw/airborne/modules/imu/imu_bmi088_i2c.c +++ b/sw/airborne/modules/imu/imu_bmi088_i2c.c @@ -101,8 +101,8 @@ void imu_bmi088_init(void) imu_bmi088.bmi.config.accel_bw = IMU_BMI088_ACCEL_BW; // Set the default scaling - imu_set_defaults_gyro(IMU_BMI088_ID, NULL, NULL, BMI088_GYRO_SENS_FRAC[IMU_BMI088_GYRO_RANGE]); - imu_set_defaults_accel(IMU_BMI088_ID, NULL, NULL, BMI088_ACCEL_SENS_FRAC[IMU_BMI088_ACCEL_RANGE]); + imu_set_defaults_gyro(IMU_BMI088_ID, NULL, NULL, &BMI088_GYRO_SENS_F[IMU_BMI088_GYRO_RANGE]); + imu_set_defaults_accel(IMU_BMI088_ID, NULL, NULL, &BMI088_ACCEL_SENS_F[IMU_BMI088_ACCEL_RANGE]); } void imu_bmi088_periodic(void) diff --git a/sw/airborne/modules/imu/imu_cube.c b/sw/airborne/modules/imu/imu_cube.c index 22c8b8d257..f72b01328f 100644 --- a/sw/airborne/modules/imu/imu_cube.c +++ b/sw/airborne/modules/imu/imu_cube.c @@ -87,8 +87,8 @@ void imu_cube_init(void) eulers.theta = ANGLE_BFP_OF_REAL(0); eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(90)); int32_rmat_of_eulers(&rmat, &eulers); - imu_set_defaults_gyro(IMU_CUBE2_ID, &rmat, NULL, NULL); - imu_set_defaults_accel(IMU_CUBE2_ID, &rmat, NULL, NULL); + imu_set_defaults_gyro(IMU_CUBE2_ID, &rmat, NULL, NULL, NULL); + imu_set_defaults_accel(IMU_CUBE2_ID, &rmat, NULL, NULL, NULL); #else /* IMU 2 (ICM20602 isolated) */ mpu60x0_spi_init(&imu2, &CUBE_IMU2_SPI_DEV, CUBE_IMU2_SPI_SLAVE_IDX); @@ -104,8 +104,8 @@ void imu_cube_init(void) eulers.theta = ANGLE_BFP_OF_REAL(0); eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(270)); int32_rmat_of_eulers(&rmat, &eulers); - imu_set_defaults_gyro(IMU_CUBE2_ID, &rmat, NULL, MPU60X0_GYRO_SENS_FRAC[MPU60X0_GYRO_RANGE_2000]); - imu_set_defaults_accel(IMU_CUBE2_ID, &rmat, NULL, MPU60X0_ACCEL_SENS_FRAC[MPU60X0_ACCEL_RANGE_16G]); + imu_set_defaults_gyro(IMU_CUBE2_ID, &rmat, NULL, &MPU60X0_GYRO_SENS_F[MPU60X0_GYRO_RANGE_2000]); + imu_set_defaults_accel(IMU_CUBE2_ID, &rmat, NULL, &MPU60X0_ACCEL_SENS_F[MPU60X0_ACCEL_RANGE_16G]); #endif /* IMU 3 (ICM2094 isolated) */ diff --git a/sw/airborne/modules/imu/imu_disco.c b/sw/airborne/modules/imu/imu_disco.c index 9e356ede80..89b4125738 100644 --- a/sw/airborne/modules/imu/imu_disco.c +++ b/sw/airborne/modules/imu/imu_disco.c @@ -81,8 +81,8 @@ void imu_disco_init(void) imu_disco.mpu.config.accel_range = DISCO_ACCEL_RANGE; // Set the default scaling - imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[DISCO_GYRO_RANGE]); - imu_set_defaults_accel(IMU_BOARD_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[DISCO_ACCEL_RANGE]); + imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, &MPU60X0_GYRO_SENS_F[DISCO_GYRO_RANGE]); + imu_set_defaults_accel(IMU_BOARD_ID, NULL, NULL, &MPU60X0_ACCEL_SENS_F[DISCO_ACCEL_RANGE]); /* AKM8963 */ ak8963_init(&imu_disco.ak, &(DISCO_MAG_I2C_DEV), AK8963_ADDR); diff --git a/sw/airborne/modules/imu/imu_mpu6000.c b/sw/airborne/modules/imu/imu_mpu6000.c index 94493e8a81..97d98533ca 100644 --- a/sw/airborne/modules/imu/imu_mpu6000.c +++ b/sw/airborne/modules/imu/imu_mpu6000.c @@ -130,8 +130,8 @@ void imu_mpu_spi_init(void) imu_mpu_spi.mpu.config.accel_range = IMU_MPU_ACCEL_RANGE; // Set the default scaling - imu_set_defaults_gyro(IMU_MPU6000_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE]); - imu_set_defaults_accel(IMU_MPU6000_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE]); + imu_set_defaults_gyro(IMU_MPU6000_ID, NULL, NULL, &MPU60X0_GYRO_SENS_F[IMU_MPU_GYRO_RANGE]); + imu_set_defaults_accel(IMU_MPU6000_ID, NULL, NULL, &MPU60X0_ACCEL_SENS_F[IMU_MPU_ACCEL_RANGE]); } void imu_mpu_spi_periodic(void) diff --git a/sw/airborne/modules/imu/imu_mpu6000_hmc5883.c b/sw/airborne/modules/imu/imu_mpu6000_hmc5883.c index 17be9e8c80..fce922d843 100644 --- a/sw/airborne/modules/imu/imu_mpu6000_hmc5883.c +++ b/sw/airborne/modules/imu/imu_mpu6000_hmc5883.c @@ -131,8 +131,8 @@ void imu_mpu_hmc_init(void) imu_mpu_hmc.mpu.config.accel_range = IMU_MPU_ACCEL_RANGE; // Set the default scaling - imu_set_defaults_gyro(IMU_MPU6000_HMC_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[IMU_MPU_GYRO_RANGE]); - imu_set_defaults_accel(IMU_MPU6000_HMC_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[IMU_MPU_ACCEL_RANGE]); + imu_set_defaults_gyro(IMU_MPU6000_HMC_ID, NULL, NULL, &MPU60X0_GYRO_SENS_F[IMU_MPU_GYRO_RANGE]); + imu_set_defaults_accel(IMU_MPU6000_HMC_ID, NULL, NULL, &MPU60X0_ACCEL_SENS_F[IMU_MPU_ACCEL_RANGE]); /* initialize mag and set default options */ hmc58xx_init(&imu_mpu_hmc.hmc, &IMU_HMC_I2C_DEV, HMC58XX_ADDR); diff --git a/sw/airborne/modules/imu/imu_mpu60x0_i2c.c b/sw/airborne/modules/imu/imu_mpu60x0_i2c.c index cba9683190..3df6897646 100644 --- a/sw/airborne/modules/imu/imu_mpu60x0_i2c.c +++ b/sw/airborne/modules/imu/imu_mpu60x0_i2c.c @@ -74,8 +74,8 @@ void imu_mpu_i2c_init(void) imu_mpu_i2c.mpu.config.accel_range = IMU_MPU60X0_ACCEL_RANGE; // Set the default scaling - imu_set_defaults_gyro(IMU_MPU60X0_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[IMU_MPU60X0_GYRO_RANGE]); - imu_set_defaults_accel(IMU_MPU60X0_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[IMU_MPU60X0_ACCEL_RANGE]); + imu_set_defaults_gyro(IMU_MPU60X0_ID, NULL, NULL, &MPU60X0_GYRO_SENS_F[IMU_MPU60X0_GYRO_RANGE]); + imu_set_defaults_accel(IMU_MPU60X0_ID, NULL, NULL, &MPU60X0_ACCEL_SENS_F[IMU_MPU60X0_ACCEL_RANGE]); } void imu_mpu_i2c_periodic(void) diff --git a/sw/airborne/modules/imu/imu_mpu9250_i2c.c b/sw/airborne/modules/imu/imu_mpu9250_i2c.c index b6cb27021f..e3e1825f37 100644 --- a/sw/airborne/modules/imu/imu_mpu9250_i2c.c +++ b/sw/airborne/modules/imu/imu_mpu9250_i2c.c @@ -114,8 +114,8 @@ void imu_mpu9250_init(void) imu_mpu9250.mpu.config.accel_range = IMU_MPU9250_ACCEL_RANGE; // Set the default scaling - imu_set_defaults_gyro(IMU_MPU9250_ID, NULL, NULL, MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE]); - imu_set_defaults_accel(IMU_MPU9250_ID, NULL, NULL, MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE]); + imu_set_defaults_gyro(IMU_MPU9250_ID, NULL, NULL, &MPU9250_GYRO_SENS_F[IMU_MPU9250_GYRO_RANGE]); + imu_set_defaults_accel(IMU_MPU9250_ID, NULL, NULL, &MPU9250_ACCEL_SENS_F[IMU_MPU9250_ACCEL_RANGE]); } void imu_mpu9250_periodic(void) diff --git a/sw/airborne/modules/imu/imu_mpu9250_spi.c b/sw/airborne/modules/imu/imu_mpu9250_spi.c index e08bf92c00..6d75bde9fb 100644 --- a/sw/airborne/modules/imu/imu_mpu9250_spi.c +++ b/sw/airborne/modules/imu/imu_mpu9250_spi.c @@ -126,8 +126,8 @@ void imu_mpu9250_init(void) imu_mpu9250.mpu.config.accel_range = IMU_MPU9250_ACCEL_RANGE; // Set the default scaling - imu_set_defaults_gyro(IMU_MPU9250_ID, NULL, NULL, MPU9250_GYRO_SENS_FRAC[IMU_MPU9250_GYRO_RANGE]); - imu_set_defaults_accel(IMU_MPU9250_ID, NULL, NULL, MPU9250_ACCEL_SENS_FRAC[IMU_MPU9250_ACCEL_RANGE]); + imu_set_defaults_gyro(IMU_MPU9250_ID, NULL, NULL, &MPU9250_GYRO_SENS_F[IMU_MPU9250_GYRO_RANGE]); + imu_set_defaults_accel(IMU_MPU9250_ID, NULL, NULL, &MPU9250_ACCEL_SENS_F[IMU_MPU9250_ACCEL_RANGE]); /* "internal" ak8963 magnetometer as I2C slave */ #if IMU_MPU9250_READ_MAG diff --git a/sw/airborne/modules/imu/imu_nps.c b/sw/airborne/modules/imu/imu_nps.c index c7214f5afd..cd2a1408da 100644 --- a/sw/airborne/modules/imu/imu_nps.c +++ b/sw/airborne/modules/imu/imu_nps.c @@ -35,30 +35,15 @@ void imu_nps_init(void) imu_nps.accel_available = false; // Set the default scaling - const struct Int32Rates gyro_scale[2] = { - {NPS_GYRO_SENSITIVITY_PP_NUM, NPS_GYRO_SENSITIVITY_QQ_NUM, NPS_GYRO_SENSITIVITY_RR_NUM}, - {NPS_GYRO_SENSITIVITY_PP_DEN, NPS_GYRO_SENSITIVITY_QQ_DEN, NPS_GYRO_SENSITIVITY_RR_DEN} - }; - const struct Int32Rates gyro_neutral = { - NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R - }; - const struct Int32Vect3 accel_scale[2] = { - {NPS_ACCEL_SENSITIVITY_XX_NUM, NPS_ACCEL_SENSITIVITY_YY_NUM, NPS_ACCEL_SENSITIVITY_ZZ_NUM}, - {NPS_ACCEL_SENSITIVITY_XX_DEN, NPS_ACCEL_SENSITIVITY_YY_DEN, NPS_ACCEL_SENSITIVITY_ZZ_DEN} - }; - const struct Int32Vect3 accel_neutral = { - NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z - }; - const struct Int32Vect3 mag_scale[2] = { - {NPS_MAG_SENSITIVITY_XX_NUM, NPS_MAG_SENSITIVITY_YY_NUM, NPS_MAG_SENSITIVITY_ZZ_NUM}, - {NPS_MAG_SENSITIVITY_XX_DEN, NPS_MAG_SENSITIVITY_YY_DEN, NPS_MAG_SENSITIVITY_ZZ_DEN} - }; - const struct Int32Vect3 mag_neutral = { - NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z - }; - imu_set_defaults_gyro(IMU_NPS_ID, NULL, &gyro_neutral, gyro_scale); - imu_set_defaults_accel(IMU_NPS_ID, NULL, &accel_neutral, accel_scale); - imu_set_defaults_mag(IMU_NPS_ID, NULL, &mag_neutral, mag_scale); + const struct FloatVect3 gyro_scale_f = {NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR}; + const struct Int32Rates gyro_neutral = {NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R}; + const struct FloatVect3 accel_scale_f = {NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, NPS_ACCEL_SENSITIVITY_ZZ}; + const struct Int32Vect3 accel_neutral = {NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z}; + const struct FloatVect3 mag_scale_f = {NPS_MAG_SENSITIVITY_XX, NPS_MAG_SENSITIVITY_YY, NPS_MAG_SENSITIVITY_ZZ}; + const struct Int32Vect3 mag_neutral = {NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z}; + imu_set_defaults_gyro(IMU_NPS_ID, NULL, &gyro_neutral, &gyro_scale_f); + imu_set_defaults_accel(IMU_NPS_ID, NULL, &accel_neutral, &accel_scale_f); + imu_set_defaults_mag(IMU_NPS_ID, NULL, &mag_neutral, &mag_scale_f); } diff --git a/sw/airborne/modules/imu/imu_px4fmu.c b/sw/airborne/modules/imu/imu_px4fmu.c index 5488274d7e..dde26d0ca0 100644 --- a/sw/airborne/modules/imu/imu_px4fmu.c +++ b/sw/airborne/modules/imu/imu_px4fmu.c @@ -71,8 +71,8 @@ void imu_px4fmu_init(void) imu_px4fmu.mpu.config.accel_range = PX4FMU_ACCEL_RANGE; // Set the default scaling - imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, MPU60X0_GYRO_SENS_FRAC[PX4FMU_GYRO_RANGE]); - imu_set_defaults_accel(IMU_BOARD_ID, NULL, NULL, MPU60X0_ACCEL_SENS_FRAC[PX4FMU_ACCEL_RANGE]); + imu_set_defaults_gyro(IMU_BOARD_ID, NULL, NULL, &MPU60X0_GYRO_SENS_F[PX4FMU_GYRO_RANGE]); + imu_set_defaults_accel(IMU_BOARD_ID, NULL, NULL, &MPU60X0_ACCEL_SENS_F[PX4FMU_ACCEL_RANGE]); /* initialize mag on i2c2 and set default options */ hmc58xx_init(&imu_px4fmu.hmc, &i2c2, HMC58XX_ADDR); diff --git a/sw/airborne/modules/imu/imu_px4fmu_v2.4.c b/sw/airborne/modules/imu/imu_px4fmu_v2.4.c index 3e168250d1..ca6ac90244 100644 --- a/sw/airborne/modules/imu/imu_px4fmu_v2.4.c +++ b/sw/airborne/modules/imu/imu_px4fmu_v2.4.c @@ -47,16 +47,10 @@ void imu_px4_init(void) { lsm303d_spi_init(&imu_px4.lsm_acc, &IMU_PX4FMU_SPI_DEV, IMU_LSM_SPI_SLAVE_IDX, LSM303D_TARGET_ACC); // Set the default scaling - const struct Int32Rates gyro_scale[2] = { - {L3GD20_SENS_2000_NUM, L3GD20_SENS_2000_NUM, L3GD20_SENS_2000_NUM}, - {L3GD20_SENS_2000_DEN, L3GD20_SENS_2000_DEN, L3GD20_SENS_2000_DEN} - }; - const struct Int32Vect3 accel_scale[2] = { - {LSM303D_ACCEL_SENS_16G_NUM, LSM303D_ACCEL_SENS_16G_NUM, LSM303D_ACCEL_SENS_16G_NUM}, - {LSM303D_ACCEL_SENS_16G_DEN, LSM303D_ACCEL_SENS_16G_DEN, LSM303D_ACCEL_SENS_16G_DEN} - }; - imu_set_defaults_gyro(IMU_PX4_ID, NULL, NULL, gyro_scale); - imu_set_defaults_accel(IMU_PX4_ID, NULL, NULL, accel_scale); + const struct FloatVect3 gyro_scale_f = {L3GD20_SENS_2000, L3GD20_SENS_2000, L3GD20_SENS_2000}; + const struct FloatVect3 accel_scale_f = {LSM303D_ACCEL_SENS_16G, LSM303D_ACCEL_SENS_16G, LSM303D_ACCEL_SENS_16G}; + imu_set_defaults_gyro(IMU_PX4_ID, NULL, NULL, &gyro_scale_f); + imu_set_defaults_accel(IMU_PX4_ID, NULL, NULL, &accel_scale_f); #if !IMU_PX4_DISABLE_MAG /* LSM303d mag init */ diff --git a/sw/airborne/modules/sensors/sensors_hitl.c b/sw/airborne/modules/sensors/sensors_hitl.c index 6472023fc5..11f703e97b 100644 --- a/sw/airborne/modules/sensors/sensors_hitl.c +++ b/sw/airborne/modules/sensors/sensors_hitl.c @@ -60,30 +60,16 @@ void sensors_hitl_init(void) imu_hitl.accel_available = false; // Set the default scaling - const struct Int32Rates gyro_scale[2] = { - {NPS_GYRO_SENSITIVITY_PP_NUM, NPS_GYRO_SENSITIVITY_QQ_NUM, NPS_GYRO_SENSITIVITY_RR_NUM}, - {NPS_GYRO_SENSITIVITY_PP_DEN, NPS_GYRO_SENSITIVITY_QQ_DEN, NPS_GYRO_SENSITIVITY_RR_DEN} - }; - const struct Int32Rates gyro_neutral = { - NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R - }; - const struct Int32Vect3 accel_scale[2] = { - {NPS_ACCEL_SENSITIVITY_XX_NUM, NPS_ACCEL_SENSITIVITY_YY_NUM, NPS_ACCEL_SENSITIVITY_ZZ_NUM}, - {NPS_ACCEL_SENSITIVITY_XX_DEN, NPS_ACCEL_SENSITIVITY_YY_DEN, NPS_ACCEL_SENSITIVITY_ZZ_DEN} - }; - const struct Int32Vect3 accel_neutral = { - NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z - }; - const struct Int32Vect3 mag_scale[2] = { - {NPS_MAG_SENSITIVITY_XX_NUM, NPS_MAG_SENSITIVITY_YY_NUM, NPS_MAG_SENSITIVITY_ZZ_NUM}, - {NPS_MAG_SENSITIVITY_XX_DEN, NPS_MAG_SENSITIVITY_YY_DEN, NPS_MAG_SENSITIVITY_ZZ_DEN} - }; - const struct Int32Vect3 mag_neutral = { - NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z - }; - imu_set_defaults_gyro(IMU_NPS_ID, NULL, &gyro_neutral, gyro_scale); - imu_set_defaults_accel(IMU_NPS_ID, NULL, &accel_neutral, accel_scale); - imu_set_defaults_mag(IMU_NPS_ID, NULL, &mag_neutral, mag_scale); + const struct FloatVect3 gyro_scale_f = {NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR}; + const struct Int32Rates gyro_neutral = {NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R}; + const struct FloatVect3 accel_scale_f = {NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, NPS_ACCEL_SENSITIVITY_ZZ}; + const struct Int32Vect3 accel_neutral = {NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z}; + const struct FloatVect3 mag_scale_f = {NPS_MAG_SENSITIVITY_XX, NPS_MAG_SENSITIVITY_YY, NPS_MAG_SENSITIVITY_ZZ}; + const struct Int32Vect3 mag_neutral = {NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z}; + + imu_set_defaults_gyro(IMU_NPS_ID, NULL, &gyro_neutral, &gyro_scale_f); + imu_set_defaults_accel(IMU_NPS_ID, NULL, &accel_neutral, &accel_scale_f); + imu_set_defaults_mag(IMU_NPS_ID, NULL, &mag_neutral, &mag_scale_f); } void sensors_hitl_periodic(void) { diff --git a/sw/airborne/peripherals/bmi088.c b/sw/airborne/peripherals/bmi088.c index f428ed52c9..b6544d10e2 100644 --- a/sw/airborne/peripherals/bmi088.c +++ b/sw/airborne/peripherals/bmi088.c @@ -36,17 +36,12 @@ const float BMI088_GYRO_SENS[5] = { BMI088_GYRO_SENS_125, }; -const struct Int32Rates BMI088_GYRO_SENS_FRAC[5][2] = { - { {BMI088_GYRO_SENS_2000_NUM, BMI088_GYRO_SENS_2000_NUM, BMI088_GYRO_SENS_2000_NUM}, - {BMI088_GYRO_SENS_2000_DEN, BMI088_GYRO_SENS_2000_DEN, BMI088_GYRO_SENS_2000_DEN} }, - { {BMI088_GYRO_SENS_1000_NUM, BMI088_GYRO_SENS_1000_NUM, BMI088_GYRO_SENS_1000_NUM}, - {BMI088_GYRO_SENS_1000_DEN, BMI088_GYRO_SENS_1000_DEN, BMI088_GYRO_SENS_1000_DEN} }, - { {BMI088_GYRO_SENS_500_NUM, BMI088_GYRO_SENS_500_NUM, BMI088_GYRO_SENS_500_NUM}, - {BMI088_GYRO_SENS_500_DEN, BMI088_GYRO_SENS_500_DEN, BMI088_GYRO_SENS_500_DEN} }, - { {BMI088_GYRO_SENS_250_NUM, BMI088_GYRO_SENS_250_NUM, BMI088_GYRO_SENS_250_NUM}, - {BMI088_GYRO_SENS_250_DEN, BMI088_GYRO_SENS_250_DEN, BMI088_GYRO_SENS_250_DEN} }, - { {BMI088_GYRO_SENS_125_NUM, BMI088_GYRO_SENS_125_NUM, BMI088_GYRO_SENS_125_NUM}, - {BMI088_GYRO_SENS_125_DEN, BMI088_GYRO_SENS_125_DEN, BMI088_GYRO_SENS_125_DEN} } +const struct FloatVect3 BMI088_GYRO_SENS_F[5] = { + {BMI088_GYRO_SENS_2000, BMI088_GYRO_SENS_2000, BMI088_GYRO_SENS_2000}, + {BMI088_GYRO_SENS_1000, BMI088_GYRO_SENS_1000, BMI088_GYRO_SENS_1000}, + {BMI088_GYRO_SENS_500, BMI088_GYRO_SENS_500, BMI088_GYRO_SENS_500}, + {BMI088_GYRO_SENS_250, BMI088_GYRO_SENS_250, BMI088_GYRO_SENS_250}, + {BMI088_GYRO_SENS_125, BMI088_GYRO_SENS_125, BMI088_GYRO_SENS_125} }; const float BMI088_ACCEL_SENS[4] = { @@ -56,15 +51,11 @@ const float BMI088_ACCEL_SENS[4] = { BMI088_ACCEL_SENS_24G }; -const struct Int32Vect3 BMI088_ACCEL_SENS_FRAC[4][2] = { - { {BMI088_ACCEL_SENS_3G_NUM, BMI088_ACCEL_SENS_3G_NUM, BMI088_ACCEL_SENS_3G_NUM}, - {BMI088_ACCEL_SENS_3G_DEN, BMI088_ACCEL_SENS_3G_DEN, BMI088_ACCEL_SENS_3G_DEN} }, - { {BMI088_ACCEL_SENS_6G_NUM, BMI088_ACCEL_SENS_6G_NUM, BMI088_ACCEL_SENS_6G_NUM}, - {BMI088_ACCEL_SENS_6G_DEN, BMI088_ACCEL_SENS_6G_DEN, BMI088_ACCEL_SENS_6G_DEN} }, - { {BMI088_ACCEL_SENS_12G_NUM, BMI088_ACCEL_SENS_12G_NUM, BMI088_ACCEL_SENS_12G_NUM}, - {BMI088_ACCEL_SENS_12G_DEN, BMI088_ACCEL_SENS_12G_DEN, BMI088_ACCEL_SENS_12G_DEN} }, - { {BMI088_ACCEL_SENS_24G_NUM, BMI088_ACCEL_SENS_24G_NUM, BMI088_ACCEL_SENS_24G_NUM}, - {BMI088_ACCEL_SENS_24G_DEN, BMI088_ACCEL_SENS_24G_DEN, BMI088_ACCEL_SENS_24G_DEN} } +const struct FloatVect3 BMI088_ACCEL_SENS_F[4] = { + {BMI088_ACCEL_SENS_3G, BMI088_ACCEL_SENS_3G, BMI088_ACCEL_SENS_3G}, + {BMI088_ACCEL_SENS_6G, BMI088_ACCEL_SENS_6G, BMI088_ACCEL_SENS_6G}, + {BMI088_ACCEL_SENS_12G, BMI088_ACCEL_SENS_12G, BMI088_ACCEL_SENS_12G}, + {BMI088_ACCEL_SENS_24G, BMI088_ACCEL_SENS_24G, BMI088_ACCEL_SENS_24G} }; void bmi088_set_default_config(struct Bmi088Config *c) diff --git a/sw/airborne/peripherals/bmi088.h b/sw/airborne/peripherals/bmi088.h index fbb20eeac1..9425f18472 100644 --- a/sw/airborne/peripherals/bmi088.h +++ b/sw/airborne/peripherals/bmi088.h @@ -29,6 +29,7 @@ #include "std.h" #include "math/pprz_algebra_int.h" +#include "math/pprz_algebra_float.h" /* Include address and register definition */ #include "peripherals/bmi088_regs.h" @@ -68,7 +69,7 @@ // Get default sensitivity from a table extern const float BMI088_GYRO_SENS[5]; // Get default sensitivity numerator and denominator from a table -extern const struct Int32Rates BMI088_GYRO_SENS_FRAC[5][2]; +extern const struct FloatVect3 BMI088_GYRO_SENS_F[5]; /** default accel sensitivy from the datasheet * sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC @@ -92,7 +93,7 @@ extern const struct Int32Rates BMI088_GYRO_SENS_FRAC[5][2]; // Get default sensitivity from a table extern const float BMI088_ACCEL_SENS[4]; // Get default sensitivity numerator and denominator from a table -extern const struct Int32Vect3 BMI088_ACCEL_SENS_FRAC[4][2]; +extern const struct FloatVect3 BMI088_ACCEL_SENS_F[4]; enum Bmi088ConfStatus { BMI088_CONF_UNINIT, diff --git a/sw/airborne/peripherals/invensense2.c b/sw/airborne/peripherals/invensense2.c index 42e94e26f4..361b0a9795 100644 --- a/sw/airborne/peripherals/invensense2.c +++ b/sw/airborne/peripherals/invensense2.c @@ -44,31 +44,21 @@ static bool invensense2_select_bank(struct invensense2_t *inv, uint8_t bank); static bool invensense2_config(struct invensense2_t *inv); /* Default gyro scalings */ -static const struct Int32Rates invensense2_gyro_scale[5][2] = { - { {30267, 30267, 30267}, - {55463, 55463, 55463} }, // 250DPS - { {60534, 60534, 60534}, - {55463, 55463, 55463} }, // 500DPS - { {40147, 40147, 40147}, - {18420, 18420, 18420} }, // 1000DPS - { {40147, 40147, 40147}, - {9210, 9210, 9210} }, // 2000DPS - { {40147, 40147, 40147}, - {4605, 4605, 4605} } // 4000DPS +static const struct FloatVect3 invensense2_gyro_scale_f[5] = { + {0.545415, 0.545415, 0.545415}, // 250DPS: RATE_BFP_OF_REAL(radians(250)/(2**15)) + {1.09083, 1.09083, 1.09083}, // 500DPS + {2.18166, 2.18166, 2.18166}, // 1000DPS + {4.36332, 4.36332, 4.36332}, // 2000DPS + {8.72664, 8.72664, 8.72664}, // 4000DPS }; /* Default accel scalings */ -static const struct Int32Vect3 invensense2_accel_scale[5][2] = { - { {3189, 3189, 3189}, - {5203, 5203, 5203} }, // 2G - { {6378, 6378, 6378}, - {5203, 5203, 5203} }, // 4G - { {12756, 12756, 12756}, - {5203, 5203, 5203} }, // 8G - { {25512, 25512, 25512}, - {5203, 5203, 5203} }, // 16G - { {51024, 51024, 51024}, - {5203, 5203, 5203} } // 30G +static const struct FloatVect3 invensense2_accel_scale_f[5] = { + {0.61312, 0.61312, 0.61312}, // 2G: ACCEL_BFP_OF_REAL(2G*9.81/(2**15)) + {1.22583, 1.22583, 1.22583}, // 4G + {2.4525, 2.4525, 2.4525}, // 8G + {4.905, 4.905, 4.905}, // 16G + {9.196875, 9.196875, 9.196875}, // 30G }; /** @@ -339,8 +329,8 @@ static void invensense2_fix_config(struct invensense2_t *inv) { } /* Set the default values */ - imu_set_defaults_gyro(inv->abi_id, NULL, NULL, invensense2_gyro_scale[inv->gyro_range]); - imu_set_defaults_accel(inv->abi_id, NULL, NULL, invensense2_accel_scale[inv->accel_range]); + imu_set_defaults_gyro(inv->abi_id, NULL, NULL, &invensense2_gyro_scale_f[inv->gyro_range]); + imu_set_defaults_accel(inv->abi_id, NULL, NULL, &invensense2_accel_scale_f[inv->accel_range]); } /** diff --git a/sw/airborne/peripherals/invensense3.c b/sw/airborne/peripherals/invensense3.c index b54cf6b364..0a163facdf 100644 --- a/sw/airborne/peripherals/invensense3.c +++ b/sw/airborne/peripherals/invensense3.c @@ -30,6 +30,7 @@ #include "peripherals/invensense3_regs.h" #include "math/pprz_isa.h" #include "math/pprz_algebra_int.h" +#include "math/pprz_algebra_float.h" #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/gpio_arch.h" @@ -47,37 +48,24 @@ static bool invensense3_reset_fifo(struct invensense3_t *inv); static int samples_from_odr(int odr); /* Default gyro scalings */ -static const struct Int32Rates invensense3_gyro_scale[8][2] = { - { {40147, 40147, 40147}, - {9210, 9210, 9210} }, // 2000DPS - { {40147, 40147, 40147}, - {18420, 18420, 18420} }, // 1000DPS - { {60534, 60534, 60534}, - {55463, 55463, 55463} }, // 500DPS - { {30267, 30267, 30267}, - {55463, 55463, 55463} }, // 250DPS - { {30267, 30267, 30267}, - {110926, 110926, 110926} }, // 125DPS vvv (TODO: the new scales are not tested yet) vvv - { {3292054, 3292054, 3292054}, - {24144015, 24144015, 24144015} }, // 62.5DPS - { {1646027, 1646027, 1646027}, - {24144015, 24144015, 24144015} }, // 31.25DPS - { {1646027, 1646027, 1646027}, - {48288030, 48288030, 48288030} }, // 15.625DPS +static const struct FloatVect3 invensense3_gyro_scale_f[8] = { + {4.36332, 4.36332, 4.36332}, // 2000DPS + {2.18166, 2.18166, 2.18166}, // 1000DPS + {1.09083, 1.09083, 1.09083}, // 500DPS + {0.545415, 0.545415, 0.545415}, // 250DPS: RATE_BFP_OF_REAL(radians(250)/(2**15)) + {0.272707, 0.272707, 0.272707}, // 125DPS + {0.136353, 0.136353, 0.136353}, // 62.5DPS + {0.0681769, 0.0681769, 0.0681769}, // 31.25DPS + {0.0340884, 0.0340884, 0.0340884}, // 15.625DPS }; /* Default accel scalings */ -static const struct Int32Vect3 invensense3_accel_scale[5][2] = { - { {51024, 51024, 51024}, - {5203, 5203, 5203} }, // 32G - { {25512, 25512, 25512}, - {5203, 5203, 5203} }, // 16G - { {12756, 12756, 12756}, - {5203, 5203, 5203} }, // 8G - { {6378, 6378, 6378}, - {5203, 5203, 5203} }, // 4G - { {3189, 3189, 3189}, - {5203, 5203, 5203} } // 2G +static const struct FloatVect3 invensense3_accel_scale_f[5] = { + {9.81, 9.81, 9.81}, // 32G + {4.905, 4.905, 4.905}, // 16G + {2.4525, 2.4525, 2.4525}, // 8G + {1.22583, 1.22583, 1.22583}, // 4G + {0.61312, 0.61312, 0.61312}, // 2G: ACCEL_BFP_OF_REAL(2G*9.81/(2**15)) }; /* AAF settings (3dB Bandwidth [Hz], AAF_DELT, AAF_DELTSQR, AAF_BITSHIFT) */ @@ -711,8 +699,8 @@ static void invensense3_fix_config(struct invensense3_t *inv) { } /* Set the default values */ - imu_set_defaults_gyro(inv->abi_id, NULL, NULL, invensense3_gyro_scale[inv->gyro_range]); - imu_set_defaults_accel(inv->abi_id, NULL, NULL, invensense3_accel_scale[inv->accel_range]); + imu_set_defaults_gyro(inv->abi_id, NULL, NULL, &invensense3_gyro_scale_f[inv->gyro_range]); + imu_set_defaults_accel(inv->abi_id, NULL, NULL, &invensense3_accel_scale_f[inv->accel_range]); } /** diff --git a/sw/airborne/peripherals/l3gd20.h b/sw/airborne/peripherals/l3gd20.h index 98dfe6848f..31bd2ce629 100644 --- a/sw/airborne/peripherals/l3gd20.h +++ b/sw/airborne/peripherals/l3gd20.h @@ -37,8 +37,7 @@ * sens = 70e-3 * pi/180 * 2^INT32_RATE_FRAC * sens = (70e-3 / 180.0f) * pi * 4096 */ -#define L3GD20_SENS_2000_NUM 5004 -#define L3GD20_SENS_2000_DEN 1000 +#define L3GD20_SENS_2000 5.004 enum L3gd20ConfStatus { L3G_CONF_UNINIT = 0, diff --git a/sw/airborne/peripherals/lsm303d.h b/sw/airborne/peripherals/lsm303d.h index af25f1c1e6..7f2e759d52 100644 --- a/sw/airborne/peripherals/lsm303d.h +++ b/sw/airborne/peripherals/lsm303d.h @@ -58,8 +58,7 @@ * fixed point sens: 9.81 [m/s^2] / 732 [LSB/g] * 2^INT32_ACCEL_FRAC * sens = 9.81 / 732 * 1024 = 13.72 */ -#define LSM303D_ACCEL_SENS_16G_NUM 13723 -#define LSM303D_ACCEL_SENS_16G_DEN 1000 +#define LSM303D_ACCEL_SENS_16G 13.723 struct Lsm303dConfig { uint8_t acc_rate; ///< Data Output Rate (Hz) diff --git a/sw/airborne/peripherals/mpu60x0.c b/sw/airborne/peripherals/mpu60x0.c index 915829de37..7c2b186253 100644 --- a/sw/airborne/peripherals/mpu60x0.c +++ b/sw/airborne/peripherals/mpu60x0.c @@ -35,17 +35,14 @@ const float MPU60X0_GYRO_SENS[4] = { MPU60X0_GYRO_SENS_2000 }; -const struct Int32Rates MPU60X0_GYRO_SENS_FRAC[4][2] = { - { {MPU60X0_GYRO_SENS_250_NUM, MPU60X0_GYRO_SENS_250_NUM, MPU60X0_GYRO_SENS_250_NUM}, - {MPU60X0_GYRO_SENS_250_DEN, MPU60X0_GYRO_SENS_250_DEN, MPU60X0_GYRO_SENS_250_DEN} }, - { {MPU60X0_GYRO_SENS_500_NUM, MPU60X0_GYRO_SENS_500_NUM, MPU60X0_GYRO_SENS_500_NUM}, - {MPU60X0_GYRO_SENS_500_DEN, MPU60X0_GYRO_SENS_500_DEN, MPU60X0_GYRO_SENS_500_DEN} }, - { {MPU60X0_GYRO_SENS_1000_NUM, MPU60X0_GYRO_SENS_1000_NUM, MPU60X0_GYRO_SENS_1000_NUM}, - {MPU60X0_GYRO_SENS_1000_DEN, MPU60X0_GYRO_SENS_1000_DEN, MPU60X0_GYRO_SENS_1000_DEN} }, - { {MPU60X0_GYRO_SENS_2000_NUM, MPU60X0_GYRO_SENS_2000_NUM, MPU60X0_GYRO_SENS_2000_NUM}, - {MPU60X0_GYRO_SENS_2000_DEN, MPU60X0_GYRO_SENS_2000_DEN, MPU60X0_GYRO_SENS_2000_DEN} } +const struct FloatVect3 MPU60X0_GYRO_SENS_F[4] = { + {MPU60X0_GYRO_SENS_250, MPU60X0_GYRO_SENS_250, MPU60X0_GYRO_SENS_250}, + {MPU60X0_GYRO_SENS_500, MPU60X0_GYRO_SENS_500, MPU60X0_GYRO_SENS_500}, + {MPU60X0_GYRO_SENS_1000, MPU60X0_GYRO_SENS_1000, MPU60X0_GYRO_SENS_1000}, + {MPU60X0_GYRO_SENS_2000, MPU60X0_GYRO_SENS_2000, MPU60X0_GYRO_SENS_2000} }; + const float MPU60X0_ACCEL_SENS[4] = { MPU60X0_ACCEL_SENS_2G, MPU60X0_ACCEL_SENS_4G, @@ -53,15 +50,12 @@ const float MPU60X0_ACCEL_SENS[4] = { MPU60X0_ACCEL_SENS_16G }; -const struct Int32Vect3 MPU60X0_ACCEL_SENS_FRAC[4][2] = { - { {MPU60X0_ACCEL_SENS_2G_NUM, MPU60X0_ACCEL_SENS_2G_NUM, MPU60X0_ACCEL_SENS_2G_NUM}, - {MPU60X0_ACCEL_SENS_2G_DEN, MPU60X0_ACCEL_SENS_2G_DEN, MPU60X0_ACCEL_SENS_2G_DEN} }, - { {MPU60X0_ACCEL_SENS_4G_NUM, MPU60X0_ACCEL_SENS_4G_NUM, MPU60X0_ACCEL_SENS_4G_NUM}, - {MPU60X0_ACCEL_SENS_4G_DEN, MPU60X0_ACCEL_SENS_4G_DEN, MPU60X0_ACCEL_SENS_4G_DEN} }, - { {MPU60X0_ACCEL_SENS_8G_NUM, MPU60X0_ACCEL_SENS_8G_NUM, MPU60X0_ACCEL_SENS_8G_NUM}, - {MPU60X0_ACCEL_SENS_8G_DEN, MPU60X0_ACCEL_SENS_8G_DEN, MPU60X0_ACCEL_SENS_8G_DEN} }, - { {MPU60X0_ACCEL_SENS_16G_NUM, MPU60X0_ACCEL_SENS_16G_NUM, MPU60X0_ACCEL_SENS_16G_NUM}, - {MPU60X0_ACCEL_SENS_16G_DEN, MPU60X0_ACCEL_SENS_16G_DEN, MPU60X0_ACCEL_SENS_16G_DEN} } + +const struct FloatVect3 MPU60X0_ACCEL_SENS_F[4] = { + {MPU60X0_ACCEL_SENS_2G, MPU60X0_ACCEL_SENS_2G, MPU60X0_ACCEL_SENS_2G}, + {MPU60X0_ACCEL_SENS_4G, MPU60X0_ACCEL_SENS_4G, MPU60X0_ACCEL_SENS_4G}, + {MPU60X0_ACCEL_SENS_8G, MPU60X0_ACCEL_SENS_8G, MPU60X0_ACCEL_SENS_8G}, + {MPU60X0_ACCEL_SENS_16G, MPU60X0_ACCEL_SENS_16G, MPU60X0_ACCEL_SENS_16G} }; void mpu60x0_set_default_config(struct Mpu60x0Config *c) diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h index 15225b9bce..f76a2b692d 100644 --- a/sw/airborne/peripherals/mpu60x0.h +++ b/sw/airborne/peripherals/mpu60x0.h @@ -29,6 +29,7 @@ #include "std.h" #include "math/pprz_algebra_int.h" +#include "math/pprz_algebra_float.h" /* Include address and register definition */ #include "peripherals/mpu60x0_regs.h" @@ -59,22 +60,13 @@ * sens = 1/32.8 * pi/180 * 4096 = 2.17953 */ #define MPU60X0_GYRO_SENS_250 0.544883 -#define MPU60X0_GYRO_SENS_250_NUM 19327 -#define MPU60X0_GYRO_SENS_250_DEN 35470 #define MPU60X0_GYRO_SENS_500 1.08977 -#define MPU60X0_GYRO_SENS_500_NUM 57663 -#define MPU60X0_GYRO_SENS_500_DEN 52913 #define MPU60X0_GYRO_SENS_1000 2.17953 -#define MPU60X0_GYRO_SENS_1000_NUM 18271 -#define MPU60X0_GYRO_SENS_1000_DEN 8383 #define MPU60X0_GYRO_SENS_2000 4.35906 -#define MPU60X0_GYRO_SENS_2000_NUM 36542 -#define MPU60X0_GYRO_SENS_2000_DEN 8383 // Get default sensitivity from a table extern const float MPU60X0_GYRO_SENS[4]; -// Get default sensitivity numerator and denominator from a table -extern const struct Int32Rates MPU60X0_GYRO_SENS_FRAC[4][2]; +extern const struct FloatVect3 MPU60X0_GYRO_SENS_F[4]; /** default accel sensitivy from the datasheet * sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC @@ -82,22 +74,13 @@ extern const struct Int32Rates MPU60X0_GYRO_SENS_FRAC[4][2]; * sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525 */ #define MPU60X0_ACCEL_SENS_2G 0.613125 -#define MPU60X0_ACCEL_SENS_2G_NUM 981 -#define MPU60X0_ACCEL_SENS_2G_DEN 1600 #define MPU60X0_ACCEL_SENS_4G 1.22625 -#define MPU60X0_ACCEL_SENS_4G_NUM 981 -#define MPU60X0_ACCEL_SENS_4G_DEN 800 #define MPU60X0_ACCEL_SENS_8G 2.4525 -#define MPU60X0_ACCEL_SENS_8G_NUM 981 -#define MPU60X0_ACCEL_SENS_8G_DEN 400 #define MPU60X0_ACCEL_SENS_16G 4.905 -#define MPU60X0_ACCEL_SENS_16G_NUM 981 -#define MPU60X0_ACCEL_SENS_16G_DEN 200 // Get default sensitivity from a table extern const float MPU60X0_ACCEL_SENS[4]; -// Get default sensitivity numerator and denominator from a table -extern const struct Int32Vect3 MPU60X0_ACCEL_SENS_FRAC[4][2]; +extern const struct FloatVect3 MPU60X0_ACCEL_SENS_F[4]; /** MPU60x0 sensor type */ diff --git a/sw/airborne/peripherals/mpu9250.c b/sw/airborne/peripherals/mpu9250.c index 7177bb36c9..96aa740191 100644 --- a/sw/airborne/peripherals/mpu9250.c +++ b/sw/airborne/peripherals/mpu9250.c @@ -36,15 +36,11 @@ const float MPU9250_GYRO_SENS[4] = { MPU9250_GYRO_SENS_2000 }; -const struct Int32Rates MPU9250_GYRO_SENS_FRAC[4][2] = { - { {MPU9250_GYRO_SENS_250_NUM, MPU9250_GYRO_SENS_250_NUM, MPU9250_GYRO_SENS_250_NUM}, - {MPU9250_GYRO_SENS_250_DEN, MPU9250_GYRO_SENS_250_DEN, MPU9250_GYRO_SENS_250_DEN} }, - { {MPU9250_GYRO_SENS_500_NUM, MPU9250_GYRO_SENS_500_NUM, MPU9250_GYRO_SENS_500_NUM}, - {MPU9250_GYRO_SENS_500_DEN, MPU9250_GYRO_SENS_500_DEN, MPU9250_GYRO_SENS_500_DEN} }, - { {MPU9250_GYRO_SENS_1000_NUM, MPU9250_GYRO_SENS_1000_NUM, MPU9250_GYRO_SENS_1000_NUM}, - {MPU9250_GYRO_SENS_1000_DEN, MPU9250_GYRO_SENS_1000_DEN, MPU9250_GYRO_SENS_1000_DEN} }, - { {MPU9250_GYRO_SENS_2000_NUM, MPU9250_GYRO_SENS_2000_NUM, MPU9250_GYRO_SENS_2000_NUM}, - {MPU9250_GYRO_SENS_2000_DEN, MPU9250_GYRO_SENS_2000_DEN, MPU9250_GYRO_SENS_2000_DEN} } +const struct FloatVect3 MPU9250_GYRO_SENS_F[4] = { + {MPU9250_GYRO_SENS_250, MPU9250_GYRO_SENS_250, MPU9250_GYRO_SENS_250}, + {MPU9250_GYRO_SENS_500, MPU9250_GYRO_SENS_500, MPU9250_GYRO_SENS_500}, + {MPU9250_GYRO_SENS_1000, MPU9250_GYRO_SENS_1000, MPU9250_GYRO_SENS_1000}, + {MPU9250_GYRO_SENS_2000, MPU9250_GYRO_SENS_2000, MPU9250_GYRO_SENS_2000} }; const float MPU9250_ACCEL_SENS[4] = { @@ -54,15 +50,11 @@ const float MPU9250_ACCEL_SENS[4] = { MPU9250_ACCEL_SENS_16G }; -const struct Int32Vect3 MPU9250_ACCEL_SENS_FRAC[4][2] = { - { {MPU9250_ACCEL_SENS_2G_NUM, MPU9250_ACCEL_SENS_2G_NUM, MPU9250_ACCEL_SENS_2G_NUM}, - {MPU9250_ACCEL_SENS_2G_DEN, MPU9250_ACCEL_SENS_2G_DEN, MPU9250_ACCEL_SENS_2G_DEN} }, - { {MPU9250_ACCEL_SENS_4G_NUM, MPU9250_ACCEL_SENS_4G_NUM, MPU9250_ACCEL_SENS_4G_NUM}, - {MPU9250_ACCEL_SENS_4G_DEN, MPU9250_ACCEL_SENS_4G_DEN, MPU9250_ACCEL_SENS_4G_DEN} }, - { {MPU9250_ACCEL_SENS_8G_NUM, MPU9250_ACCEL_SENS_8G_NUM, MPU9250_ACCEL_SENS_8G_NUM}, - {MPU9250_ACCEL_SENS_8G_DEN, MPU9250_ACCEL_SENS_8G_DEN, MPU9250_ACCEL_SENS_8G_DEN} }, - { {MPU9250_ACCEL_SENS_16G_NUM, MPU9250_ACCEL_SENS_16G_NUM, MPU9250_ACCEL_SENS_16G_NUM}, - {MPU9250_ACCEL_SENS_16G_DEN, MPU9250_ACCEL_SENS_16G_DEN, MPU9250_ACCEL_SENS_16G_DEN} } +const struct FloatVect3 MPU9250_ACCEL_SENS_F[4] = { + {MPU9250_ACCEL_SENS_2G, MPU9250_ACCEL_SENS_2G, MPU9250_ACCEL_SENS_2G}, + {MPU9250_ACCEL_SENS_4G, MPU9250_ACCEL_SENS_4G, MPU9250_ACCEL_SENS_4G}, + {MPU9250_ACCEL_SENS_8G, MPU9250_ACCEL_SENS_8G, MPU9250_ACCEL_SENS_8G}, + {MPU9250_ACCEL_SENS_16G, MPU9250_ACCEL_SENS_16G, MPU9250_ACCEL_SENS_16G} }; void mpu9250_set_default_config(struct Mpu9250Config *c) diff --git a/sw/airborne/peripherals/mpu9250.h b/sw/airborne/peripherals/mpu9250.h index 18bcf8afaf..b1fc29bb19 100644 --- a/sw/airborne/peripherals/mpu9250.h +++ b/sw/airborne/peripherals/mpu9250.h @@ -29,6 +29,7 @@ #include "std.h" #include "math/pprz_algebra_int.h" +#include "math/pprz_algebra_float.h" /* Include address and register definition */ #include "peripherals/mpu9250_regs.h" @@ -59,22 +60,14 @@ * sens = 1/32.8 * pi/180 * 4096 = 2.17953 */ #define MPU9250_GYRO_SENS_250 0.544883 -#define MPU9250_GYRO_SENS_250_NUM 19327 -#define MPU9250_GYRO_SENS_250_DEN 35470 #define MPU9250_GYRO_SENS_500 1.08977 -#define MPU9250_GYRO_SENS_500_NUM 57663 -#define MPU9250_GYRO_SENS_500_DEN 52913 #define MPU9250_GYRO_SENS_1000 2.17953 -#define MPU9250_GYRO_SENS_1000_NUM 18271 -#define MPU9250_GYRO_SENS_1000_DEN 8383 #define MPU9250_GYRO_SENS_2000 4.35906 -#define MPU9250_GYRO_SENS_2000_NUM 36542 -#define MPU9250_GYRO_SENS_2000_DEN 8383 // Get default sensitivity from a table extern const float MPU9250_GYRO_SENS[4]; // Get default sensitivity numerator and denominator from a table -extern const struct Int32Rates MPU9250_GYRO_SENS_FRAC[4][2]; +extern const struct FloatVect3 MPU9250_GYRO_SENS_F[4]; /** default accel sensitivy from the datasheet * sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC @@ -82,22 +75,14 @@ extern const struct Int32Rates MPU9250_GYRO_SENS_FRAC[4][2]; * sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525 */ #define MPU9250_ACCEL_SENS_2G 0.613125 -#define MPU9250_ACCEL_SENS_2G_NUM 981 -#define MPU9250_ACCEL_SENS_2G_DEN 1600 #define MPU9250_ACCEL_SENS_4G 1.22625 -#define MPU9250_ACCEL_SENS_4G_NUM 981 -#define MPU9250_ACCEL_SENS_4G_DEN 800 #define MPU9250_ACCEL_SENS_8G 2.4525 -#define MPU9250_ACCEL_SENS_8G_NUM 981 -#define MPU9250_ACCEL_SENS_8G_DEN 400 #define MPU9250_ACCEL_SENS_16G 4.905 -#define MPU9250_ACCEL_SENS_16G_NUM 981 -#define MPU9250_ACCEL_SENS_16G_DEN 200 // Get default sensitivity from a table extern const float MPU9250_ACCEL_SENS[4]; // Get default sensitivity numerator and denominator from a table -extern const struct Int32Vect3 MPU9250_ACCEL_SENS_FRAC[4][2]; +extern const struct FloatVect3 MPU9250_ACCEL_SENS_F[4]; enum Mpu9250ConfStatus { MPU9250_CONF_UNINIT, diff --git a/sw/tools/calibration/calibration_utils.py b/sw/tools/calibration/calibration_utils.py index da17215258..696a367e76 100644 --- a/sw/tools/calibration/calibration_utils.py +++ b/sw/tools/calibration/calibration_utils.py @@ -170,34 +170,11 @@ def estimate_mag_current_relation(meas): offset.append(intercept) return coefficient, offset -def continious_frac(v): - max_val = 2**16 - if v > 0: - s = 1 - else: - v = -v - s = -1 - return _continious_frac(v, max_val, int(v), v, (1, int(v)), (0,1), s) - -def _continious_frac(v, max_val, a, x, num, den, s): - x1 = 1 / (x - a) - a1 = int(x1) - (num1, num2) = num - num3 = a1 * num2 + num1 - (den1, den2) = den - den3 = a1 * den2 + den1 - if num3 > max_val or den3 > max_val: - return (num2, s*den2) - elif (num3 / den3) == v: - return (num3, s*den3) - else: - return _continious_frac(v, max_val, a1, x1, (num2, num3), (den2, den3), s) - def format_xml(p, sensor, sensor_id, res): """Print xml for airframe file.""" - x_sens = continious_frac(p[3]*2**res) - y_sens = continious_frac(p[4]*2**res) - z_sens = continious_frac(p[5]*2**res) + x_sens = p[3]*2**res + y_sens = p[4]*2**res + z_sens = p[5]*2**res s = f"" s += f'\n' @@ -205,19 +182,12 @@ def format_xml(p, sensor, sensor_id, res): s += f' \n' s += f' \n' s += f' \n' - s += f' \n' + s += f' \n' s += f' \n' s += f' \n' - s += f' \n' + s += f' \n' s += f' \n' s += f'\n' - s += f'\n' - s += f'\n' - s += f'\n' - s += f'\n' - s += f'\n' - s += f'\n' - s += f'\n' return s def print_xml(p, sensor, sensor_id, res):