Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

splited sensor model in multiple files

Browse Source
This commit is contained in:
Antoine Drouin
2008-02-28 13:34:43 +00:00
parent cf7680cd7b
commit 7063c447e7
15 changed files with 832 additions and 487 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/booz_sensors_model.c
+27 -435
View File
@@ -9,35 +9,35 @@
struct BoozSensorsModel bsm;
static void booz_sensors_model_accel_init(void);
static void booz_sensors_model_accel_run(MAT* dcm);
extern void booz_sensors_model_accel_init(double time);
extern void booz_sensors_model_accel_run(double time, MAT* dcm);
static void booz_sensors_model_gyro_init(void);
static void booz_sensors_model_gyro_run(double dt);
extern void booz_sensors_model_gyro_init(double time);
extern void booz_sensors_model_gyro_run(double time);
static void booz_sensors_model_mag_init(void);
static void booz_sensors_model_mag_run(MAT* dcm);
extern void booz_sensors_model_mag_init(double time);
extern void booz_sensors_model_mag_run(double time, MAT* dcm);
static void booz_sensors_model_range_meter_init(void);
static void booz_sensors_model_range_meter_run();
extern void booz_sensors_model_rangemeter_init(double time);
extern void booz_sensors_model_rangemeter_run(double time);
static void booz_sensors_model_gps_init(void);
static void booz_sensors_model_gps_run(double dt, MAT* dcm_t);
extern void booz_sensors_model_baro_init(double time);
extern void booz_sensors_model_baro_run(double time);
static VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out);
static VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* out);
static void UpdateSensorLatency(VEC* cur_reading, GSList* history,
double latency, VEC* sensor_reading);
extern void booz_sensors_model_gps_init(double time);
extern void booz_sensors_model_gps_run(double time, MAT* dcm_t);
void booz_sensors_model_init(void) {
booz_sensors_model_accel_init();
booz_sensors_model_gyro_init();
booz_sensors_model_mag_init();
booz_sensors_model_range_meter_init();
booz_sensors_model_gps_init();
void booz_sensors_model_init(double time) {
booz_sensors_model_accel_init(time);
booz_sensors_model_gyro_init(time);
booz_sensors_model_mag_init(time);
booz_sensors_model_rangemeter_init(time);
booz_sensors_model_baro_init(time);
booz_sensors_model_gps_init(time);
}
void booz_sensors_model_run(double dt) {
void booz_sensors_model_run(double time) {
/* extract eulers angles from state */
static VEC *eulers = VNULL;
@@ -54,424 +54,16 @@ void booz_sensors_model_run(double dt) {
dcm_t = m_resize(dcm_t,AXIS_NB, AXIS_NB);
dcm_t = m_transp(dcm, dcm_t);
booz_sensors_model_accel_run(dcm);
booz_sensors_model_gyro_run(dt);
booz_sensors_model_mag_run(dcm);
booz_sensors_model_range_meter_run();
booz_sensors_model_gps_run(dt, dcm_t);
booz_sensors_model_accel_run(time, dcm);
booz_sensors_model_gyro_run(time);
booz_sensors_model_mag_run(time, dcm);
booz_sensors_model_rangemeter_run(time);
booz_sensors_model_baro_run(time);
booz_sensors_model_gps_run(time, dcm_t);
}
static void booz_sensors_model_accel_init(void) {
bsm.accel = v_get(AXIS_NB);
bsm.accel->ve[AXIS_X] = 0.;
bsm.accel->ve[AXIS_Y] = 0.;
bsm.accel->ve[AXIS_Z] = 0.;
bsm.accel_resolution = BSM_ACCEL_RESOLUTION;
bsm.accel_sensitivity = m_get(AXIS_NB, AXIS_NB);
m_zero(bsm.accel_sensitivity);
bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = BSM_ACCEL_SENSITIVITY_XX;
bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = BSM_ACCEL_SENSITIVITY_YY;
bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = BSM_ACCEL_SENSITIVITY_ZZ;
bsm.accel_neutral = v_get(AXIS_NB);
bsm.accel_neutral->ve[AXIS_X] = BSM_ACCEL_NEUTRAL_X;
bsm.accel_neutral->ve[AXIS_Y] = BSM_ACCEL_NEUTRAL_Y;
bsm.accel_neutral->ve[AXIS_Z] = BSM_ACCEL_NEUTRAL_Z;
bsm.accel_noise_std_dev = v_get(AXIS_NB);
bsm.accel_noise_std_dev->ve[AXIS_X] = BSM_ACCEL_NOISE_STD_DEV_X;
bsm.accel_noise_std_dev->ve[AXIS_Y] = BSM_ACCEL_NOISE_STD_DEV_Y;
bsm.accel_noise_std_dev->ve[AXIS_Z] = BSM_ACCEL_NOISE_STD_DEV_Z;
bsm.accel_bias = v_get(AXIS_NB);
bsm.accel_bias->ve[AXIS_P] = BSM_ACCEL_BIAS_X;
bsm.accel_bias->ve[AXIS_Q] = BSM_ACCEL_BIAS_Y;
bsm.accel_bias->ve[AXIS_R] = BSM_ACCEL_BIAS_Z;
}
static void booz_sensors_model_gyro_init(void) {
bsm.gyro = v_get(AXIS_NB);
bsm.gyro->ve[AXIS_P] = 0.;
bsm.gyro->ve[AXIS_Q] = 0.;
bsm.gyro->ve[AXIS_R] = 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];
}
static void booz_sensors_model_mag_init(void) {
bsm.mag = v_get(AXIS_NB);
bsm.mag->ve[AXIS_X] = 0.;
bsm.mag->ve[AXIS_Y] = 0.;
bsm.mag->ve[AXIS_Z] = 0.;
bsm.mag_resolution = 4096;
bsm.mag_sensitivity = m_get(AXIS_NB, AXIS_NB);
m_zero(bsm.mag_sensitivity);
bsm.mag_sensitivity->me[AXIS_X][AXIS_X] = -(double)bsm.mag_resolution / 6.;
bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y] = -(double)bsm.mag_resolution / 6.;
bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z] = (double)bsm.mag_resolution / 6.;
bsm.mag_neutral = v_get(AXIS_NB);
bsm.mag_neutral->ve[AXIS_X] = 0.;
bsm.mag_neutral->ve[AXIS_Y] = 0.;
bsm.mag_neutral->ve[AXIS_Z] = 0.;
bsm.mag_noise_std_dev = v_get(AXIS_NB);
bsm.mag_noise_std_dev->ve[AXIS_X] = 2e-2;
bsm.mag_noise_std_dev->ve[AXIS_Y] = 2e-2;
bsm.mag_noise_std_dev->ve[AXIS_Z] = 2e-2;
}
static void booz_sensors_model_range_meter_init(void) {
bsm.range_meter = 0.;
bsm.range_meter_resolution = BSM_RANGE_METER_RESOLUTION;
bsm.range_meter_sensivity = BSM_RANGE_METER_SENSITIVITY;
}
static void booz_sensors_model_gps_init(void) {
bsm.speed_sensor = v_get(AXIS_NB);
v_zero(bsm.speed_sensor);
bsm.speed_noise_std_dev = v_get(AXIS_NB);
bsm.speed_noise_std_dev->ve[AXIS_X] = 1e-1;
bsm.speed_noise_std_dev->ve[AXIS_Y] = 1e-1;
bsm.speed_noise_std_dev->ve[AXIS_Z] = 1e-1;
bsm.speed_latency = .25;
bsm.speed_history = NULL;
bsm.pos_sensor = v_get(AXIS_NB);
v_zero(bsm.pos_sensor);
bsm.pos_noise_std_dev = v_get(AXIS_NB);
bsm.pos_noise_std_dev->ve[AXIS_X] = 3e-1;
bsm.pos_noise_std_dev->ve[AXIS_Y] = 3e-1;
bsm.pos_noise_std_dev->ve[AXIS_Z] = 3e-1;
bsm.pos_bias_initial = v_get(AXIS_NB);
bsm.pos_bias_initial->ve[AXIS_X] = 1e-1;
bsm.pos_bias_initial->ve[AXIS_Y] = 1e-1;
bsm.pos_bias_initial->ve[AXIS_Z] = 1e-1;
bsm.pos_bias_random_walk_std_dev = v_get(AXIS_NB);
bsm.pos_bias_random_walk_std_dev->ve[AXIS_X] = 1e-1;
bsm.pos_bias_random_walk_std_dev->ve[AXIS_Y] = 1e-1;
bsm.pos_bias_random_walk_std_dev->ve[AXIS_Z] = 1e-1;
bsm.pos_bias_random_walk_value = v_get(AXIS_NB);
bsm.pos_bias_random_walk_value->ve[AXIS_X] = bsm.pos_bias_initial->ve[AXIS_X];
bsm.pos_bias_random_walk_value->ve[AXIS_Y] = bsm.pos_bias_initial->ve[AXIS_Y];
bsm.pos_bias_random_walk_value->ve[AXIS_Z] = bsm.pos_bias_initial->ve[AXIS_Z];
bsm.pos_latency = .25;
bsm.pos_history = NULL;
}
#define RoundSensor(_sensor) { \
_sensor->ve[AXIS_X] = rint(_sensor->ve[AXIS_X]); \
_sensor->ve[AXIS_Y] = rint(_sensor->ve[AXIS_Y]); \
_sensor->ve[AXIS_Z] = rint(_sensor->ve[AXIS_Z]); \
}
#define BoundSensor(_sensor, _min, _max) { \
if ( _sensor->ve[AXIS_X] < _min) _sensor->ve[AXIS_X] = _min; \
if ( _sensor->ve[AXIS_X] > _max) _sensor->ve[AXIS_X] = _max; \
if ( _sensor->ve[AXIS_Y] < _min) _sensor->ve[AXIS_Y] = _min; \
if ( _sensor->ve[AXIS_Y] > _max) _sensor->ve[AXIS_Y] = _max; \
if ( _sensor->ve[AXIS_Z] < _min) _sensor->ve[AXIS_Z] = _min; \
if ( _sensor->ve[AXIS_Z] > _max) _sensor->ve[AXIS_Z] = _max; \
}
#define CopyVect(_dest, _src) { \
_dest->ve[AXIS_X] = _src->ve[AXIS_X]; \
_dest->ve[AXIS_Y] = _src->ve[AXIS_Y]; \
_dest->ve[AXIS_Z] = _src->ve[AXIS_Z]; \
}
static void booz_sensors_model_accel_run( MAT* dcm ) {
/* */
static VEC* accel_body = VNULL;
accel_body = v_resize(accel_body, AXIS_NB);
accel_body = v_zero(accel_body);
#if 1
/* compute sum of forces in body frame except gravity */
/* square of prop rotational speeds */
static VEC *omega_square = VNULL;
omega_square = v_resize(omega_square,SERVOS_NB);
BoozFlighModelGetRPMS(omega_square);
omega_square = v_star(omega_square, omega_square, omega_square);
/* extract body speed from state */
static VEC *speed_body = VNULL;
speed_body = v_resize(speed_body, AXIS_NB);
BoozFlighModelGetSpeed(speed_body);
accel_body = booz_get_forces_body_frame(accel_body , dcm, omega_square, speed_body, FALSE);
/* divide by mass */
accel_body = sv_mlt(1./bfm.mass, accel_body, accel_body);
static VEC* g_inert = VNULL;
g_inert = v_resize(g_inert, AXIS_NB);
g_inert->ve[AXIS_X] = 0;
g_inert->ve[AXIS_Y] = 0;
g_inert->ve[AXIS_Z] = 9.81;
static VEC* g_body = VNULL;
g_body = v_resize(g_body, AXIS_NB);
g_body = mv_mlt(dcm, g_inert, g_body);
accel_body = v_sub(accel_body, g_body, accel_body);
//#else
// printf(" accel_body # %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]);
IvySendMsg("148 BOOZ_SIM_WIND %f %f %f",
accel_body->ve[AXIS_X],
accel_body->ve[AXIS_Y],
accel_body->ve[AXIS_Z]);
accel_body = mv_mlt(dcm, g_inert, accel_body);
accel_body = sv_mlt(-1., accel_body, accel_body);
// printf(" accel_body ~ %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]);
#endif
/* compute accel reading */
bsm.accel = mv_mlt(bsm.accel_sensitivity, accel_body, bsm.accel);
bsm.accel = v_add(bsm.accel, bsm.accel_neutral, bsm.accel);
/* compute accel error readings */
static VEC *accel_error = VNULL;
accel_error = v_resize(accel_error, AXIS_NB);
accel_error = v_zero(accel_error);
/* add a gaussian noise */
accel_error = v_add_gaussian_noise(accel_error, bsm.accel_noise_std_dev, accel_error);
/* constant bias */
accel_error = v_add(accel_error, bsm.accel_bias, accel_error);
/* scale to adc units FIXME : should use full adc gain ? sum ? */
accel_error->ve[AXIS_X] = accel_error->ve[AXIS_X] * bsm.accel_sensitivity->me[AXIS_X][AXIS_X];
accel_error->ve[AXIS_Y] = accel_error->ve[AXIS_Y] * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y];
accel_error->ve[AXIS_Z] = accel_error->ve[AXIS_Z] * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z];
/* add per accel error reading */
bsm.accel = v_add(bsm.accel, accel_error, bsm.accel);
/* round signal to account for adc discretisation */
RoundSensor(bsm.accel);
/* saturation */
BoundSensor(bsm.accel, 0, bsm.accel_resolution);
// printf("sim adc %f %f %f\n",bsm.accel->ve[AXIS_X] ,bsm.accel->ve[AXIS_Y] ,bsm.accel->ve[AXIS_Z]);
}
static void booz_sensors_model_gyro_run( double dt ) {
/* extract rotational speed from flight model state */
static VEC *rate_body = VNULL;
rate_body = v_resize(rate_body, AXIS_NB);
rate_body->ve[AXIS_P] = bfm.state->ve[BFMS_P];
rate_body->ve[AXIS_Q] = bfm.state->ve[BFMS_Q];
rate_body->ve[AXIS_R] = bfm.state->ve[BFMS_R];
/* compute gyros readings */
bsm.gyro = mv_mlt(bsm.gyro_sensitivity, rate_body, bsm.gyro);
bsm.gyro = v_add(bsm.gyro, bsm.gyro_neutral, bsm.gyro);
/* 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, 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);
/* round signal to account for adc discretisation */
RoundSensor(bsm.gyro);
/* saturation */
BoundSensor(bsm.gyro, 0, bsm.gyro_resolution);
}
static void booz_sensors_model_mag_run( MAT* dcm ) {
/* rotate h to body frame */
static VEC *h_body = VNULL;
h_body = v_resize(h_body, AXIS_NB);
h_body = mv_mlt(dcm, bfm.h_earth, h_body);
bsm.mag = mv_mlt(bsm.mag_sensitivity, h_body, bsm.mag);
bsm.mag = v_add(bsm.mag, bsm.mag_neutral, bsm.mag);
/* compute mag error readings */
static VEC *mag_error = VNULL;
mag_error = v_resize(mag_error, AXIS_NB);
mag_error = v_zero(mag_error);
/* add a gaussian noise */
mag_error = v_add_gaussian_noise(mag_error, bsm.mag_noise_std_dev, mag_error);
mag_error->ve[AXIS_X] = mag_error->ve[AXIS_X] * bsm.mag_sensitivity->me[AXIS_X][AXIS_X];
mag_error->ve[AXIS_Y] = mag_error->ve[AXIS_Y] * bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y];
mag_error->ve[AXIS_Z] = mag_error->ve[AXIS_Z] * bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z];
/* add error */
bsm.mag = v_add(bsm.mag, mag_error, bsm.mag);
// printf("h body %f %f %f\n", h_body->ve[AXIS_X], h_body->ve[AXIS_Y], h_body->ve[AXIS_Z]);
// printf("mag %f %f %f\n", bsm.mag->ve[AXIS_X], bsm.mag->ve[AXIS_Y], bsm.mag->ve[AXIS_Z]);
/* round signal to account for adc discretisation */
RoundSensor(bsm.mag);
}
static void booz_sensors_model_range_meter_run() {
double dz = bfm.state->ve[BFMS_Z];
if (dz > 0.) dz = 0.;
double dx = dz * tan(bfm.state->ve[BFMS_THETA]);
double dy = dz * tan(bfm.state->ve[BFMS_PHI]);
double dist = sqrt( dx*dx + dy*dy + dz*dz);
dist *= bsm.range_meter_sensivity;
/* add gaussian noise */
if (dist > BSM_RANGE_METER_MAX_RANGE)
dist = BSM_RANGE_METER_MAX_RANGE;
dist = rint(dist);
bsm.range_meter = dist;
}
static void booz_sensors_model_gps_run( double dt, MAT* dcm_t ) {
/* simulate speed sensor */
/* extract body speed from state */
static VEC *speed_body = VNULL;
speed_body = v_resize(speed_body, AXIS_NB);
BoozFlighModelGetSpeed(speed_body);
static VEC *cur_speed_reading = VNULL;
cur_speed_reading = v_resize(cur_speed_reading, AXIS_NB);
/* convert to earth frame */
cur_speed_reading = mv_mlt(dcm_t, speed_body, cur_speed_reading);
/* add a gaussian noise */
cur_speed_reading = v_add_gaussian_noise(cur_speed_reading, bsm.speed_noise_std_dev,
cur_speed_reading);
UpdateSensorLatency(cur_speed_reading, bsm.speed_history, bsm.speed_latency, bsm.speed_sensor);
/* simulate position sensor */
static VEC *cur_pos_reading = VNULL;
cur_pos_reading = v_resize(cur_pos_reading, AXIS_NB);
/* extract pos from state */
BoozFlighModelGetPos(cur_pos_reading);
/* compute position error reading */
static VEC *pos_error = VNULL;
pos_error = v_resize(pos_error, AXIS_NB);
pos_error = v_zero(pos_error);
/* add a gaussian noise */
pos_error = v_add_gaussian_noise(pos_error, bsm.pos_noise_std_dev, pos_error);
/* update random walk bias */
bsm.pos_bias_random_walk_value =
v_update_random_walk(bsm.pos_bias_random_walk_value,
bsm.pos_bias_random_walk_std_dev, dt,
bsm.pos_bias_random_walk_value);
pos_error = v_add(pos_error, bsm.pos_bias_random_walk_value, pos_error);
/* add error reading */
cur_pos_reading = v_add(cur_pos_reading, pos_error, cur_pos_reading);
UpdateSensorLatency(cur_pos_reading, bsm.pos_history, bsm.pos_latency, bsm.pos_sensor);
}
static void UpdateSensorLatency(VEC* cur_reading, GSList* history,
double latency, VEC* sensor_reading) {
/* add new reading */
struct BoozDatedSensor* cur_read = g_new(struct BoozDatedSensor, 1);
cur_read->time = bfm.time;
cur_read->value = v_get(AXIS_NB);
CopyVect(cur_read->value, cur_reading);
history = g_slist_prepend(history, cur_read);
/* remove old readings */
GSList* last = g_slist_last(history);
while (last &&
((struct BoozDatedSensor*)last->data)->time < bfm.time - latency) {
history = g_slist_remove_link(history, last);
v_free(((struct BoozDatedSensor*)last->data)->value);
g_free((struct BoozDatedSensor*)last->data);
g_slist_free(last);
last = g_slist_last(history);
}
/* update sensor */
CopyVect(sensor_reading, ((struct BoozDatedSensor*)last->data)->value);
}
static VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* out) {
static VEC *tmp = VNULL;
tmp = v_resize(tmp, AXIS_NB);
tmp = sv_mlt(dt, std_dev, tmp);
out = v_add_gaussian_noise(in, tmp, out);
return out;
}
static VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out) {
static VEC *tmp = VNULL;
tmp = v_resize(tmp, AXIS_NB);
tmp = v_rand(tmp);
static VEC *one = VNULL;
one = v_resize(one, AXIS_NB);
one = v_ones(one);
tmp = v_mltadd(one, tmp, -2., tmp);
tmp = v_star(tmp, std_dev, tmp);
out = v_add(in, tmp, out);
return out;
}
sw/simulator/booz_sensors_model.h
+59 -38
View File
@@ -1,63 +1,84 @@
#ifndef BOOZ_SENSORS_MODEL_H
#define BOOZ_SENSORS_MODEL_H
#include "6dof.h"
#include <matrix.h>
#include <glib.h>
extern void booz_sensors_model_init(void);
extern void booz_sensors_model_run( double dt);
#include "6dof.h"
#include "std.h"
extern void booz_sensors_model_init(double time);
extern void booz_sensors_model_run( double time);
extern bool_t booz_sensors_model_accel_available();
extern bool_t booz_sensors_model_gyro_available();
extern bool_t booz_sensors_model_mag_available();
extern bool_t booz_sensors_model_baro_available();
extern bool_t booz_sensors_model_gps_available();
struct BoozSensorsModel {
VEC* accel;
/* Accelerometer */
VEC* accel;
unsigned int accel_resolution;
MAT* accel_sensitivity;
VEC* accel_neutral;
VEC* accel_noise_std_dev;
VEC* accel_bias;
MAT* accel_sensitivity;
VEC* accel_neutral;
VEC* accel_noise_std_dev;
VEC* accel_bias;
double accel_next_update;
int accel_available;
VEC* gyro;
/* Gyrometer */
VEC* gyro;
unsigned int gyro_resolution;
MAT* gyro_sensitivity;
VEC* gyro_neutral;
VEC* gyro_noise_std_dev;
VEC* gyro_bias_initial;
VEC* gyro_bias_random_walk_std_dev;
VEC* gyro_bias_random_walk_value;
MAT* gyro_sensitivity;
VEC* gyro_neutral;
VEC* gyro_noise_std_dev;
VEC* gyro_bias_initial;
VEC* gyro_bias_random_walk_std_dev;
VEC* gyro_bias_random_walk_value;
double gyro_next_update;
int gyro_available;
VEC* mag;
/* Magnetometer */
VEC* mag;
unsigned int mag_resolution;
MAT* mag_sensitivity;
VEC* mag_neutral;
VEC* mag_noise_std_dev;
MAT* mag_sensitivity;
VEC* mag_neutral;
VEC* mag_noise_std_dev;
double mag_next_update;
int mag_available;
double range_meter;
unsigned int range_meter_resolution;
double range_meter_sensivity;
/* Rangemeter */
double rangemeter;
double rangemeter_next_update;
int rangemeter_available;
/* Barometer */
double baro;
unsigned int baro_resolution;
double baro_next_update;
int baro_available;
/* imaginary sensors - gps maybe */
VEC* speed_sensor;
VEC* speed_noise_std_dev;
double speed_latency;
GSList* speed_history;
VEC* pos_sensor;
VEC* pos_noise_std_dev;
VEC* pos_bias_initial;
VEC* pos_bias_random_walk_std_dev;
VEC* pos_bias_random_walk_value;
double pos_latency;
GSList* pos_history;
/* GPS */
VEC* gps_speed;
VEC* gps_speed_noise_std_dev;
GSList* gps_speed_history;
VEC* gps_pos;
VEC* gps_pos_noise_std_dev;
VEC* gps_pos_bias_initial;
VEC* gps_pos_bias_random_walk_std_dev;
VEC* gps_pos_bias_random_walk_value;
GSList* gps_pos_history;
double gps_next_update;
int gps_available;
};
extern struct BoozSensorsModel bsm;
struct BoozDatedSensor {
VEC* value;
double time;
};
#endif /* BOOZ_SENSORS_MODEL_H */
sw/simulator/booz_sensors_model_accel.c
+130
View File
@@ -0,0 +1,130 @@
#include "booz_sensors_model.h"
#include "booz_sensors_model_params.h"
#include "booz_sensors_model_utils.h"
#include "booz_flight_model.h"
bool_t booz_sensors_model_accel_available() {
if (bsm.accel_available) {
bsm.accel_available = FALSE;
return TRUE;
}
return FALSE;
}
void booz_sensors_model_accel_init(double time) {
bsm.accel = v_get(AXIS_NB);
bsm.accel->ve[AXIS_X] = 0.;
bsm.accel->ve[AXIS_Y] = 0.;
bsm.accel->ve[AXIS_Z] = 0.;
bsm.accel_resolution = BSM_ACCEL_RESOLUTION;
bsm.accel_sensitivity = m_get(AXIS_NB, AXIS_NB);
m_zero(bsm.accel_sensitivity);
bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = BSM_ACCEL_SENSITIVITY_XX;
bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = BSM_ACCEL_SENSITIVITY_YY;
bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = BSM_ACCEL_SENSITIVITY_ZZ;
bsm.accel_neutral = v_get(AXIS_NB);
bsm.accel_neutral->ve[AXIS_X] = BSM_ACCEL_NEUTRAL_X;
bsm.accel_neutral->ve[AXIS_Y] = BSM_ACCEL_NEUTRAL_Y;
bsm.accel_neutral->ve[AXIS_Z] = BSM_ACCEL_NEUTRAL_Z;
bsm.accel_noise_std_dev = v_get(AXIS_NB);
bsm.accel_noise_std_dev->ve[AXIS_X] = BSM_ACCEL_NOISE_STD_DEV_X;
bsm.accel_noise_std_dev->ve[AXIS_Y] = BSM_ACCEL_NOISE_STD_DEV_Y;
bsm.accel_noise_std_dev->ve[AXIS_Z] = BSM_ACCEL_NOISE_STD_DEV_Z;
bsm.accel_bias = v_get(AXIS_NB);
bsm.accel_bias->ve[AXIS_P] = BSM_ACCEL_BIAS_X;
bsm.accel_bias->ve[AXIS_Q] = BSM_ACCEL_BIAS_Y;
bsm.accel_bias->ve[AXIS_R] = BSM_ACCEL_BIAS_Z;
bsm.accel_next_update = time;
bsm.accel_available = FALSE;
}
void booz_sensors_model_accel_run( double time, MAT* dcm ) {
if (time < bsm.accel_next_update)
return;
/* */
static VEC* accel_body = VNULL;
accel_body = v_resize(accel_body, AXIS_NB);
accel_body = v_zero(accel_body);
#if 1
/* compute sum of forces in body frame except gravity */
/* square of prop rotational speeds */
static VEC *omega_square = VNULL;
omega_square = v_resize(omega_square,SERVOS_NB);
BoozFlighModelGetRPMS(omega_square);
omega_square = v_star(omega_square, omega_square, omega_square);
/* extract body speed from state */
static VEC *speed_body = VNULL;
speed_body = v_resize(speed_body, AXIS_NB);
BoozFlighModelGetSpeed(speed_body);
accel_body = booz_get_forces_body_frame(accel_body , dcm, omega_square, speed_body, FALSE);
/* divide by mass */
accel_body = sv_mlt(1./bfm.mass, accel_body, accel_body);
static VEC* g_inert = VNULL;
g_inert = v_resize(g_inert, AXIS_NB);
g_inert->ve[AXIS_X] = 0;
g_inert->ve[AXIS_Y] = 0;
g_inert->ve[AXIS_Z] = 9.81;
static VEC* g_body = VNULL;
g_body = v_resize(g_body, AXIS_NB);
g_body = mv_mlt(dcm, g_inert, g_body);
accel_body = v_sub(accel_body, g_body, accel_body);
//#else
// printf(" accel_body # %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]);
#if 0
IvySendMsg("148 BOOZ_SIM_WIND %f %f %f",
accel_body->ve[AXIS_X],
accel_body->ve[AXIS_Y],
accel_body->ve[AXIS_Z]);
#endif
accel_body = mv_mlt(dcm, g_inert, accel_body);
accel_body = sv_mlt(-1., accel_body, accel_body);
// printf(" accel_body ~ %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]);
#endif
/* compute accel reading */
bsm.accel = mv_mlt(bsm.accel_sensitivity, accel_body, bsm.accel);
bsm.accel = v_add(bsm.accel, bsm.accel_neutral, bsm.accel);
/* compute accel error readings */
static VEC *accel_error = VNULL;
accel_error = v_resize(accel_error, AXIS_NB);
accel_error = v_zero(accel_error);
/* add a gaussian noise */
accel_error = v_add_gaussian_noise(accel_error, bsm.accel_noise_std_dev, accel_error);
/* constant bias */
accel_error = v_add(accel_error, bsm.accel_bias, accel_error);
/* scale to adc units FIXME : should use full adc gain ? sum ? */
accel_error->ve[AXIS_X] = accel_error->ve[AXIS_X] * bsm.accel_sensitivity->me[AXIS_X][AXIS_X];
accel_error->ve[AXIS_Y] = accel_error->ve[AXIS_Y] * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y];
accel_error->ve[AXIS_Z] = accel_error->ve[AXIS_Z] * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z];
/* add per accel error reading */
bsm.accel = v_add(bsm.accel, accel_error, bsm.accel);
/* round signal to account for adc discretisation */
RoundSensor(bsm.accel);
/* saturation */
BoundSensor(bsm.accel, 0, bsm.accel_resolution);
// printf("sim adc %f %f %f\n",bsm.accel->ve[AXIS_X] ,bsm.accel->ve[AXIS_Y] ,bsm.accel->ve[AXIS_Z]);
bsm.accel_next_update += BSM_ACCEL_DT;
bsm.accel_available = TRUE;
}
sw/simulator/booz_sensors_model_baro.c
+37
View File
@@ -0,0 +1,37 @@
#include "booz_sensors_model.h"
#include "booz_sensors_model_params.h"
#include "booz_sensors_model_utils.h"
#include "booz_flight_model.h"
bool_t booz_sensors_model_baro_available() {
if (bsm.baro_available) {
bsm.baro_available = FALSE;
return TRUE;
}
return FALSE;
}
void booz_sensors_model_baro_init( double time ) {
bsm.baro = 0.;
bsm.baro_next_update = time;
bsm.baro_available = FALSE;
}
void booz_sensors_model_baro_run( double time ) {
if (time < bsm.baro_next_update)
return;
double z = bfm.state->ve[BFMS_Z];
double p = ( z / 0.084 ) + BSM_BARO_QNH;
double baro_reading = p * BSM_BARO_SENSITIVITY;
/* FIXME : add noise and random walk */
baro_reading = rint(baro_reading);
bsm.baro = baro_reading;
bsm.baro_next_update += BSM_BARO_DT;
bsm.baro_available = TRUE;
}
sw/simulator/booz_sensors_model_gps.c
+110
View File
@@ -0,0 +1,110 @@
#include "booz_sensors_model.h"
#include "booz_sensors_model_params.h"
#include "booz_sensors_model_utils.h"
#include "booz_flight_model.h"
bool_t booz_sensors_model_gps_available() {
if (bsm.gps_available) {
bsm.gps_available = FALSE;
return TRUE;
}
return FALSE;
}
void booz_sensors_model_gps_init( double time ) {
bsm.gps_speed = v_get(AXIS_NB);
v_zero(bsm.gps_speed);
bsm.gps_speed_noise_std_dev = v_get(AXIS_NB);
bsm.gps_speed_noise_std_dev->ve[AXIS_X] = BSM_GPS_SPEED_NOISE_STD_DEV;
bsm.gps_speed_noise_std_dev->ve[AXIS_Y] = BSM_GPS_SPEED_NOISE_STD_DEV;
bsm.gps_speed_noise_std_dev->ve[AXIS_Z] = BSM_GPS_SPEED_NOISE_STD_DEV;
bsm.gps_speed_history = NULL;
bsm.gps_pos = v_get(AXIS_NB);
v_zero(bsm.gps_pos);
bsm.gps_pos_noise_std_dev = v_get(AXIS_NB);
bsm.gps_pos_noise_std_dev->ve[AXIS_X] = BSM_GPS_POS_NOISE_STD_DEV;
bsm.gps_pos_noise_std_dev->ve[AXIS_Y] = BSM_GPS_POS_NOISE_STD_DEV;
bsm.gps_pos_noise_std_dev->ve[AXIS_Z] = BSM_GPS_POS_NOISE_STD_DEV;
bsm.gps_pos_bias_initial = v_get(AXIS_NB);
bsm.gps_pos_bias_initial->ve[AXIS_X] = BSM_GPS_POS_BIAS_INITIAL_X;
bsm.gps_pos_bias_initial->ve[AXIS_Y] = BSM_GPS_POS_BIAS_INITIAL_Y;
bsm.gps_pos_bias_initial->ve[AXIS_Z] = BSM_GPS_POS_BIAS_INITIAL_Z;
bsm.gps_pos_bias_random_walk_std_dev = v_get(AXIS_NB);
bsm.gps_pos_bias_random_walk_std_dev->ve[AXIS_X] = BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X;
bsm.gps_pos_bias_random_walk_std_dev->ve[AXIS_Y] = BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y;
bsm.gps_pos_bias_random_walk_std_dev->ve[AXIS_Z] = BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z;
bsm.gps_pos_bias_random_walk_value = v_get(AXIS_NB);
bsm.gps_pos_bias_random_walk_value->ve[AXIS_X] = bsm.gps_pos_bias_initial->ve[AXIS_X];
bsm.gps_pos_bias_random_walk_value->ve[AXIS_Y] = bsm.gps_pos_bias_initial->ve[AXIS_Y];
bsm.gps_pos_bias_random_walk_value->ve[AXIS_Z] = bsm.gps_pos_bias_initial->ve[AXIS_Z];
bsm.gps_pos_history = NULL;
bsm.gps_next_update = time;
bsm.gps_available = FALSE;
}
void booz_sensors_model_gps_run( double time, MAT* dcm_t ) {
if (time < bsm.gps_next_update)
return;
/*
* simulate speed sensor
*/
/* extract body speed from state */
static VEC *speed_body = VNULL;
speed_body = v_resize(speed_body, AXIS_NB);
BoozFlighModelGetSpeed(speed_body);
static VEC *cur_speed_reading = VNULL;
cur_speed_reading = v_resize(cur_speed_reading, AXIS_NB);
/* convert to earth frame */
cur_speed_reading = mv_mlt(dcm_t, speed_body, cur_speed_reading);
/* add a gaussian noise */
cur_speed_reading = v_add_gaussian_noise(cur_speed_reading, bsm.gps_speed_noise_std_dev,
cur_speed_reading);
UpdateSensorLatency(time, cur_speed_reading, bsm.gps_speed_history, BSM_GPS_SPEED_LATENCY, bsm.gps_speed);
/*
* simulate position sensor
*/
static VEC *cur_pos_reading = VNULL;
cur_pos_reading = v_resize(cur_pos_reading, AXIS_NB);
/* extract pos from state */
BoozFlighModelGetPos(cur_pos_reading);
/* compute position error reading */
static VEC *pos_error = VNULL;
pos_error = v_resize(pos_error, AXIS_NB);
pos_error = v_zero(pos_error);
/* add a gaussian noise */
pos_error = v_add_gaussian_noise(pos_error, bsm.gps_pos_noise_std_dev, pos_error);
/* update random walk bias */
bsm.gps_pos_bias_random_walk_value =
v_update_random_walk(bsm.gps_pos_bias_random_walk_value,
bsm.gps_pos_bias_random_walk_std_dev, BSM_GPS_DT,
bsm.gps_pos_bias_random_walk_value);
pos_error = v_add(pos_error, bsm.gps_pos_bias_random_walk_value, pos_error);
/* add error reading */
cur_pos_reading = v_add(cur_pos_reading, pos_error, cur_pos_reading);
UpdateSensorLatency(time, cur_pos_reading, bsm.gps_pos_history, BSM_GPS_POS_LATENCY, bsm.gps_pos);
bsm.gps_next_update += BSM_GPS_DT;
bsm.gps_available = TRUE;
}
sw/simulator/booz_sensors_model_gyro.c
+104
View File
@@ -0,0 +1,104 @@
#include "booz_sensors_model.h"
#include "booz_sensors_model_params.h"
#include "booz_sensors_model_utils.h"
#include "booz_flight_model.h"
bool_t booz_sensors_model_gyro_available() {
if (bsm.gyro_available) {
bsm.gyro_available = FALSE;
return TRUE;
}
return FALSE;
}
void booz_sensors_model_gyro_init(double time) {
bsm.gyro = v_get(AXIS_NB);
bsm.gyro->ve[AXIS_P] = 0.;
bsm.gyro->ve[AXIS_Q] = 0.;
bsm.gyro->ve[AXIS_R] = 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;
}
void booz_sensors_model_gyro_run( double time ) {
if (time < bsm.gyro_next_update)
return;
/* extract rotational speed from flight model state */
static VEC *rate_body = VNULL;
rate_body = v_resize(rate_body, AXIS_NB);
rate_body->ve[AXIS_P] = bfm.state->ve[BFMS_P];
rate_body->ve[AXIS_Q] = bfm.state->ve[BFMS_Q];
rate_body->ve[AXIS_R] = bfm.state->ve[BFMS_R];
/* compute gyros readings */
bsm.gyro = mv_mlt(bsm.gyro_sensitivity, rate_body, bsm.gyro);
bsm.gyro = v_add(bsm.gyro, bsm.gyro_neutral, bsm.gyro);
/* 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);
/* round signal to account for adc discretisation */
RoundSensor(bsm.gyro);
/* saturation */
BoundSensor(bsm.gyro, 0, bsm.gyro_resolution);
bsm.gyro_next_update += BSM_GYRO_DT;
bsm.gyro_available = TRUE;
}
sw/simulator/booz_sensors_model_mag.c
+79
View File
@@ -0,0 +1,79 @@
#include "booz_sensors_model.h"
#include "booz_sensors_model_params.h"
#include "booz_sensors_model_utils.h"
#include "booz_flight_model.h"
bool_t booz_sensors_model_mag_available() {
if (bsm.mag_available) {
bsm.mag_available = FALSE;
return TRUE;
}
return FALSE;
}
void booz_sensors_model_mag_init( double time ) {
bsm.mag = v_get(AXIS_NB);
bsm.mag->ve[AXIS_X] = 0.;
bsm.mag->ve[AXIS_Y] = 0.;
bsm.mag->ve[AXIS_Z] = 0.;
bsm.mag_resolution = 4096;
bsm.mag_sensitivity = m_get(AXIS_NB, AXIS_NB);
m_zero(bsm.mag_sensitivity);
bsm.mag_sensitivity->me[AXIS_X][AXIS_X] = -(double)bsm.mag_resolution / 6.;
bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y] = -(double)bsm.mag_resolution / 6.;
bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z] = (double)bsm.mag_resolution / 6.;
bsm.mag_neutral = v_get(AXIS_NB);
bsm.mag_neutral->ve[AXIS_X] = 0.;
bsm.mag_neutral->ve[AXIS_Y] = 0.;
bsm.mag_neutral->ve[AXIS_Z] = 0.;
bsm.mag_noise_std_dev = v_get(AXIS_NB);
bsm.mag_noise_std_dev->ve[AXIS_X] = 2e-2;
bsm.mag_noise_std_dev->ve[AXIS_Y] = 2e-2;
bsm.mag_noise_std_dev->ve[AXIS_Z] = 2e-2;
bsm.mag_next_update = time;
bsm.mag_available = FALSE;
}
void booz_sensors_model_mag_run( double time, MAT* dcm ) {
if (time < bsm.mag_next_update)
return;
/* rotate h to body frame */
static VEC *h_body = VNULL;
h_body = v_resize(h_body, AXIS_NB);
h_body = mv_mlt(dcm, bfm.h_earth, h_body);
bsm.mag = mv_mlt(bsm.mag_sensitivity, h_body, bsm.mag);
bsm.mag = v_add(bsm.mag, bsm.mag_neutral, bsm.mag);
/* compute mag error readings */
static VEC *mag_error = VNULL;
mag_error = v_resize(mag_error, AXIS_NB);
mag_error = v_zero(mag_error);
/* add a gaussian noise */
mag_error = v_add_gaussian_noise(mag_error, bsm.mag_noise_std_dev, mag_error);
mag_error->ve[AXIS_X] = mag_error->ve[AXIS_X] * bsm.mag_sensitivity->me[AXIS_X][AXIS_X];
mag_error->ve[AXIS_Y] = mag_error->ve[AXIS_Y] * bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y];
mag_error->ve[AXIS_Z] = mag_error->ve[AXIS_Z] * bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z];
/* add error */
bsm.mag = v_add(bsm.mag, mag_error, bsm.mag);
// printf("h body %f %f %f\n", h_body->ve[AXIS_X], h_body->ve[AXIS_Y], h_body->ve[AXIS_Z]);
// printf("mag %f %f %f\n", bsm.mag->ve[AXIS_X], bsm.mag->ve[AXIS_Y], bsm.mag->ve[AXIS_Z]);
/* round signal to account for adc discretisation */
RoundSensor(bsm.mag);
bsm.mag_next_update += BSM_MAG_DT;
bsm.mag_available = TRUE;
}
sw/simulator/booz_sensors_model_params.h
+27 -9
View File
@@ -44,6 +44,8 @@
#define BSM_ACCEL_BIAS_X 1e-3
#define BSM_ACCEL_BIAS_Y 1e-3
#define BSM_ACCEL_BIAS_Z 1e-3
/* s */
#define BSM_ACCEL_DT (1./250.)
@@ -73,39 +75,55 @@
#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(.5)
#define BSM_GYRO_BIAS_INITIAL_P RadOfDeg( .5)
//#define BSM_GYRO_BIAS_INITIAL_Q RadOfDeg(-0.5)
#define BSM_GYRO_BIAS_INITIAL_Q RadOfDeg(0.)
#define BSM_GYRO_BIAS_INITIAL_Q RadOfDeg(-0.5)
#define BSM_GYRO_BIAS_INITIAL_R RadOfDeg( .25)
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(5.e-1)
//#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(5.e-1)
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0)
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(5.e-1)
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(5.e-1)
#define BSM_GYRO_DT (1./250.)
/*
* Magnetometer
*/
#define BSM_MAG_DT (1./20.)
/*
* Range meter
*/
#define BSM_RANGE_METER_RESOLUTION (1024)
#define BSM_RANGE_METER_SENSITIVITY (1024. / 12.)
#define BSM_RANGE_METER_MAX_RANGE (6. * BSM_RANGE_METER_SENSITIVITY)
#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
*/
#define BSM_BARO_QNH 101300
#define BSM_BARO_SENSITIVITY 4.
#define BSM_BARO_DT (1./10.)
/*
* GPS
*/
#define BSM_GPS_SPEED_NOISE_STD_DEV 1e-1
#define BSM_GPS_SPEED_LATENCY 0.25
#define BSM_GPS_POS_NOISE_STD_DEV 3e-1
#define BSM_GPS_POS_BIAS_INITIAL_X 1e-1
#define BSM_GPS_POS_BIAS_INITIAL_Y -1e-1
#define BSM_GPS_POS_BIAS_INITIAL_Z -5e-1
#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-1
#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-1
#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-1
#define BSM_GPS_POS_LATENCY 0.25
#define BSM_GPS_DT (1./4.)
#endif /* BOOZ_SENSORS_MODEL_PARAMS_H */
sw/simulator/booz_sensors_model_rangemeter.c
+47
View File
@@ -0,0 +1,47 @@
#include "booz_sensors_model.h"
#include "booz_sensors_model_params.h"
#include "booz_sensors_model_utils.h"
#include "booz_flight_model.h"
bool_t booz_sensors_model_rangemeter_available() {
if (bsm.rangemeter_available) {
bsm.rangemeter_available = FALSE;
return TRUE;
}
return FALSE;
}
void booz_sensors_model_rangemeter_init(double time) {
bsm.rangemeter = 0.;
bsm.rangemeter_next_update = time;
bsm.rangemeter_available = FALSE;
}
void booz_sensors_model_rangemeter_run( double time ) {
if (time < bsm.rangemeter_next_update)
return;
/* compute dist from ground */
double dz = bfm.state->ve[BFMS_Z];
if (dz > 0.) dz = 0.;
double dx = dz * tan(bfm.state->ve[BFMS_THETA]);
double dy = dz * tan(bfm.state->ve[BFMS_PHI]);
double dist = sqrt( dx*dx + dy*dy + dz*dz);
dist *= BSM_RANGEMETER_SENSITIVITY;
/* add gaussian noise */
if (dist > BSM_RANGEMETER_MAX_RANGE)
dist = BSM_RANGEMETER_MAX_RANGE;
dist = rint(dist);
bsm.rangemeter = dist;
bsm.rangemeter_next_update += BSM_RANGEMETER_DT;
bsm.rangemeter_available = TRUE;
}
sw/simulator/booz_sensors_model_utils.c
+47
View File
@@ -0,0 +1,47 @@
#include "booz_sensors_model_utils.h"
#include "6dof.h"
void UpdateSensorLatency(double time, VEC* cur_reading, GSList* history,
double latency, VEC* sensor_reading) {
/* add new reading */
struct BoozDatedSensor* cur_read = g_new(struct BoozDatedSensor, 1);
cur_read->time = time;
cur_read->value = v_get(AXIS_NB);
CopyVect(cur_read->value, cur_reading);
history = g_slist_prepend(history, cur_read);
/* remove old readings */
GSList* last = g_slist_last(history);
while (last &&
((struct BoozDatedSensor*)last->data)->time < time - latency) {
history = g_slist_remove_link(history, last);
v_free(((struct BoozDatedSensor*)last->data)->value);
g_free((struct BoozDatedSensor*)last->data);
g_slist_free(last);
last = g_slist_last(history);
}
/* update sensor */
CopyVect(sensor_reading, ((struct BoozDatedSensor*)last->data)->value);
}
VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* out) {
static VEC *tmp = VNULL;
tmp = v_resize(tmp, AXIS_NB);
tmp = sv_mlt(dt, std_dev, tmp);
out = v_add_gaussian_noise(in, tmp, out);
return out;
}
VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out) {
static VEC *tmp = VNULL;
tmp = v_resize(tmp, AXIS_NB);
tmp = v_rand(tmp);
static VEC *one = VNULL;
one = v_resize(one, AXIS_NB);
one = v_ones(one);
tmp = v_mltadd(one, tmp, -2., tmp);
tmp = v_star(tmp, std_dev, tmp);
out = v_add(in, tmp, out);
return out;
}
sw/simulator/booz_sensors_model_utils.h
+41
View File
@@ -0,0 +1,41 @@
#ifndef BOOZ_SENSORS_MODEL_UTILS_H
#define BOOZ_SENSORS_MODEL_UTILS_H
#include <matrix.h>
#include <glib.h>
struct BoozDatedSensor {
VEC* value;
double time;
};
extern void UpdateSensorLatency(double time, VEC* cur_reading, GSList* history,
double latency, VEC* sensor_reading);
extern VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* out);
extern VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out);
#define RoundSensor(_sensor) { \
_sensor->ve[AXIS_X] = rint(_sensor->ve[AXIS_X]); \
_sensor->ve[AXIS_Y] = rint(_sensor->ve[AXIS_Y]); \
_sensor->ve[AXIS_Z] = rint(_sensor->ve[AXIS_Z]); \
}
#define BoundSensor(_sensor, _min, _max) { \
if ( _sensor->ve[AXIS_X] < _min) _sensor->ve[AXIS_X] = _min; \
if ( _sensor->ve[AXIS_X] > _max) _sensor->ve[AXIS_X] = _max; \
if ( _sensor->ve[AXIS_Y] < _min) _sensor->ve[AXIS_Y] = _min; \
if ( _sensor->ve[AXIS_Y] > _max) _sensor->ve[AXIS_Y] = _max; \
if ( _sensor->ve[AXIS_Z] < _min) _sensor->ve[AXIS_Z] = _min; \
if ( _sensor->ve[AXIS_Z] > _max) _sensor->ve[AXIS_Z] = _max; \
}
#define CopyVect(_dest, _src) { \
_dest->ve[AXIS_X] = _src->ve[AXIS_X]; \
_dest->ve[AXIS_Y] = _src->ve[AXIS_Y]; \
_dest->ve[AXIS_Z] = _src->ve[AXIS_Z]; \
}
#endif /* BOOZ_SENSORS_MODEL_UTILS_H */
sw/simulator/main_booz_sim.c
+21 -5
View File
@@ -100,18 +100,34 @@ static gboolean booz_sim_periodic(gpointer data __attribute__ ((unused))) {
booz_filter_main_periodic_task();
/* feed sensors */
max1167_hw_feed_value(sim_time, bsm.gyro, bsm.accel);
micromag_hw_feed_value(sim_time, bsm.mag);
if (booz_sensor_model_gyro_available()) {
max1167_hw_feed_value(sim_time, bsm.gyro, bsm.accel);
booz_filter_main_event_task();
}
if (booz_sensor_model_mag_available()) {
micromag_hw_feed_value(sim_time, bsm.mag);
booz_filter_main_event_task();
}
if (booz_sensor_model_baro_available()) {
scp1000_hw_feed_value(sim_time, bsm.baro);
booz_filter_main_event_task();
}
if (booz_sensor_model_gps_available()) {
// scp1000_hw_feed_value(sim_time, bsm.baro);
booz_filter_main_event_task();
}
/* process sensors events */
/* it will run the filter and the inter-process communication which */
/* will post a BoozLinkMcuEvent in the Controller process */
booz_filter_main_event_task();
booz_filter_main_event_task();
/* process the BoozLinkMcuEvent */
/* this will update the controller estimator */
booz_controller_main_event_task();
#if 0
/* cheat in simulation : psi not available from filter yet */
// booz_estimator_set_psi(bfm.state->ve[BFMS_PSI]);
/* in simulation compute dcm as a helper for for nav */
@@ -123,7 +139,7 @@ static gboolean booz_sim_periodic(gpointer data __attribute__ ((unused))) {
bsm.pos_sensor->ve[AXIS_X],
bsm.pos_sensor->ve[AXIS_Y],
bsm.pos_sensor->ve[AXIS_Z] );
#endif
/* post a radio control event */
booz_sim_set_ppm_from_joystick();
sw/simulator/tests/Makefile
+47
View File
@@ -0,0 +1,47 @@
CFLAGS = -Wall \
-I .. \
-I ../../../var/BOOZ \
-I ../../airborne \
-I ../../include \
-I /usr/include/meschach \
`pkg-config glib-2.0 --cflags` \
LDFLAGS = -lm \
-lmeschach \
-lpcre \
-lglibivy \
`pkg-config glib-2.0 --libs` \
SIMDIR = ..
#
#
#
TEST_FDM_SRCS = test_fdm.c \
$(SIMDIR)/booz_flight_model.c \
$(SIMDIR)/booz_flight_model_utils.c
test_fdm : $(TEST_FDM_SRCS)
gcc $(CFLAGS) -o $@ $^ $(LDFLAGS)
#
#
#
TEST_SENSORS_SRCS = test_sensors.c \
$(SIMDIR)/booz_flight_model.c \
$(SIMDIR)/booz_flight_model_utils.c \
$(SIMDIR)/booz_sensors_model.c \
$(SIMDIR)/booz_sensors_model_utils.c \
$(SIMDIR)/booz_sensors_model_accel.c \
$(SIMDIR)/booz_sensors_model_gyro.c \
$(SIMDIR)/booz_sensors_model_mag.c \
$(SIMDIR)/booz_sensors_model_rangemeter.c \
$(SIMDIR)/booz_sensors_model_baro.c \
$(SIMDIR)/booz_sensors_model_gps.c \
test_sensors : $(TEST_SENSORS_SRCS)
gcc $(CFLAGS) -o $@ $^ $(LDFLAGS)
sw/simulator/tests/test_fdm.c
+24
View File
@@ -0,0 +1,24 @@
#include "booz_flight_model.h"
#define DT_FDM (1./250.)
#define NB_ITER 100
int main(int argc, char** argv) {
double actuators_values[] = {0.6, 0.6, 0.6, 0.6};
double time = 0.;
booz_flight_model_init();
bfm.on_ground = FALSE;
int i;
for (i=0; i<NB_ITER; i++) {
booz_flight_model_run(DT_FDM, actuators_values);
time += DT_FDM;
printf("%f \t %f\n", time, bfm.state->ve[BFMS_Z]);
}
return 0;
}
sw/simulator/tests/test_sensors.c
+32
View File
@@ -0,0 +1,32 @@
#include "booz_flight_model.h"
#include "booz_sensors_model.h"
#define DT_FDM (1./250.)
#define NB_ITER 100
int main(int argc, char** argv) {
double actuators_values[] = {0.6, 0.6, 0.6, 0.6};
double time = 0.;
booz_flight_model_init();
booz_sensors_model_init(time);
bfm.on_ground = FALSE;
int i;
for (i=0; i<NB_ITER; i++) {
booz_flight_model_run(DT_FDM, actuators_values);
time += DT_FDM;
booz_sensors_model_run(time);
// if (booz_sensors_model_baro_available())
// printf("%f \t %f\n", time, bsm.baro);
// if (booz_sensors_model_gps_available())
// printf("%f \t %f \t %f\n", time, bsm.gps_pos->ve[AXIS_Z], bsm.gps_speed->ve[AXIS_Z]);
if (booz_sensors_model_accel_available())
printf("%f \t %f \t %f \t %f\n", time, bsm.accel->ve[AXIS_X], bsm.accel->ve[AXIS_Y], bsm.accel->ve[AXIS_Z]);
}
return 0;
}