diff --git a/conf/simulator/nps/nps_sensors_params_aspirin.h b/conf/simulator/nps/nps_sensors_params_aspirin.h new file mode 100644 index 0000000000..4aecaa4d9f --- /dev/null +++ b/conf/simulator/nps/nps_sensors_params_aspirin.h @@ -0,0 +1,181 @@ +/* + * $Id$ + * + * Copyright (C) 2009 Antoine Drouin + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +#ifndef NPS_SENSORS_PARAMS_H +#define NPS_SENSORS_PARAMS_H + +#include "generated/airframe.h" + +#if 1 +#define NPS_BODY_TO_IMU_PHI IMU_BODY_TO_IMU_PHI +#define NPS_BODY_TO_IMU_THETA IMU_BODY_TO_IMU_THETA +#define NPS_BODY_TO_IMU_PSI IMU_BODY_TO_IMU_PSI +#else +#define NPS_BODY_TO_IMU_PHI RadOfDeg(0.) +#define NPS_BODY_TO_IMU_THETA RadOfDeg(0.) +#define NPS_BODY_TO_IMU_PSI RadOfDeg(0.) +#endif + +/* + * Accelerometer + */ +/* ADXL345 configured to +-2g with 10bit resolution */ +#define NPS_ACCEL_MIN -511 +#define NPS_ACCEL_MAX 511 +/* ms-2 */ +/* aka 2^10/ACCEL_X_SENS */ +#define NPS_ACCEL_SENSITIVITY_XX ACCEL_BFP_OF_REAL(1./37.91) +#define NPS_ACCEL_SENSITIVITY_YY ACCEL_BFP_OF_REAL(1./37.91) +#define NPS_ACCEL_SENSITIVITY_ZZ ACCEL_BFP_OF_REAL(1./39.24) + +#define NPS_ACCEL_NEUTRAL_X 0 +#define NPS_ACCEL_NEUTRAL_Y 0 +#define NPS_ACCEL_NEUTRAL_Z 0 +/* m2s-4 */ +#define NPS_ACCEL_NOISE_STD_DEV_X 5.e-2 +#define NPS_ACCEL_NOISE_STD_DEV_Y 5.e-2 +#define NPS_ACCEL_NOISE_STD_DEV_Z 5.e-2 +/* ms-2 */ +#define NPS_ACCEL_BIAS_X 0 +#define NPS_ACCEL_BIAS_Y 0 +#define NPS_ACCEL_BIAS_Z 0 +/* s */ +#define NPS_ACCEL_DT (1./512.) + + + +/* + * Gyrometer + */ +/* IMU-3000 has 16 bit resolution */ +#define NPS_GYRO_MIN -32767 +#define NPS_GYRO_MAX 32767 + +/* 2^12/GYRO_X_SENS */ +#define NPS_GYRO_SENSITIVITY_PP RATE_BFP_OF_REAL(1./4.359) +#define NPS_GYRO_SENSITIVITY_QQ RATE_BFP_OF_REAL(1./4.359) +#define NPS_GYRO_SENSITIVITY_RR RATE_BFP_OF_REAL(1./4.359) + +#define NPS_GYRO_NEUTRAL_P 0 +#define NPS_GYRO_NEUTRAL_Q 0 +#define NPS_GYRO_NEUTRAL_R 0 + +#define NPS_GYRO_NOISE_STD_DEV_P RadOfDeg(0.) +#define NPS_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.) +#define NPS_GYRO_NOISE_STD_DEV_R RadOfDeg(0.) + +#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0) +#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg( 0.0) +#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg( 0.0) + +#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5) +#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.5) +#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.5) +/* s */ +#define NPS_GYRO_DT (1./512.) + + + +/* + * Magnetometer + */ + /* HMC5843 has 12 bit resolution */ +#define NPS_MAG_MIN -2047 +#define NPS_MAG_MAX 2047 + +#define NPS_MAG_IMU_TO_SENSOR_PHI 0. +#define NPS_MAG_IMU_TO_SENSOR_THETA 0. +#define NPS_MAG_IMU_TO_SENSOR_PSI RadOfDeg(45.) + +#define NPS_MAG_SENSITIVITY_XX MAG_BFP_OF_REAL(-1./4.94075530) +#define NPS_MAG_SENSITIVITY_YY MAG_BFP_OF_REAL( 1./5.10207664) +#define NPS_MAG_SENSITIVITY_ZZ MAG_BFP_OF_REAL(-1./4.90788848) + +#define NPS_MAG_NEUTRAL_X 2358 +#define NPS_MAG_NEUTRAL_Y 2362 +#define NPS_MAG_NEUTRAL_Z 2119 + +#define NPS_MAG_NOISE_STD_DEV_X 2e-3 +#define NPS_MAG_NOISE_STD_DEV_Y 2e-3 +#define NPS_MAG_NOISE_STD_DEV_Z 2e-3 + +#define NPS_MAG_DT (1./100.) + + +/* + * Range meter + */ +#define BSM_RANGEMETER_RESOLUTION (1024) +#define BSM_RANGEMETER_SENSITIVITY (1024. / 12.) +#define BSM_RANGEMETER_MAX_RANGE (6. * BSM_RANGEMETER_SENSITIVITY) +#define BSM_RANGEMETER_DT (1./20.) + + +/* + * Barometer + */ +/* m */ +/* aka 2^8/INS_BARO_SENS */ +#define NPS_BARO_QNH 900. +#define NPS_BARO_SENSITIVITY 17.066667 +#define NPS_BARO_DT (1./100.) +#define NPS_BARO_NOISE_STD_DEV 5.e-2 + +/* + * GPS + */ + +#define GPS_PERFECT 1 + +#if GPS_PERFECT + +#define NPS_GPS_SPEED_NOISE_STD_DEV 0. +#define NPS_GPS_SPEED_LATENCY 0. +#define NPS_GPS_POS_NOISE_STD_DEV 0.001 +#define NPS_GPS_POS_BIAS_INITIAL_X 0. +#define NPS_GPS_POS_BIAS_INITIAL_Y 0. +#define NPS_GPS_POS_BIAS_INITIAL_Z 0. +#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 0. +#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 0. +#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 0. +#define NPS_GPS_POS_LATENCY 0. + +#else + +#define NPS_GPS_SPEED_NOISE_STD_DEV 0.5 +#define NPS_GPS_SPEED_LATENCY 0.2 +#define NPS_GPS_POS_NOISE_STD_DEV 2 +#define NPS_GPS_POS_BIAS_INITIAL_X 0e-1 +#define NPS_GPS_POS_BIAS_INITIAL_Y -0e-1 +#define NPS_GPS_POS_BIAS_INITIAL_Z -0e-1 +#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-3 +#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-3 +#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-3 +#define NPS_GPS_POS_LATENCY 0.2 + +#endif /* GPS_PERFECT */ + +#define NPS_GPS_DT (1./4.) + +#endif /* NPS_SENSORS_PARAMS_H */ diff --git a/conf/simulator/nps/nps_sensors_params_booz2_a1.h b/conf/simulator/nps/nps_sensors_params_booz2_a1.h index 89e9f07124..3464326aa8 100644 --- a/conf/simulator/nps/nps_sensors_params_booz2_a1.h +++ b/conf/simulator/nps/nps_sensors_params_booz2_a1.h @@ -1,6 +1,6 @@ /* * $Id$ - * + * * Copyright (C) 2009 Antoine Drouin * * This file is part of paparazzi. @@ -18,7 +18,7 @@ * You should have received a copy of the GNU General Public License * along with paparazzi; see the file COPYING. If not, write to * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * Boston, MA 02111-1307, USA. * */ @@ -37,10 +37,11 @@ #define NPS_BODY_TO_IMU_PSI RadOfDeg(0.) #endif -/* - * Accelerometer +/* + * Accelerometer */ -#define NPS_ACCEL_RESOLUTION (65536) +#define NPS_ACCEL_MIN 0 +#define NPS_ACCEL_MAX 65536 /* ms-2 */ /* aka 2^10/ACCEL_X_SENS */ #define NPS_ACCEL_SENSITIVITY_XX ACCEL_BFP_OF_REAL(-1./2.50411474) @@ -66,7 +67,8 @@ /* * Gyrometer */ -#define NPS_GYRO_RESOLUTION 65536 +#define NPS_GYRO_MIN 0 +#define NPS_GYRO_MAX 65536 /* 2^12/GYRO_X_SENS */ #define NPS_GYRO_SENSITIVITY_PP RATE_BFP_OF_REAL( 1./1.01) @@ -96,7 +98,8 @@ /* * Magnetometer */ -//#define BSM_MAG_RESOLUTION 65536 +#define NPS_MAG_MIN -32767 +#define NPS_MAG_MAX 32767 #define NPS_MAG_IMU_TO_SENSOR_PHI 0. #define NPS_MAG_IMU_TO_SENSOR_THETA 0. diff --git a/conf/simulator/nps/nps_sensors_params_booz2_a1p.h b/conf/simulator/nps/nps_sensors_params_booz2_a1p.h index 6d5ca25dd1..3edb61d98f 100644 --- a/conf/simulator/nps/nps_sensors_params_booz2_a1p.h +++ b/conf/simulator/nps/nps_sensors_params_booz2_a1p.h @@ -1,6 +1,6 @@ /* * $Id$ - * + * * Copyright (C) 2009 Antoine Drouin * * This file is part of paparazzi. @@ -18,7 +18,7 @@ * You should have received a copy of the GNU General Public License * along with paparazzi; see the file COPYING. If not, write to * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * Boston, MA 02111-1307, USA. * */ @@ -37,10 +37,11 @@ #define NPS_BODY_TO_IMU_PSI RadOfDeg(0.) #endif -/* - * Accelerometer +/* + * Accelerometer */ -#define NPS_ACCEL_RESOLUTION (65536) +#define NPS_ACCEL_MIN 0 +#define NPS_ACCEL_MAX 65536 /* ms-2 */ /* aka 2^10/ACCEL_X_SENS */ #define NPS_ACCEL_SENSITIVITY_XX ACCEL_BFP_OF_REAL(-1./2.50411474) @@ -66,7 +67,8 @@ /* * Gyrometer */ -#define NPS_GYRO_RESOLUTION 65536 +#define NPS_GYRO_MIN 0 +#define NPS_GYRO_MAX 65536 /* 2^12/GYRO_X_SENS */ #define NPS_GYRO_SENSITIVITY_PP RATE_BFP_OF_REAL( 1./1.01) @@ -96,7 +98,8 @@ /* * Magnetometer */ -//#define BSM_MAG_RESOLUTION 65536 +#define NPS_MAG_MIN -32767 +#define NPS_MAG_MAX 32767 #define NPS_MAG_IMU_TO_SENSOR_PHI 0. #define NPS_MAG_IMU_TO_SENSOR_THETA 0. diff --git a/sw/simulator/nps/nps_sensor_accel.c b/sw/simulator/nps/nps_sensor_accel.c index 9876ed7869..29868c2c83 100644 --- a/sw/simulator/nps/nps_sensor_accel.c +++ b/sw/simulator/nps/nps_sensor_accel.c @@ -9,7 +9,8 @@ void nps_sensor_accel_init(struct NpsSensorAccel* accel, double time) { FLOAT_VECT3_ZERO(accel->value); - accel->resolution = NPS_ACCEL_RESOLUTION; + 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); VECT3_ASSIGN(accel->neutral, @@ -65,7 +66,7 @@ void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, stru /* round signal to account for adc discretisation */ DOUBLE_VECT3_ROUND(accel->value); /* saturate */ - VECT3_BOUND_CUBE(accel->value, 0, accel->resolution); + VECT3_BOUND_CUBE(accel->value, accel->min, accel->max); accel->next_update += NPS_ACCEL_DT; accel->data_available = TRUE; diff --git a/sw/simulator/nps/nps_sensor_accel.h b/sw/simulator/nps/nps_sensor_accel.h index 4b641994b2..4262e01f31 100644 --- a/sw/simulator/nps/nps_sensor_accel.h +++ b/sw/simulator/nps/nps_sensor_accel.h @@ -8,7 +8,8 @@ struct NpsSensorAccel { struct DoubleVect3 value; - unsigned int resolution; + int min; + int max; struct DoubleMat33 sensitivity; struct DoubleVect3 neutral; struct DoubleVect3 noise_std_dev; diff --git a/sw/simulator/nps/nps_sensor_gyro.c b/sw/simulator/nps/nps_sensor_gyro.c index 0fe36283ff..f6248a9ddd 100644 --- a/sw/simulator/nps/nps_sensor_gyro.c +++ b/sw/simulator/nps/nps_sensor_gyro.c @@ -8,7 +8,8 @@ void nps_sensor_gyro_init(struct NpsSensorGyro* gyro, double time) { FLOAT_VECT3_ZERO(gyro->value); - gyro->resolution = NPS_GYRO_RESOLUTION; + 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); VECT3_ASSIGN(gyro->neutral, @@ -54,7 +55,7 @@ void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct Do /* round signal to account for adc discretisation */ DOUBLE_VECT3_ROUND(gyro->value); /* saturate */ - VECT3_BOUND_CUBE(gyro->value, 0, gyro->resolution); + VECT3_BOUND_CUBE(gyro->value, gyro->min, gyro->max); gyro->next_update += NPS_GYRO_DT; gyro->data_available = TRUE; diff --git a/sw/simulator/nps/nps_sensor_gyro.h b/sw/simulator/nps/nps_sensor_gyro.h index f6568b2518..55490e9447 100644 --- a/sw/simulator/nps/nps_sensor_gyro.h +++ b/sw/simulator/nps/nps_sensor_gyro.h @@ -8,7 +8,8 @@ struct NpsSensorGyro { struct DoubleVect3 value; - unsigned int resolution; + int min; + int max; struct DoubleMat33 sensitivity; struct DoubleVect3 neutral; struct DoubleVect3 noise_std_dev; diff --git a/sw/simulator/nps/nps_sensor_mag.c b/sw/simulator/nps/nps_sensor_mag.c index 6ab23030a9..fc5ea946b3 100644 --- a/sw/simulator/nps/nps_sensor_mag.c +++ b/sw/simulator/nps/nps_sensor_mag.c @@ -7,7 +7,8 @@ void nps_sensor_mag_init(struct NpsSensorMag* mag, double time) { VECT3_ASSIGN(mag->value, 0., 0., 0.); - // mag->resolution = NPS_MAG_RESOLUTION; + 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); VECT3_ASSIGN(mag->neutral, @@ -46,7 +47,7 @@ void nps_sensor_mag_run_step(struct NpsSensorMag* mag, double time, struct Doubl /* round signal to account for adc discretisation */ DOUBLE_VECT3_ROUND(mag->value); /* saturate */ - // VECT3_BOUND_CUBE(mag->value, 0, mag->resolution); + VECT3_BOUND_CUBE(mag->value, mag->min, mag->max); mag->next_update += NPS_MAG_DT; mag->data_available = TRUE; diff --git a/sw/simulator/nps/nps_sensor_mag.h b/sw/simulator/nps/nps_sensor_mag.h index 8eda06160c..ab0fdeb849 100644 --- a/sw/simulator/nps/nps_sensor_mag.h +++ b/sw/simulator/nps/nps_sensor_mag.h @@ -8,7 +8,8 @@ struct NpsSensorMag { struct DoubleVect3 value; - unsigned int resolution; + int min; + int max; struct DoubleMat33 sensitivity; struct DoubleVect3 neutral; struct DoubleVect3 noise_std_dev;