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

paparazzi · Jan 29, 2024 · fe35af7 · fe35af7
1 parent 41998a1
commit fe35af7
Show file tree

Hide file tree

Showing 7 changed files with 79 additions and 55 deletions.
diff --git a/conf/mag/toulouse_muret.xml b/conf/mag/toulouse_muret.xml
@@ -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>
 
diff --git a/conf/simulator/nps/nps_sensors_params_common.h b/conf/simulator/nps/nps_sensors_params_common.h
@@ -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

diff --git a/sw/airborne/modules/imu/imu_nps.c b/sw/airborne/modules/imu/imu_nps.c
@@ -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;
-
 }
 
 

diff --git a/sw/airborne/modules/sensors/sensors_hitl.c b/sw/airborne/modules/sensors/sensors_hitl.c
@@ -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

diff --git a/sw/simulator/nps/nps_sensor_accel.c b/sw/simulator/nps/nps_sensor_accel.c
@@ -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,

diff --git a/sw/simulator/nps/nps_sensor_gyro.c b/sw/simulator/nps/nps_sensor_gyro.c
@@ -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,

diff --git a/sw/simulator/nps/nps_sensor_mag.c b/sw/simulator/nps/nps_sensor_mag.c
@@ -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,