From 4173b5352b04e3facd92e250dcf69f2f83474ab2 Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Thu, 18 Jun 2009 10:55:25 +0000 Subject: [PATCH] --- conf/simulator/nps_sensors_params_booz2_a1.h | 36 +++++------ sw/airborne/pprz_algebra.h | 8 ++- sw/airborne/pprz_algebra_float.h | 15 +++++ sw/simulator/nps_sensor_gyro.c | 63 +++++++------------- sw/simulator/nps_sensor_gyro.h | 2 + 5 files changed, 61 insertions(+), 63 deletions(-) diff --git a/conf/simulator/nps_sensors_params_booz2_a1.h b/conf/simulator/nps_sensors_params_booz2_a1.h index 8214bd291d..1dbe71b585 100644 --- a/conf/simulator/nps_sensors_params_booz2_a1.h +++ b/conf/simulator/nps_sensors_params_booz2_a1.h @@ -70,31 +70,27 @@ #define NPS_GYRO_RESOLUTION 65536 /* 2^12/GYRO_X_SENS */ -#define BSM_GYRO_SENSITIVITY_PP ( 4055.) -#define BSM_GYRO_SENSITIVITY_QQ (-4055.) -#define BSM_GYRO_SENSITIVITY_RR (-4055.) +#define NPS_GYRO_SENSITIVITY_PP ( 4055.) +#define NPS_GYRO_SENSITIVITY_QQ (-4055.) +#define NPS_GYRO_SENSITIVITY_RR (-4055.) -#define BSM_GYRO_NEUTRAL_P 33924 -#define BSM_GYRO_NEUTRAL_Q 33417 -#define BSM_GYRO_NEUTRAL_R 32809 +#define NPS_GYRO_NEUTRAL_P 33924 +#define NPS_GYRO_NEUTRAL_Q 33417 +#define NPS_GYRO_NEUTRAL_R 32809 -//#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.) -//#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.) -//#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.) +#define NPS_GYRO_NOISE_STD_DEV_P RadOfDeg(0.5) +#define NPS_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.5) +#define NPS_GYRO_NOISE_STD_DEV_R RadOfDeg(0.5) -#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.5) -#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.5) -#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.5) +#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0) +#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg( .0) +#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg( .0) -#define BSM_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0) -#define BSM_GYRO_BIAS_INITIAL_Q RadOfDeg( .0) -#define BSM_GYRO_BIAS_INITIAL_R RadOfDeg( .0) +#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.) +#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.) +#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.) -#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.) -#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.) -#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.) - -#define BSM_GYRO_DT (1./512.) +#define NPS_GYRO_DT (1./512.) diff --git a/sw/airborne/pprz_algebra.h b/sw/airborne/pprz_algebra.h index 4948e83236..a4744f6516 100644 --- a/sw/airborne/pprz_algebra.h +++ b/sw/airborne/pprz_algebra.h @@ -268,6 +268,12 @@ } +/* + * 3x3 matrices + */ +/* accessor : row and col range from 0 to 2 */ +#define MAT33_ELMT(_m, _row, _col) ((_m).m[_row*3+_col]) + /* multiply _vin by _mat, store in _vout */ #define MAT33_VECT3_MUL(_vout, _mat, _vin) { \ @@ -332,7 +338,7 @@ */ /* accessor : row and col range from 0 to 2 */ -#define RMAT_ELMT(_rm, _row, _col) (_rm.m[_row*3+_col]) +#define RMAT_ELMT(_rm, _row, _col) MAT33_ELMT(_rm, _row, _col) /* trace */ #define RMAT_TRACE(_rm) (RMAT_ELMT(_rm, 0, 0)+RMAT_ELMT(_rm, 1, 1)+RMAT_ELMT(_rm, 2, 2)) diff --git a/sw/airborne/pprz_algebra_float.h b/sw/airborne/pprz_algebra_float.h index f3fea58581..55e085ba74 100644 --- a/sw/airborne/pprz_algebra_float.h +++ b/sw/airborne/pprz_algebra_float.h @@ -100,6 +100,21 @@ struct FloatRates { } +/* + * 3x3 matrices + */ +#define FLOAT_MAT33_ZERO(_m) { \ + RMAT_ELMT(_m, 0, 0) = 0.; \ + RMAT_ELMT(_m, 0, 1) = 0.; \ + RMAT_ELMT(_m, 0, 2) = 0.; \ + RMAT_ELMT(_m, 1, 0) = 0.; \ + RMAT_ELMT(_m, 1, 1) = 0.; \ + RMAT_ELMT(_m, 1, 2) = 0.; \ + RMAT_ELMT(_m, 2, 0) = 0.; \ + RMAT_ELMT(_m, 2, 1) = 0.; \ + RMAT_ELMT(_m, 2, 2) = 0.; \ + } + /* * Rotation Matrices */ diff --git a/sw/simulator/nps_sensor_gyro.c b/sw/simulator/nps_sensor_gyro.c index b40c108501..5be8b8bd31 100644 --- a/sw/simulator/nps_sensor_gyro.c +++ b/sw/simulator/nps_sensor_gyro.c @@ -16,50 +16,29 @@ bool_t nps_sensor_gyro_available() { void nps_sensor_gyro_init(double time) { VECT3_ASSIGN(sensors.gyro.value, 0., 0., 0.); sensors.gyro.resolution = NPS_GYRO_RESOLUTION; -#if 0 - bsm.gyro_resolution = BSM_GYRO_RESOLUTION; - - bsm.gyro_sensitivity = m_get(AXIS_NB, AXIS_NB); - m_zero(bsm.gyro_sensitivity); - bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] = BSM_GYRO_SENSITIVITY_PP; - bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] = BSM_GYRO_SENSITIVITY_QQ; - bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] = BSM_GYRO_SENSITIVITY_RR; - - bsm.gyro_neutral = v_get(AXIS_NB); - bsm.gyro_neutral->ve[AXIS_P] = BSM_GYRO_NEUTRAL_P; - bsm.gyro_neutral->ve[AXIS_Q] = BSM_GYRO_NEUTRAL_Q; - bsm.gyro_neutral->ve[AXIS_R] = BSM_GYRO_NEUTRAL_R; - - bsm.gyro_noise_std_dev = v_get(AXIS_NB); - bsm.gyro_noise_std_dev->ve[AXIS_P] = BSM_GYRO_NOISE_STD_DEV_P; - bsm.gyro_noise_std_dev->ve[AXIS_Q] = BSM_GYRO_NOISE_STD_DEV_Q; - bsm.gyro_noise_std_dev->ve[AXIS_R] = BSM_GYRO_NOISE_STD_DEV_R; - - bsm.gyro_bias_initial = v_get(AXIS_NB); - bsm.gyro_bias_initial->ve[AXIS_P] = BSM_GYRO_BIAS_INITIAL_P; - bsm.gyro_bias_initial->ve[AXIS_Q] = BSM_GYRO_BIAS_INITIAL_Q; - bsm.gyro_bias_initial->ve[AXIS_R] = BSM_GYRO_BIAS_INITIAL_R; - - bsm.gyro_bias_random_walk_std_dev = v_get(AXIS_NB); - bsm.gyro_bias_random_walk_std_dev->ve[AXIS_P] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P; - bsm.gyro_bias_random_walk_std_dev->ve[AXIS_Q] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q; - bsm.gyro_bias_random_walk_std_dev->ve[AXIS_R] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R; - - bsm.gyro_bias_random_walk_value = v_get(AXIS_NB); - bsm.gyro_bias_random_walk_value->ve[AXIS_P] = bsm.gyro_bias_initial->ve[AXIS_P]; - bsm.gyro_bias_random_walk_value->ve[AXIS_Q] = bsm.gyro_bias_initial->ve[AXIS_Q]; - bsm.gyro_bias_random_walk_value->ve[AXIS_R] = bsm.gyro_bias_initial->ve[AXIS_R]; - - bsm.gyro_next_update = time; - bsm.gyro_available = FALSE; -#endif + FLOAT_MAT33_ZERO(sensors.gyro.sensitivity); + MAT33_ELMT(sensors.gyro.sensitivity, 0, 0) = NPS_GYRO_SENSITIVITY_PP; + MAT33_ELMT(sensors.gyro.sensitivity, 1, 1) = NPS_GYRO_SENSITIVITY_QQ; + MAT33_ELMT(sensors.gyro.sensitivity, 2, 2) = NPS_GYRO_SENSITIVITY_RR; + VECT3_ASSIGN(sensors.gyro.neutral, + NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R); + VECT3_ASSIGN(sensors.gyro.noise_std_dev, + NPS_GYRO_NOISE_STD_DEV_P, NPS_GYRO_NOISE_STD_DEV_Q, NPS_GYRO_NOISE_STD_DEV_R); + VECT3_ASSIGN(sensors.gyro.bias_initial, + NPS_GYRO_BIAS_INITIAL_P, NPS_GYRO_BIAS_INITIAL_Q, NPS_GYRO_BIAS_INITIAL_R); + VECT3_ASSIGN(sensors.gyro.bias_random_walk_std_dev, + NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P, + NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q, + NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R); + sensors.gyro.next_update = time; + sensors.gyro.data_available = FALSE; } void booz_sensors_model_gyro_run( double time ) { -#if 0 - if (time < bsm.gyro_next_update) + if (time < sensors.gyro.next_update) return; +#if 0 /* rotate to IMU frame */ /* convert to imu frame */ static VEC* rate_imu = VNULL; @@ -94,10 +73,10 @@ void booz_sensors_model_gyro_run( double time ) { RoundSensor(bsm.gyro); /* saturation */ BoundSensor(bsm.gyro, 0, bsm.gyro_resolution); - - bsm.gyro_next_update += BSM_GYRO_DT; - bsm.gyro_available = TRUE; #endif + + sensors.gyro.next_update += NPS_GYRO_DT; + sensors.gyro.data_available = TRUE; } diff --git a/sw/simulator/nps_sensor_gyro.h b/sw/simulator/nps_sensor_gyro.h index cdbf512549..9bfb0debc5 100644 --- a/sw/simulator/nps_sensor_gyro.h +++ b/sw/simulator/nps_sensor_gyro.h @@ -1,7 +1,9 @@ #ifndef NPS_SENSORS_GYRO_H #define NPS_SENSORS_GYRO_H +#include "pprz_algebra.h" #include "pprz_algebra_double.h" +#include "pprz_algebra_float.h" #include "std.h" struct NpsSensorGyro {