Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-24 13:55:51 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[imu] fix some bugs introduced with imu float refactor (#3495)

Browse Source 
* imu cube fix too many inputs for set defaults

* fix signs, replace FloatVect3 by FloatRates for gyros

* fix nps
This commit is contained in:
NoahWe
2025-07-02 13:45:50 +02:00
committed by GitHub
parent 8a226b2ce3
commit b4ed21ae1e
17 changed files with 38 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/boards/ardrone/navdata.c
+1 -1
View File
@@ -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
sw/airborne/modules/imu/imu.c
+12 -12
View File
@@ -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
sw/airborne/modules/imu/imu.h
+2 -2
View File
@@ -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);
sw/airborne/modules/imu/imu_aspirin.c
+2 -2
View File
@@ -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 };
sw/airborne/modules/imu/imu_aspirin_i2c.c
+2 -2
View File
@@ -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 };
sw/airborne/modules/imu/imu_cube.c
+2 -2
View File
@@ -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);
sw/airborne/modules/imu/imu_nps.c
+4 -4
View File
@@ -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);
sw/airborne/modules/imu/imu_px4fmu_v2.4.c
+1 -1
View File
@@ -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);
sw/airborne/modules/sensors/sensors_hitl.c
+4 -4
View File
@@ -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);
sw/airborne/peripherals/bmi088.c
+1 -1
View File
@@ -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},
sw/airborne/peripherals/bmi088.h
+1 -1
View File
@@ -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
sw/airborne/peripherals/invensense2.c
+1 -1
View File
@@ -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
sw/airborne/peripherals/invensense3.c
+1 -1
View File
@@ -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
sw/airborne/peripherals/mpu60x0.c
+1 -1
View File
@@ -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},
sw/airborne/peripherals/mpu60x0.h
+1 -1
View File
@@ -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
sw/airborne/peripherals/mpu9250.c
+1 -1
View File
@@ -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},
sw/airborne/peripherals/mpu9250.h
+1 -1
View File
@@ -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