diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index fec8f6da50..64a84877b1 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -89,7 +89,7 @@ static pthread_cond_t navdata_cond = PTHREAD_COND_INITIALIZER; /** * Default gyro scale */ -static const struct FloatVect3 gyro_scale_f = { 4.359f, 4.359f, 4.359f }; +static const struct FloatRates gyro_scale_f = { 4.359f, 4.359f, 4.359f }; /** * Default accel scale/neutral diff --git a/sw/airborne/modules/imu/imu.c b/sw/airborne/modules/imu/imu.c index 83bbb61090..8fe347fe7b 100644 --- a/sw/airborne/modules/imu/imu.c +++ b/sw/airborne/modules/imu/imu.c @@ -470,9 +470,9 @@ void imu_init(void) 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].scale_f.p = (float)imu.gyros[i].scale[0].p / (float)imu.gyros[i].scale[1].p; + imu.gyros[i].scale_f.q = (float)imu.gyros[i].scale[0].q / (float)imu.gyros[i].scale[1].q; + imu.gyros[i].scale_f.r = (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); @@ -609,7 +609,7 @@ void imu_init(void) * @param neutral Neutral values * @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 FloatVect3 *scale_f) +void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct FloatRates *scale_f) { // Find the correct gyro struct imu_gyro_t *gyro = imu_get_gyro(abi_id, true); @@ -626,7 +626,7 @@ 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_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); + RATES_ASSIGN(gyro->scale_f, IMU_GYRO_P_SIGN*scale_f->p, IMU_GYRO_Q_SIGN*scale_f->q, IMU_GYRO_R_SIGN*scale_f->r); } } @@ -655,7 +655,7 @@ 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_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); + VECT3_ASSIGN(accel->scale_f, IMU_ACCEL_X_SIGN*scale_f->x, IMU_ACCEL_Y_SIGN*scale_f->y, IMU_ACCEL_Z_SIGN*scale_f->z); } } @@ -711,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_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; + scaled.p = (gyro->unscaled.p - gyro->neutral.p) * gyro->scale_f.p; + scaled.q = (gyro->unscaled.q - gyro->neutral.q) * gyro->scale_f.q; + scaled.r = (gyro->unscaled.r - gyro->neutral.r) * gyro->scale_f.r; // Rotate the sensor int32_rmat_transp_ratemult(&scaled_rot, &gyro->body_to_sensor, &scaled); @@ -739,9 +739,9 @@ 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_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); + f_sample.p = RATE_FLOAT_OF_BFP((data[i].p - gyro->neutral.p) * gyro->scale_f.p); + f_sample.q = RATE_FLOAT_OF_BFP((data[i].q - gyro->neutral.q) * gyro->scale_f.q); + f_sample.r = RATE_FLOAT_OF_BFP((data[i].r - gyro->neutral.r) * gyro->scale_f.r); #if IMU_LOG_HIGHSPEED diff --git a/sw/airborne/modules/imu/imu.h b/sw/airborne/modules/imu/imu.h index 00510d406d..efbdb45d9e 100644 --- a/sw/airborne/modules/imu/imu.h +++ b/sw/airborne/modules/imu/imu.h @@ -56,7 +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 FloatRates 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) @@ -115,7 +115,7 @@ 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 FloatVect3 *scale_f); +extern void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct FloatRates *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); diff --git a/sw/airborne/modules/imu/imu_aspirin.c b/sw/airborne/modules/imu/imu_aspirin.c index a2896a56fd..ce5a32ca37 100644 --- a/sw/airborne/modules/imu/imu_aspirin.c +++ b/sw/airborne/modules/imu/imu_aspirin.c @@ -90,9 +90,9 @@ void imu_aspirin_init(void) // Default scaling values #ifdef IMU_ASPIRIN_VERSION_1_5 - const struct FloatVect3 gyro_scale_f = { 4.359f, 4.359f, 4.359f }; + const struct FloatRates gyro_scale_f = { 4.359f, 4.359f, 4.359f }; #else - const struct FloatVect3 gyro_scale_f = { 4.973f, 4.973f, 4.973f }; + const struct FloatRates gyro_scale_f = { 4.973f, 4.973f, 4.973f }; #endif const struct FloatVect3 accel_scale_f = { 37.91f, 37.91f, 37.91f }; diff --git a/sw/airborne/modules/imu/imu_aspirin_i2c.c b/sw/airborne/modules/imu/imu_aspirin_i2c.c index bb5167c934..760aae00ca 100644 --- a/sw/airborne/modules/imu/imu_aspirin_i2c.c +++ b/sw/airborne/modules/imu/imu_aspirin_i2c.c @@ -111,9 +111,9 @@ void imu_aspirin_i2c_init(void) // Default scaling values #ifdef IMU_ASPIRIN_VERSION_1_5 - const struct FloatVect3 gyro_scale_f = { 4.359f, 4.359f, 4.359f }; + const struct FloatRates gyro_scale_f = { 4.359f, 4.359f, 4.359f }; #else - const struct FloatVect3 gyro_scale_f = { 4.973f, 4.973f, 4.973f }; + const struct FloatRates gyro_scale_f = { 4.973f, 4.973f, 4.973f }; #endif const struct FloatVect3 accel_scale_f = { 37.91f, 37.91f, 37.91f }; diff --git a/sw/airborne/modules/imu/imu_cube.c b/sw/airborne/modules/imu/imu_cube.c index f72b01328f..91ae66e96b 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, NULL); - imu_set_defaults_accel(IMU_CUBE2_ID, &rmat, NULL, NULL, NULL); + imu_set_defaults_gyro(IMU_CUBE2_ID, &rmat, NULL, NULL); + imu_set_defaults_accel(IMU_CUBE2_ID, &rmat, NULL, NULL); #else /* IMU 2 (ICM20602 isolated) */ mpu60x0_spi_init(&imu2, &CUBE_IMU2_SPI_DEV, CUBE_IMU2_SPI_SLAVE_IDX); diff --git a/sw/airborne/modules/imu/imu_nps.c b/sw/airborne/modules/imu/imu_nps.c index cd2a1408da..cb95d4997d 100644 --- a/sw/airborne/modules/imu/imu_nps.c +++ b/sw/airborne/modules/imu/imu_nps.c @@ -34,12 +34,12 @@ void imu_nps_init(void) imu_nps.mag_available = false; imu_nps.accel_available = false; - // Set the default scaling - const struct FloatVect3 gyro_scale_f = {NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR}; +// Set the default scaling + const struct FloatRates gyro_scale_f = {RATE_FLOAT_OF_BFP(NPS_GYRO_SENSITIVITY_PP), RATE_FLOAT_OF_BFP(NPS_GYRO_SENSITIVITY_QQ), RATE_FLOAT_OF_BFP(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 FloatVect3 accel_scale_f = {ACCEL_FLOAT_OF_BFP(NPS_ACCEL_SENSITIVITY_XX), ACCEL_FLOAT_OF_BFP(NPS_ACCEL_SENSITIVITY_YY), ACCEL_FLOAT_OF_BFP(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 FloatVect3 mag_scale_f = {MAG_FLOAT_OF_BFP(NPS_MAG_SENSITIVITY_XX), MAG_FLOAT_OF_BFP(NPS_MAG_SENSITIVITY_YY), MAG_FLOAT_OF_BFP(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); diff --git a/sw/airborne/modules/imu/imu_px4fmu_v2.4.c b/sw/airborne/modules/imu/imu_px4fmu_v2.4.c index ca6ac90244..108dd337d0 100644 --- a/sw/airborne/modules/imu/imu_px4fmu_v2.4.c +++ b/sw/airborne/modules/imu/imu_px4fmu_v2.4.c @@ -47,7 +47,7 @@ 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 FloatVect3 gyro_scale_f = {L3GD20_SENS_2000, L3GD20_SENS_2000, L3GD20_SENS_2000}; + const struct FloatRates 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); diff --git a/sw/airborne/modules/sensors/sensors_hitl.c b/sw/airborne/modules/sensors/sensors_hitl.c index 11f703e97b..0c571def30 100644 --- a/sw/airborne/modules/sensors/sensors_hitl.c +++ b/sw/airborne/modules/sensors/sensors_hitl.c @@ -59,12 +59,12 @@ void sensors_hitl_init(void) imu_hitl.mag_available = false; imu_hitl.accel_available = false; - // Set the default scaling - const struct FloatVect3 gyro_scale_f = {NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR}; +// Set the default scaling + const struct FloatRates gyro_scale_f = {RATE_FLOAT_OF_BFP(NPS_GYRO_SENSITIVITY_PP), RATE_FLOAT_OF_BFP(NPS_GYRO_SENSITIVITY_QQ), RATE_FLOAT_OF_BFP(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 FloatVect3 accel_scale_f = {ACCEL_FLOAT_OF_BFP(NPS_ACCEL_SENSITIVITY_XX), ACCEL_FLOAT_OF_BFP(NPS_ACCEL_SENSITIVITY_YY), ACCEL_FLOAT_OF_BFP(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 FloatVect3 mag_scale_f = {MAG_FLOAT_OF_BFP(NPS_MAG_SENSITIVITY_XX), MAG_FLOAT_OF_BFP(NPS_MAG_SENSITIVITY_YY), MAG_FLOAT_OF_BFP(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); diff --git a/sw/airborne/peripherals/bmi088.c b/sw/airborne/peripherals/bmi088.c index b6544d10e2..ab376ad763 100644 --- a/sw/airborne/peripherals/bmi088.c +++ b/sw/airborne/peripherals/bmi088.c @@ -36,7 +36,7 @@ const float BMI088_GYRO_SENS[5] = { BMI088_GYRO_SENS_125, }; -const struct FloatVect3 BMI088_GYRO_SENS_F[5] = { +const struct FloatRates 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}, diff --git a/sw/airborne/peripherals/bmi088.h b/sw/airborne/peripherals/bmi088.h index 9425f18472..b9e1726d63 100644 --- a/sw/airborne/peripherals/bmi088.h +++ b/sw/airborne/peripherals/bmi088.h @@ -69,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 FloatVect3 BMI088_GYRO_SENS_F[5]; +extern const struct FloatRates BMI088_GYRO_SENS_F[5]; /** default accel sensitivy from the datasheet * sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC diff --git a/sw/airborne/peripherals/invensense2.c b/sw/airborne/peripherals/invensense2.c index 361b0a9795..32d1002933 100644 --- a/sw/airborne/peripherals/invensense2.c +++ b/sw/airborne/peripherals/invensense2.c @@ -44,7 +44,7 @@ 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 FloatVect3 invensense2_gyro_scale_f[5] = { +static const struct FloatRates 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 diff --git a/sw/airborne/peripherals/invensense3.c b/sw/airborne/peripherals/invensense3.c index 0a163facdf..9060e3a3d6 100644 --- a/sw/airborne/peripherals/invensense3.c +++ b/sw/airborne/peripherals/invensense3.c @@ -48,7 +48,7 @@ static bool invensense3_reset_fifo(struct invensense3_t *inv); static int samples_from_odr(int odr); /* Default gyro scalings */ -static const struct FloatVect3 invensense3_gyro_scale_f[8] = { +static const struct FloatRates 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 diff --git a/sw/airborne/peripherals/mpu60x0.c b/sw/airborne/peripherals/mpu60x0.c index 7c2b186253..22a162f07b 100644 --- a/sw/airborne/peripherals/mpu60x0.c +++ b/sw/airborne/peripherals/mpu60x0.c @@ -35,7 +35,7 @@ const float MPU60X0_GYRO_SENS[4] = { MPU60X0_GYRO_SENS_2000 }; -const struct FloatVect3 MPU60X0_GYRO_SENS_F[4] = { +const struct FloatRates 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}, diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h index f76a2b692d..09c157d1ee 100644 --- a/sw/airborne/peripherals/mpu60x0.h +++ b/sw/airborne/peripherals/mpu60x0.h @@ -66,7 +66,7 @@ // Get default sensitivity from a table extern const float MPU60X0_GYRO_SENS[4]; -extern const struct FloatVect3 MPU60X0_GYRO_SENS_F[4]; +extern const struct FloatRates MPU60X0_GYRO_SENS_F[4]; /** default accel sensitivy from the datasheet * sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC diff --git a/sw/airborne/peripherals/mpu9250.c b/sw/airborne/peripherals/mpu9250.c index 96aa740191..0767d19887 100644 --- a/sw/airborne/peripherals/mpu9250.c +++ b/sw/airborne/peripherals/mpu9250.c @@ -36,7 +36,7 @@ const float MPU9250_GYRO_SENS[4] = { MPU9250_GYRO_SENS_2000 }; -const struct FloatVect3 MPU9250_GYRO_SENS_F[4] = { +const struct FloatRates 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}, diff --git a/sw/airborne/peripherals/mpu9250.h b/sw/airborne/peripherals/mpu9250.h index b1fc29bb19..eda7a52290 100644 --- a/sw/airborne/peripherals/mpu9250.h +++ b/sw/airborne/peripherals/mpu9250.h @@ -67,7 +67,7 @@ // 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 FloatVect3 MPU9250_GYRO_SENS_F[4]; +extern const struct FloatRates MPU9250_GYRO_SENS_F[4]; /** default accel sensitivy from the datasheet * sens = 9.81 [m/s^2] / [LSB/g] * 2^INT32_ACCEL_FRAC