Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-25 23:46:04 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[nps] fix inconsistent sensor scaling in simulation (#3230)

Browse Source
This commit is contained in:
Gautier Hattenberger
2024-01-29 16:19:05 +01:00
committed by GitHub
parent 41998a12b3
commit fe35af7382
7 changed files with 79 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/mag/toulouse_muret.xml
+1 -1
View File
@@ -13,7 +13,7 @@
<section name="MAG_MODEL" prefix="INS_">
<define name="H_X" value="0.515118"/>
<define name="H_Y" value="0.0125878"/>
<define name="H_Z" value="0.0125878"/>
<define name="H_Z" value="0.857027"/>
</section>
</airframe>
conf/simulator/nps/nps_sensors_params_common.h
+54 -32
View File
@@ -57,26 +57,33 @@
#ifndef NPS_ACCEL_MAX
#define NPS_ACCEL_MAX 65536
#endif
/* ms-2 */
/* aka 2^10/ACCEL_X_SENS */
#define NPS_ACCEL_SENSITIVITY_NUM 981
#define NPS_ACCEL_SENSITIVITY_DEN 200
#define NPS_ACCEL_SENSITIVITY ((float)NPS_ACCEL_SENSITIVITY_NUM / (float)NPS_ACCEL_SENSITIVITY_DEN)
#ifdef IMU_ACCEL_X_SENS
#define NPS_ACCEL_SENSITIVITY_XX ACCEL_BFP_OF_REAL(1./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./NPS_ACCEL_SENSITIVITY)
#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 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./NPS_ACCEL_SENSITIVITY)
#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 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./NPS_ACCEL_SENSITIVITY)
#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
@@ -133,25 +140,32 @@
#define NPS_GYRO_MAX 65536
#endif
/* 2^12/GYRO_X_SENS */
#define NPS_GYRO_SENSITIVITY_NUM 36542
#define NPS_GYRO_SENSITIVITY_DEN 8383
#define NPS_GYRO_SENSITIVITY ((float)NPS_GYRO_SENSITIVITY_NUM / (float)NPS_GYRO_SENSITIVITY_DEN)
#ifdef IMU_GYRO_P_SENS
#define NPS_GYRO_SENSITIVITY_PP RATE_BFP_OF_REAL(1./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./NPS_GYRO_SENSITIVITY)
#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 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./NPS_GYRO_SENSITIVITY)
#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 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./NPS_GYRO_SENSITIVITY)
#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
@@ -216,24 +230,32 @@
#define NPS_MAG_IMU_TO_SENSOR_THETA 0.
#define NPS_MAG_IMU_TO_SENSOR_PSI 0.
#define NPS_MAG_SENSITIVITY_NUM 1
#define NPS_MAG_SENSITIVITY_DEN 1
#define NPS_MAG_SENSITIVITY ((float)NPS_MAG_SENSITIVITY_NUM / (float)NPS_MAG_SENSITIVITY_DEN)
#ifdef IMU_MAG_X_SENS
#define NPS_MAG_SENSITIVITY_XX MAG_BFP_OF_REAL(1./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./NPS_MAG_SENSITIVITY)
#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 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./NPS_MAG_SENSITIVITY)
#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 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./NPS_MAG_SENSITIVITY)
#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/modules/imu/imu_nps.c
+9 -13
View File
@@ -36,22 +36,22 @@ void imu_nps_init(void)
// Set the default scaling
const struct Int32Rates gyro_scale[2] = {
{NPS_GYRO_SENSITIVITY_NUM, NPS_GYRO_SENSITIVITY_NUM, NPS_GYRO_SENSITIVITY_NUM},
{NPS_GYRO_SENSITIVITY_DEN, NPS_GYRO_SENSITIVITY_DEN, NPS_GYRO_SENSITIVITY_DEN}
{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_NUM, NPS_ACCEL_SENSITIVITY_NUM, NPS_ACCEL_SENSITIVITY_NUM},
{NPS_ACCEL_SENSITIVITY_DEN, NPS_ACCEL_SENSITIVITY_DEN, NPS_ACCEL_SENSITIVITY_DEN}
{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_NUM, NPS_MAG_SENSITIVITY_NUM, NPS_MAG_SENSITIVITY_NUM},
{NPS_MAG_SENSITIVITY_DEN, NPS_MAG_SENSITIVITY_DEN, NPS_MAG_SENSITIVITY_DEN}
{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
@@ -64,23 +64,19 @@ void imu_nps_init(void)
void imu_feed_gyro_accel(void)
{
RATES_ASSIGN(imu_nps.gyro, NPS_GYRO_SIGN_P * sensors.gyro.value.x, NPS_GYRO_SIGN_Q * sensors.gyro.value.y, NPS_GYRO_SIGN_R * sensors.gyro.value.z);
VECT3_ASSIGN(imu_nps.accel, NPS_ACCEL_SIGN_X * sensors.accel.value.x, NPS_ACCEL_SIGN_Y * sensors.accel.value.y, NPS_ACCEL_SIGN_Z * sensors.accel.value.z);
RATES_ASSIGN(imu_nps.gyro, sensors.gyro.value.x, sensors.gyro.value.y, sensors.gyro.value.z);
VECT3_COPY(imu_nps.accel, sensors.accel.value);
// set availability flags...
imu_nps.accel_available = true;
imu_nps.gyro_available = true;
}
void imu_feed_mag(void)
{
VECT3_ASSIGN(imu_nps.mag, NPS_MAG_SIGN_X * sensors.mag.value.x, NPS_MAG_SIGN_Y * sensors.mag.value.y, NPS_MAG_SIGN_Z * sensors.mag.value.z);
VECT3_COPY(imu_nps.mag, sensors.mag.value);
imu_nps.mag_available = true;
}
sw/airborne/modules/sensors/sensors_hitl.c
+6 -6
View File
@@ -61,22 +61,22 @@ void sensors_hitl_init(void)
// Set the default scaling
const struct Int32Rates gyro_scale[2] = {
{NPS_GYRO_SENSITIVITY_NUM, NPS_GYRO_SENSITIVITY_NUM, NPS_GYRO_SENSITIVITY_NUM},
{NPS_GYRO_SENSITIVITY_DEN, NPS_GYRO_SENSITIVITY_DEN, NPS_GYRO_SENSITIVITY_DEN}
{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_NUM, NPS_ACCEL_SENSITIVITY_NUM, NPS_ACCEL_SENSITIVITY_NUM},
{NPS_ACCEL_SENSITIVITY_DEN, NPS_ACCEL_SENSITIVITY_DEN, NPS_ACCEL_SENSITIVITY_DEN}
{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_NUM, NPS_MAG_SENSITIVITY_NUM, NPS_MAG_SENSITIVITY_NUM},
{NPS_MAG_SENSITIVITY_DEN, NPS_MAG_SENSITIVITY_DEN, NPS_MAG_SENSITIVITY_DEN}
{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
sw/simulator/nps/nps_sensor_accel.c
+3 -1
View File
@@ -11,7 +11,9 @@ void nps_sensor_accel_init(struct NpsSensorAccel *accel, double time)
accel->min = NPS_ACCEL_MIN;
accel->max = NPS_ACCEL_MAX;
FLOAT_MAT33_DIAG(accel->sensitivity,
NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, NPS_ACCEL_SENSITIVITY_ZZ);
NPS_ACCEL_SIGN_X * NPS_ACCEL_SENSITIVITY_XX,
NPS_ACCEL_SIGN_Y * NPS_ACCEL_SENSITIVITY_YY,
NPS_ACCEL_SIGN_Z * NPS_ACCEL_SENSITIVITY_ZZ);
VECT3_ASSIGN(accel->neutral,
NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z);
VECT3_ASSIGN(accel->noise_std_dev,
sw/simulator/nps/nps_sensor_gyro.c
+3 -1
View File
@@ -12,7 +12,9 @@ void nps_sensor_gyro_init(struct NpsSensorGyro *gyro, double time)
gyro->min = NPS_GYRO_MIN;
gyro->max = NPS_GYRO_MAX;
FLOAT_MAT33_DIAG(gyro->sensitivity,
NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR);
NPS_GYRO_SIGN_P * NPS_GYRO_SENSITIVITY_PP,
NPS_GYRO_SIGN_Q * NPS_GYRO_SENSITIVITY_QQ,
NPS_GYRO_SIGN_R * NPS_GYRO_SENSITIVITY_RR);
VECT3_ASSIGN(gyro->neutral,
NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R);
VECT3_ASSIGN(gyro->noise_std_dev,
sw/simulator/nps/nps_sensor_mag.c
+3 -1
View File
@@ -11,7 +11,9 @@ void nps_sensor_mag_init(struct NpsSensorMag *mag, double time)
mag->min = NPS_MAG_MIN;
mag->max = NPS_MAG_MAX;
FLOAT_MAT33_DIAG(mag->sensitivity,
NPS_MAG_SENSITIVITY_XX, NPS_MAG_SENSITIVITY_YY, NPS_MAG_SENSITIVITY_ZZ);
NPS_MAG_SIGN_X * NPS_MAG_SENSITIVITY_XX,
NPS_MAG_SIGN_Y * NPS_MAG_SENSITIVITY_YY,
NPS_MAG_SIGN_Z * NPS_MAG_SENSITIVITY_ZZ);
VECT3_ASSIGN(mag->neutral,
NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z);
VECT3_ASSIGN(mag->noise_std_dev,