Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-29 02:38:07 +08:00
Code Issues Actions 29 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/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