Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

restoring sensor simulation code

Browse Source
This commit is contained in:
Antoine Drouin
2009-07-30 01:41:42 +00:00
parent e9912fd19f
commit 229354b457
6 changed files with 55 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/simulator/nps/nps_sensors_params_booz2_a1.h
+9 -9
View File
@@ -77,17 +77,17 @@
#define NPS_GYRO_NEUTRAL_Q 33417
#define NPS_GYRO_NEUTRAL_R 32809
#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 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_INITIAL_P RadOfDeg( 1.0)
#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg( 2.0)
#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg( -1.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 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.)
sw/airborne/math/pprz_algebra.h
+7
View File
@@ -161,6 +161,13 @@
(_vo).z = (_va).z / (_vb).z; \
}
/* */
#define VECT3_EW_MUL(_vo, _va, _vb) { \
(_vo).x = (_va).x * (_vb).x; \
(_vo).y = (_va).y * (_vb).y; \
(_vo).z = (_va).z * (_vb).z; \
}
/* */
#define VECT3_BOUND_CUBE(_v, _min, _max) { \
if ((_v).x > (_max)) (_v).x = (_max); else if ((_v).x < (_min)) (_v).x = (_min); \
sw/simulator/nps/nps_random.c
+17
View File
@@ -37,6 +37,23 @@ void double_vect3_add_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect
vect->z += get_gaussian_noise() * std_dev->z;
}
void double_vect3_get_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev) {
vect->x = get_gaussian_noise() * std_dev->x;
vect->y = get_gaussian_noise() * std_dev->y;
vect->z = get_gaussian_noise() * std_dev->z;
}
void double_vect3_update_random_walk(struct DoubleVect3* rw, struct DoubleVect3* std_dev, double dt, double thau) {
struct DoubleVect3 drw;
double_vect3_get_gaussian_noise(&drw, std_dev);
struct DoubleVect3 tmp;
VECT3_SMUL(tmp, (-1./thau), *rw);
VECT3_ADD(drw, tmp);
VECT3_SMUL(drw, dt, drw);
VECT3_ADD(*rw, drw);
}
/*
http://www.taygeta.com/random/gaussian.html
*/
sw/simulator/nps/nps_random.h
+2 -2
View File
@@ -5,8 +5,8 @@
extern double get_gaussian_noise(void);
extern void double_vect3_add_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev);
extern void double_vect3_get_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev);
extern void double_vect3_update_random_walk(struct DoubleVect3* rw, struct DoubleVect3* std_dev, double dt, double thau);
#endif /* NPS_RANDOM_H */
sw/simulator/nps/nps_sensor_accel.c
+3 -4
View File
@@ -8,7 +8,7 @@
#include "math/pprz_algebra_int.h"
void nps_sensor_accel_init(struct NpsSensorAccel* accel, double time) {
VECT3_ASSIGN(accel->value, 0., 0., 0.);
FLOAT_VECT3_ZERO(accel->value);
accel->resolution = NPS_ACCEL_RESOLUTION;
FLOAT_MAT33_DIAG(accel->sensitivity,
NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, NPS_ACCEL_SENSITIVITY_ZZ);
@@ -56,9 +56,8 @@ void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, stru
/* white noise */
double_vect3_add_gaussian_noise(&accelero_error, &accel->noise_std_dev);
/* scale */
accelero_error.x *= MAT33_ELMT(accel->sensitivity, 0, 0);
accelero_error.y *= MAT33_ELMT(accel->sensitivity, 1, 1);
accelero_error.z *= MAT33_ELMT(accel->sensitivity, 2, 2);
struct DoubleVect3 gain = {NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, NPS_ACCEL_SENSITIVITY_ZZ};
VECT3_EW_MUL(accelero_error, accelero_error, gain);
/* add error */
VECT3_ADD(accel->value, accelero_error);
sw/simulator/nps/nps_sensor_gyro.c
+17 -28
View File
@@ -4,9 +4,10 @@
#include "nps_fdm.h"
#include NPS_SENSORS_PARAMS
#include "math/pprz_algebra_int.h"
#include "nps_random.h"
void nps_sensor_gyro_init(struct NpsSensorGyro* gyro, double time) {
VECT3_ASSIGN(gyro->value, 0., 0., 0.);
FLOAT_VECT3_ZERO(gyro->value);
gyro->resolution = NPS_GYRO_RESOLUTION;
FLOAT_MAT33_DIAG(gyro->sensitivity,
NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR);
@@ -20,6 +21,7 @@ void nps_sensor_gyro_init(struct NpsSensorGyro* gyro, double time) {
NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P,
NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q,
NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R);
FLOAT_VECT3_ZERO(gyro->bias_random_walk_value);
gyro->next_update = time;
gyro->data_available = FALSE;
}
@@ -30,43 +32,30 @@ void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct Do
return;
/* transform body rates to IMU frame */
DoubleVect3* rate_body = (struct DoubleVect3*)(&fdm.body_ecef_rotvel);
DoubleVect3 rate_imu;
struct DoubleVect3* rate_body = (struct DoubleVect3*)(&fdm.body_ecef_rotvel);
struct DoubleVect3 rate_imu;
MAT33_VECT3_MUL(rate_imu, *body_to_imu, *rate_body );
/* compute gyros readings */
MAT33_VECT3_MUL(gyro->value, gyro->sensitivity, rate_imu);
VECT3_ADD(gyro->value, gyro->neutral);
/* compute gyro error readings */
struct DoubleVect3 gyro_error;
VECT3_COPY(gyro_error, gyro->bias_initial);
double_vect3_add_gaussian_noise(&gyro_error, &gyro->noise_std_dev);
double_vect3_update_random_walk(&gyro->bias_random_walk_value, &gyro->bias_random_walk_std_dev,
NPS_GYRO_DT, 5.);
VECT3_ADD(gyro_error, gyro->bias_random_walk_value);
struct DoubleVect3 gain = {NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR};
VECT3_EW_MUL(gyro_error, gyro_error, gain);
VECT3_ADD(gyro->value, gyro_error);
/* round signal to account for adc discretisation */
DOUBLE_VECT3_ROUND(gyro->value);
/* saturate */
VECT3_BOUND_CUBE(gyro->value, 0, gyro->resolution);
#if 0
/* compute gyro error readings */
static VEC *gyro_error = VNULL;
gyro_error = v_resize(gyro_error, AXIS_NB);
gyro_error = v_zero(gyro_error);
/* add a gaussian noise */
gyro_error = v_add_gaussian_noise(gyro_error, bsm.gyro_noise_std_dev, gyro_error);
/* update random walk bias */
bsm.gyro_bias_random_walk_value =
v_update_random_walk(bsm.gyro_bias_random_walk_value,
bsm.gyro_bias_random_walk_std_dev, BSM_GYRO_DT,
bsm.gyro_bias_random_walk_value);
gyro_error = v_add(gyro_error, bsm.gyro_bias_random_walk_value, gyro_error);
/* scale to adc units FIXME : should use full adc gain ? sum ? */
gyro_error->ve[AXIS_P] =
gyro_error->ve[AXIS_P] * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
gyro_error->ve[AXIS_Q] =
gyro_error->ve[AXIS_Q] * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
gyro_error->ve[AXIS_R] =
gyro_error->ve[AXIS_R] * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
/* add per gyro error reading */
bsm.gyro = v_add(bsm.gyro, gyro_error, bsm.gyro);
#endif
gyro->next_update += NPS_GYRO_DT;
gyro->data_available = TRUE;
}