diff --git a/conf/mag/toulouse_muret.xml b/conf/mag/toulouse_muret.xml index cdc467af7dd..bf6e9862687 100644 --- a/conf/mag/toulouse_muret.xml +++ b/conf/mag/toulouse_muret.xml @@ -13,7 +13,7 @@
- +
diff --git a/conf/simulator/nps/nps_sensors_params_common.h b/conf/simulator/nps/nps_sensors_params_common.h index b280fc5c66d..4afecc4eb4d 100644 --- 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 index 596eac617f3..c7214f5afda 100644 --- 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 index 427559f5e7e..6472023fc5c 100644 --- 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 index d96fcd600d4..9420843629c 100644 --- 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 index 6362a7d89c6..0082ccccfbc 100644 --- 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 index be0a1114ff2..51d5b757057 100644 --- 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,