Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Imu scale float (#3477)

Browse Source 
* [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 <gautier.hattenberger@enac.fr>

* clarify scales

---------

Co-authored-by: Fabien-B <Fabien-B@github.com>
Co-authored-by: Gautier Hattenberger <gautier.hattenberger@enac.fr>
This commit is contained in:
Fabien-B
2025-06-25 13:27:53 +02:00
committed by GitHub
parent 993198a418
commit 4df4c8436b
32 changed files with 233 additions and 401 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/simulator/nps/nps_sensors_params_common.h
-36
View File
@@ -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
sw/airborne/boards/apogee/imu_apogee.c
+2 -2
View File
@@ -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
sw/airborne/boards/ardrone/navdata.c
+7 -15
View File
@@ -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);
sw/airborne/modules/imu/imu.c
+79 -46
View File
@@ -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);
sw/airborne/modules/imu/imu.h
+7 -3
View File
@@ -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);
sw/airborne/modules/imu/imu_aspirin.c
+5 -14
View File
@@ -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);
sw/airborne/modules/imu/imu_aspirin_2_spi.c
+2 -2
View File
@@ -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;
sw/airborne/modules/imu/imu_aspirin_i2c.c
+5 -14
View File
@@ -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);
sw/airborne/modules/imu/imu_bebop.c
+2 -2
View File
@@ -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);
sw/airborne/modules/imu/imu_bmi088_i2c.c
+2 -2
View File
@@ -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)
sw/airborne/modules/imu/imu_cube.c
+4 -4
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);
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) */
sw/airborne/modules/imu/imu_disco.c
+2 -2
View File
@@ -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);
sw/airborne/modules/imu/imu_mpu6000.c
+2 -2
View File
@@ -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)
sw/airborne/modules/imu/imu_mpu6000_hmc5883.c
+2 -2
View File
@@ -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);
sw/airborne/modules/imu/imu_mpu60x0_i2c.c
+2 -2
View File
@@ -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)
sw/airborne/modules/imu/imu_mpu9250_i2c.c
+2 -2
View File
@@ -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)
sw/airborne/modules/imu/imu_mpu9250_spi.c
+2 -2
View File
@@ -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
sw/airborne/modules/imu/imu_nps.c
+9 -24
View File
@@ -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);
}
sw/airborne/modules/imu/imu_px4fmu.c
+2 -2
View File
@@ -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);
sw/airborne/modules/imu/imu_px4fmu_v2.4.c
+4 -10
View File
@@ -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 */
sw/airborne/modules/sensors/sensors_hitl.c
+10 -24
View File
@@ -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) {
sw/airborne/peripherals/bmi088.c
+11 -20
View File
@@ -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)
sw/airborne/peripherals/bmi088.h
+3 -2
View File
@@ -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,
sw/airborne/peripherals/invensense2.c
+14 -24
View File
@@ -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]);
}
/**
sw/airborne/peripherals/invensense3.c
+18 -30
View File
@@ -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]);
}
/**
sw/airborne/peripherals/l3gd20.h
+1 -2
View File
@@ -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,
sw/airborne/peripherals/lsm303d.h
+1 -2
View File
@@ -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)
sw/airborne/peripherals/mpu60x0.c
+12 -18
View File
@@ -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)
sw/airborne/peripherals/mpu60x0.h
+3 -20
View File
@@ -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
*/
sw/airborne/peripherals/mpu9250.c
+10 -18
View File
@@ -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)
sw/airborne/peripherals/mpu9250.h
+3 -18
View File
@@ -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,
sw/tools/calibration/calibration_utils.py
+5 -35
View File
@@ -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'<define name="IMU_{sensor}_CALIB" type="array">\n'
@@ -205,19 +182,12 @@ def format_xml(p, sensor, sensor_id, res):
s += f' <field name="abi_id" value="{sensor_id}"/>\n'
s += f' <field name="calibrated" type="struct">\n'
s += f' <field name="neutral" value="true"/>\n'
s += f' <field name="scale" value="true"/>\n'
s += f' <field name="scale_f" value="true"/>\n'
s += f' </field>\n'
s += f' <field name="neutral" value="{round(p[0]):d},{int(round(p[1])):d},{int(round(p[2])):d}" type="int[]"/>\n'
s += f' <field name="scale" value="{{{{ {x_sens[0]:d}, {y_sens[0]:d}, {z_sens[0]:d} }},{{ {x_sens[1]:d}, {y_sens[1]:d}, {z_sens[1]:d} }}}}"/>\n'
s += f' <field name="scale_f" value="{{ {x_sens:.4f}, {y_sens:.4f}, {z_sens:.4f} }}"/>\n'
s += f' </field>\n'
s += f'</define>\n'
s += f'\n'
s += f'<define name="{sensor}_X_NEUTRAL" value="{round(p[0]):d}"/>\n'
s += f'<define name="{sensor}_Y_NEUTRAL" value="{round(p[1]):d}"/>\n'
s += f'<define name="{sensor}_Z_NEUTRAL" value="{round(p[2]):d}"/>\n'
s += f'<define name="{sensor}_X_SENS" value="{p[3]*2**res:.4f}" integer="16"/>\n'
s += f'<define name="{sensor}_Y_SENS" value="{p[4]*2**res:.4f}" integer="16"/>\n'
s += f'<define name="{sensor}_Z_SENS" value="{p[5]*2**res:.4f}" integer="16"/>\n'
return s
def print_xml(p, sensor, sensor_id, res):