diff --git a/sw/simulator/booz_sensors_model.c b/sw/simulator/booz_sensors_model.c index f7376ce017..5684785a5c 100644 --- a/sw/simulator/booz_sensors_model.c +++ b/sw/simulator/booz_sensors_model.c @@ -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; -} diff --git a/sw/simulator/booz_sensors_model.h b/sw/simulator/booz_sensors_model.h index e3e76a95a8..1c5f7ae17a 100644 --- a/sw/simulator/booz_sensors_model.h +++ b/sw/simulator/booz_sensors_model.h @@ -1,63 +1,84 @@ #ifndef BOOZ_SENSORS_MODEL_H #define BOOZ_SENSORS_MODEL_H -#include "6dof.h" #include #include -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 */ diff --git a/sw/simulator/booz_sensors_model_accel.c b/sw/simulator/booz_sensors_model_accel.c new file mode 100644 index 0000000000..3ba336eda6 --- /dev/null +++ b/sw/simulator/booz_sensors_model_accel.c @@ -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; +} + diff --git a/sw/simulator/booz_sensors_model_baro.c b/sw/simulator/booz_sensors_model_baro.c new file mode 100644 index 0000000000..9d4a96296d --- /dev/null +++ b/sw/simulator/booz_sensors_model_baro.c @@ -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; +} diff --git a/sw/simulator/booz_sensors_model_gps.c b/sw/simulator/booz_sensors_model_gps.c new file mode 100644 index 0000000000..458f391732 --- /dev/null +++ b/sw/simulator/booz_sensors_model_gps.c @@ -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; + +} + diff --git a/sw/simulator/booz_sensors_model_gyro.c b/sw/simulator/booz_sensors_model_gyro.c new file mode 100644 index 0000000000..602d864aac --- /dev/null +++ b/sw/simulator/booz_sensors_model_gyro.c @@ -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; +} + + diff --git a/sw/simulator/booz_sensors_model_mag.c b/sw/simulator/booz_sensors_model_mag.c new file mode 100644 index 0000000000..bf18a562d4 --- /dev/null +++ b/sw/simulator/booz_sensors_model_mag.c @@ -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; +} + diff --git a/sw/simulator/booz_sensors_model_params.h b/sw/simulator/booz_sensors_model_params.h index 32755542c1..0f176649f7 100644 --- a/sw/simulator/booz_sensors_model_params.h +++ b/sw/simulator/booz_sensors_model_params.h @@ -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 */ diff --git a/sw/simulator/booz_sensors_model_rangemeter.c b/sw/simulator/booz_sensors_model_rangemeter.c new file mode 100644 index 0000000000..af878cc2ae --- /dev/null +++ b/sw/simulator/booz_sensors_model_rangemeter.c @@ -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; + +} diff --git a/sw/simulator/booz_sensors_model_utils.c b/sw/simulator/booz_sensors_model_utils.c new file mode 100644 index 0000000000..6fd655777b --- /dev/null +++ b/sw/simulator/booz_sensors_model_utils.c @@ -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; +} diff --git a/sw/simulator/booz_sensors_model_utils.h b/sw/simulator/booz_sensors_model_utils.h new file mode 100644 index 0000000000..007640ed27 --- /dev/null +++ b/sw/simulator/booz_sensors_model_utils.h @@ -0,0 +1,41 @@ +#ifndef BOOZ_SENSORS_MODEL_UTILS_H +#define BOOZ_SENSORS_MODEL_UTILS_H + +#include +#include + +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 */ diff --git a/sw/simulator/main_booz_sim.c b/sw/simulator/main_booz_sim.c index 9a92635e17..ca6fdad5ca 100644 --- a/sw/simulator/main_booz_sim.c +++ b/sw/simulator/main_booz_sim.c @@ -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(); diff --git a/sw/simulator/tests/Makefile b/sw/simulator/tests/Makefile new file mode 100644 index 0000000000..10651cac7a --- /dev/null +++ b/sw/simulator/tests/Makefile @@ -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) diff --git a/sw/simulator/tests/test_fdm.c b/sw/simulator/tests/test_fdm.c new file mode 100644 index 0000000000..bb7cb44156 --- /dev/null +++ b/sw/simulator/tests/test_fdm.c @@ -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; ive[BFMS_Z]); + } + + return 0; +} diff --git a/sw/simulator/tests/test_sensors.c b/sw/simulator/tests/test_sensors.c new file mode 100644 index 0000000000..f712bc9876 --- /dev/null +++ b/sw/simulator/tests/test_sensors.c @@ -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; ive[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; +}