From 9797c2998dbcae06e94d550af4bafc133867f93b Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 9 Mar 2015 14:58:51 +0100 Subject: [PATCH] [nps] imu sensors: take sign into account --- .../simulator/nps/nps_sensors_params_default.h | 18 +++++++++--------- .../nps/nps_sensors_params_invariant.h | 18 +++++++++--------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/conf/simulator/nps/nps_sensors_params_default.h b/conf/simulator/nps/nps_sensors_params_default.h index de75e70dcb..2e80a9651a 100644 --- a/conf/simulator/nps/nps_sensors_params_default.h +++ b/conf/simulator/nps/nps_sensors_params_default.h @@ -38,9 +38,9 @@ #define NPS_ACCEL_MAX 4095 /* ms-2 */ /* aka 2^10/ACCEL_X_SENS */ -#define NPS_ACCEL_SENSITIVITY_XX ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS) -#define NPS_ACCEL_SENSITIVITY_YY ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS) -#define NPS_ACCEL_SENSITIVITY_ZZ ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS) +#define NPS_ACCEL_SENSITIVITY_XX IMU_ACCEL_X_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS) +#define NPS_ACCEL_SENSITIVITY_YY IMU_ACCEL_Y_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS) +#define NPS_ACCEL_SENSITIVITY_ZZ IMU_ACCEL_Z_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS) #define NPS_ACCEL_NEUTRAL_X IMU_ACCEL_X_NEUTRAL #define NPS_ACCEL_NEUTRAL_Y IMU_ACCEL_Y_NEUTRAL @@ -66,9 +66,9 @@ #define NPS_GYRO_MAX 32767 /* 2^12/GYRO_X_SENS */ -#define NPS_GYRO_SENSITIVITY_PP RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS) -#define NPS_GYRO_SENSITIVITY_QQ RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS) -#define NPS_GYRO_SENSITIVITY_RR RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS) +#define NPS_GYRO_SENSITIVITY_PP IMU_GYRO_P_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS) +#define NPS_GYRO_SENSITIVITY_QQ IMU_GYRO_Q_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS) +#define NPS_GYRO_SENSITIVITY_RR IMU_GYRO_R_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS) #define NPS_GYRO_NEUTRAL_P IMU_GYRO_P_NEUTRAL #define NPS_GYRO_NEUTRAL_Q IMU_GYRO_Q_NEUTRAL @@ -101,9 +101,9 @@ #define NPS_MAG_IMU_TO_SENSOR_THETA 0. #define NPS_MAG_IMU_TO_SENSOR_PSI 0. -#define NPS_MAG_SENSITIVITY_XX MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS) -#define NPS_MAG_SENSITIVITY_YY MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS) -#define NPS_MAG_SENSITIVITY_ZZ MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS) +#define NPS_MAG_SENSITIVITY_XX IMU_MAG_X_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS) +#define NPS_MAG_SENSITIVITY_YY IMU_MAG_Y_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS) +#define NPS_MAG_SENSITIVITY_ZZ IMU_MAG_Z_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS) #define NPS_MAG_NEUTRAL_X IMU_MAG_X_NEUTRAL #define NPS_MAG_NEUTRAL_Y IMU_MAG_Y_NEUTRAL diff --git a/conf/simulator/nps/nps_sensors_params_invariant.h b/conf/simulator/nps/nps_sensors_params_invariant.h index 51106e16b5..7310c8f327 100644 --- a/conf/simulator/nps/nps_sensors_params_invariant.h +++ b/conf/simulator/nps/nps_sensors_params_invariant.h @@ -38,9 +38,9 @@ #define NPS_ACCEL_MAX 32767 /* ms-2 */ /* aka 2^10/ACCEL_X_SENS */ -#define NPS_ACCEL_SENSITIVITY_XX ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS) -#define NPS_ACCEL_SENSITIVITY_YY ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS) -#define NPS_ACCEL_SENSITIVITY_ZZ ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS) +#define NPS_ACCEL_SENSITIVITY_XX IMU_ACCEL_X_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS) +#define NPS_ACCEL_SENSITIVITY_YY IMU_ACCEL_Y_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS) +#define NPS_ACCEL_SENSITIVITY_ZZ IMU_ACCEL_Z_SIGN * ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS) #define NPS_ACCEL_NEUTRAL_X IMU_ACCEL_X_NEUTRAL #define NPS_ACCEL_NEUTRAL_Y IMU_ACCEL_Y_NEUTRAL @@ -66,9 +66,9 @@ #define NPS_GYRO_MAX 32767 /* 2^12/GYRO_X_SENS */ -#define NPS_GYRO_SENSITIVITY_PP RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS) -#define NPS_GYRO_SENSITIVITY_QQ RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS) -#define NPS_GYRO_SENSITIVITY_RR RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS) +#define NPS_GYRO_SENSITIVITY_PP IMU_GYRO_P_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS) +#define NPS_GYRO_SENSITIVITY_QQ IMU_GYRO_Q_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS) +#define NPS_GYRO_SENSITIVITY_RR IMU_GYRO_R_SIGN * RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS) #define NPS_GYRO_NEUTRAL_P IMU_GYRO_P_NEUTRAL #define NPS_GYRO_NEUTRAL_Q IMU_GYRO_Q_NEUTRAL @@ -101,9 +101,9 @@ #define NPS_MAG_IMU_TO_SENSOR_THETA 0. #define NPS_MAG_IMU_TO_SENSOR_PSI 0. -#define NPS_MAG_SENSITIVITY_XX MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS) -#define NPS_MAG_SENSITIVITY_YY MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS) -#define NPS_MAG_SENSITIVITY_ZZ MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS) +#define NPS_MAG_SENSITIVITY_XX IMU_MAG_X_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS) +#define NPS_MAG_SENSITIVITY_YY IMU_MAG_Y_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS) +#define NPS_MAG_SENSITIVITY_ZZ IMU_MAG_Z_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS) #define NPS_MAG_NEUTRAL_X IMU_MAG_X_NEUTRAL #define NPS_MAG_NEUTRAL_Y IMU_MAG_Y_NEUTRAL