diff --git a/conf/messages.xml b/conf/messages.xml index 72c0d0d2bd..bd2c51e384 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -424,10 +424,8 @@ - - - - + + @@ -446,13 +444,6 @@ - - - - - - - @@ -467,7 +458,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/telemetry/booz.xml b/conf/telemetry/booz.xml index b589ff6471..2f1065529e 100644 --- a/conf/telemetry/booz.xml +++ b/conf/telemetry/booz.xml @@ -7,7 +7,7 @@ - + @@ -24,9 +24,14 @@ + + + + + diff --git a/sw/airborne/arm7/imu_v3_hw.h b/sw/airborne/arm7/imu_v3_hw.h index 27953b8715..a602ab5bd2 100644 --- a/sw/airborne/arm7/imu_v3_hw.h +++ b/sw/airborne/arm7/imu_v3_hw.h @@ -1,5 +1,12 @@ #ifndef IMU_V3_HW_H #define IMU_V3_HW_H +#include "imu_v3.h" + +#define ImuReadAdcs() { \ + imu_accel_raw[AXIS_X]= buf_ax.sum; \ + imu_accel_raw[AXIS_Y]= buf_ay.sum; \ + imu_accel_raw[AXIS_Z]= buf_az.sum; \ +} #endif /* IMU_V3_HW_H */ diff --git a/sw/airborne/booz_control.c b/sw/airborne/booz_control.c index d52318393c..eb2be97195 100644 --- a/sw/airborne/booz_control.c +++ b/sw/airborne/booz_control.c @@ -133,11 +133,19 @@ void booz_control_attitude_run(void) { } +void booz_control_nav_compute_setpoints(void) { + + +} void booz_control_nav_run(void) { +#if 0 booz_control_commands[COMMAND_P] = 0; booz_control_commands[COMMAND_Q] = 0; booz_control_commands[COMMAND_R] = 0; booz_control_commands[COMMAND_THROTTLE] = 0; +#else + booz_control_rate_run(); +#endif } diff --git a/sw/airborne/booz_filter_telemetry.h b/sw/airborne/booz_filter_telemetry.h index 28ecec211e..e7b07ea2b4 100644 --- a/sw/airborne/booz_filter_telemetry.h +++ b/sw/airborne/booz_filter_telemetry.h @@ -63,6 +63,16 @@ &mtt_P_theta[0][0], &mtt_P_theta[0][1], \ &mtt_P_theta[1][1]); +#define PERIODIC_SEND_BOOZ_DEBUG() { \ + float m_phi = atan2(imu_accel[AXIS_Y], imu_accel[AXIS_Z]); \ + const float g2 = \ + imu_accel[AXIS_X]*imu_accel[AXIS_X] + \ + imu_accel[AXIS_Y]*imu_accel[AXIS_Y] + \ + imu_accel[AXIS_Z]*imu_accel[AXIS_Z]; \ + float m_theta = -asin( imu_accel[AXIS_X] / sqrt( g2 ) ); \ + DOWNLINK_SEND_BOOZ_DEBUG(&m_phi, &m_theta); \ +} + #define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue() extern uint8_t telemetry_mode_Filter; diff --git a/sw/airborne/booz_telemetry.h b/sw/airborne/booz_telemetry.h index 4de9f1b402..cde856f4fa 100644 --- a/sw/airborne/booz_telemetry.h +++ b/sw/airborne/booz_telemetry.h @@ -36,19 +36,13 @@ &booz_estimator_theta, \ &booz_estimator_psi); -#define PERIODIC_SEND_BOOZ_DEBUG() \ - DOWNLINK_SEND_BOOZ_DEBUG(&booz_control_p_sp, \ - &booz_control_q_sp, \ - &booz_control_r_sp, \ - &booz_control_power_sp); - #define PERIODIC_SEND_ACTUATORS() \ DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators); #define PERIODIC_SEND_BOOZ_RATE_LOOP() \ - DOWNLINK_SEND_BOOZ_RATE_LOOP(&booz_estimator_p, &booz_control_p_sp, \ - &booz_estimator_q, &booz_control_q_sp, \ - &booz_estimator_r, &booz_control_r_sp ); + DOWNLINK_SEND_BOOZ_RATE_LOOP(&booz_estimator_uf_p, &booz_control_p_sp, \ + &booz_estimator_uf_q, &booz_control_q_sp, \ + &booz_estimator_uf_r, &booz_control_r_sp ); #define PERIODIC_SEND_BOOZ_ATT_LOOP() \ DOWNLINK_SEND_BOOZ_ATT_LOOP(&booz_estimator_phi, &booz_control_phi_sp, \ diff --git a/sw/airborne/imu_v3.h b/sw/airborne/imu_v3.h index 0f5e4fd5dc..0f9d619d94 100644 --- a/sw/airborne/imu_v3.h +++ b/sw/airborne/imu_v3.h @@ -37,6 +37,7 @@ extern float imu_vs_gyro_raw_var[AXIS_NB]; extern int32_t imu_vs_mag_raw_avg[AXIS_NB]; extern float imu_vs_mag_raw_var[AXIS_NB]; + extern struct adc_buf buf_ax; extern struct adc_buf buf_ay; extern struct adc_buf buf_az; @@ -46,17 +47,15 @@ extern struct adc_buf buf_bat; #define IMU_ACCEL_Y_NEUTRAL (506 * 32) #define IMU_ACCEL_Z_NEUTRAL (506 * 32) -#define IMU_ACCEL_X_GAIN -1. -#define IMU_ACCEL_Y_GAIN 1. -#define IMU_ACCEL_Z_GAIN 1. +#define IMU_ACCEL_X_GAIN (-(6. * 9.81) / (1024*32)) +#define IMU_ACCEL_Y_GAIN ( (6. * 9.81) / (1024*32)) +#define IMU_ACCEL_Z_GAIN ( (6. * 9.81) / (1024*32)) -#define ImuUpdateAccels() { \ - imu_accel_raw[AXIS_X]= buf_ax.sum; \ - imu_accel_raw[AXIS_Y]= buf_ay.sum; \ - imu_accel_raw[AXIS_Z]= buf_az.sum; \ - imu_accel[AXIS_X]= IMU_ACCEL_X_GAIN * (buf_ax.sum - IMU_ACCEL_X_NEUTRAL); \ - imu_accel[AXIS_Y]= IMU_ACCEL_Y_GAIN * (buf_ay.sum - IMU_ACCEL_Y_NEUTRAL); \ - imu_accel[AXIS_Z]= IMU_ACCEL_Y_GAIN * (buf_az.sum - IMU_ACCEL_Z_NEUTRAL); \ +#define ImuUpdateAccels() { \ + ImuReadAdcs(); \ + imu_accel[AXIS_X]= IMU_ACCEL_X_GAIN * (imu_accel_raw[AXIS_X] - IMU_ACCEL_X_NEUTRAL); \ + imu_accel[AXIS_Y]= IMU_ACCEL_Y_GAIN * (imu_accel_raw[AXIS_Y] - IMU_ACCEL_Y_NEUTRAL); \ + imu_accel[AXIS_Z]= IMU_ACCEL_Y_GAIN * (imu_accel_raw[AXIS_Z] - IMU_ACCEL_Z_NEUTRAL); \ } #define IMU_GYRO_P_NEUTRAL 40885 @@ -126,8 +125,8 @@ extern struct adc_buf buf_bat; imu_gyro_prev[AXIS_Q] = imu_gyro[AXIS_Q]; \ imu_gyro_prev[AXIS_R] = imu_gyro[AXIS_R]; \ imu_vs_gyro_initial_bias[AXIS_P] = imu_gyro[AXIS_P]; \ - imu_vs_gyro_initial_bias[AXIS_Q] = imu_gyro[AXIS_Q]; \ - imu_vs_gyro_initial_bias[AXIS_R] = imu_gyro[AXIS_R]; \ + imu_vs_gyro_initial_bias[AXIS_Q] = imu_gyro[AXIS_Q]; \ + imu_vs_gyro_initial_bias[AXIS_R] = imu_gyro[AXIS_R]; \ imu_vs_gyro_unbiased[AXIS_P] = 0.; \ imu_vs_gyro_unbiased[AXIS_Q] = 0.; \ imu_vs_gyro_unbiased[AXIS_R] = 0.; \ diff --git a/sw/airborne/sim/imu_v3_hw.h b/sw/airborne/sim/imu_v3_hw.h index 7b1e1490a1..98ab06b55f 100644 --- a/sw/airborne/sim/imu_v3_hw.h +++ b/sw/airborne/sim/imu_v3_hw.h @@ -1,4 +1,13 @@ #ifndef IMU_V3_HW_H #define IMU_V3_HW_H +#include "imu_v3.h" +#include "../../simulator/booz_sensors_model.h" + +#define ImuReadAdcs() { \ + imu_accel_raw[AXIS_X] = bsm.accel->ve[AXIS_X]; \ + imu_accel_raw[AXIS_Y] = bsm.accel->ve[AXIS_Y]; \ + imu_accel_raw[AXIS_Z] = bsm.accel->ve[AXIS_Z]; \ + } + #endif /* IMU_V3_HW_H */ diff --git a/sw/simulator/booz_flight_model.c b/sw/simulator/booz_flight_model.c index 0ffe7d6dc8..3848b74999 100644 --- a/sw/simulator/booz_flight_model.c +++ b/sw/simulator/booz_flight_model.c @@ -13,7 +13,7 @@ struct BoozFlightModel bfm; //static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot); -static VEC* booz_get_forces_body_frame( VEC* X, VEC* M, MAT* dcm, VEC* omega_square); +static VEC* booz_get_forces_body_frame( VEC* X, VEC* M, MAT* dcm, VEC* omega_square, VEC* rate_body, VEC* speed_body); static VEC* booz_get_moments_body_frame( VEC* X, VEC* M, VEC* omega_square ); static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot); @@ -44,6 +44,8 @@ void booz_flight_model_init( void ) { bfm.props_moment_matrix->me[AXIS_Z][SERVO_MOTOR_BACK] = -bfm.torque_factor; bfm.props_moment_matrix->me[AXIS_Z][SERVO_MOTOR_FRONT] = -bfm.torque_factor; + bfm.mass = MASS; + bfm.Inert = m_get(AXIS_NB, AXIS_NB); m_zero(bfm.Inert); bfm.Inert->me[AXIS_X][AXIS_X] = Ix; @@ -58,6 +60,9 @@ void booz_flight_model_init( void ) { } + +#define WRAP(x,a) { while (x > a) x -= 2 * a; while (x <= -a) x += 2 * a;} + void booz_flight_model_run( double dt, double* commands ) { @@ -66,16 +71,56 @@ void booz_flight_model_run( double dt, double* commands ) { bfm.mot_voltage->ve[i] = bfm.bat_voltage * commands[i]; // rk4(motor_model_derivative, bfm.mot_omega, bfm.mot_voltage, dt); rk4(booz_flight_model_get_derivatives, bfm.state, bfm.mot_voltage, dt); + /* wrap euler angles */ + WRAP( bfm.state->ve[BFMS_PHI], M_PI); + WRAP( bfm.state->ve[BFMS_THETA], M_PI_2); + WRAP( bfm.state->ve[BFMS_PSI], M_PI); + bfm.time += dt; } +VEC* booz_flight_model_get_forces_body_frame(VEC* forces) { + /* square of prop rotational speeds */ + static VEC *omega_square = VNULL; + omega_square = v_resize(omega_square,SERVOS_NB); + omega_square->ve[SERVO_MOTOR_BACK] = bfm.state->ve[BFMS_OM_B]; + omega_square->ve[SERVO_MOTOR_FRONT] = bfm.state->ve[BFMS_OM_F]; + omega_square->ve[SERVO_MOTOR_RIGHT] = bfm.state->ve[BFMS_OM_R]; + omega_square->ve[SERVO_MOTOR_LEFT] = bfm.state->ve[BFMS_OM_L]; + omega_square = v_star(omega_square, omega_square, omega_square); + /* extract eulers angles from state */ + static VEC *eulers = VNULL; + eulers = v_resize(eulers, AXIS_NB); + eulers->ve[EULER_PHI] = bfm.state->ve[BFMS_PHI]; + eulers->ve[EULER_THETA] = bfm.state->ve[BFMS_THETA]; + eulers->ve[EULER_PSI] = bfm.state->ve[BFMS_PSI]; + /* direct cosine matrix ( inertial to body )*/ + static MAT *dcm = MNULL; + dcm = m_resize(dcm,AXIS_NB, AXIS_NB); + dcm = dcm_of_eulers(eulers, dcm); + /* extract body speed from state */ + static VEC *speed_body = VNULL; + speed_body = v_resize(speed_body, AXIS_NB); + speed_body->ve[AXIS_X] = bfm.state->ve[BFMS_U]; + speed_body->ve[AXIS_Y] = bfm.state->ve[BFMS_V]; + speed_body->ve[AXIS_Z] = bfm.state->ve[BFMS_W]; + /* extracts body rates from 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 forces */ + forces = booz_get_forces_body_frame( bfm.state, forces , dcm, omega_square, rate_body, speed_body); + return forces; +} /* compute the sum of external forces. assumes that dcm and omega_square are already precomputed from X */ -static VEC* booz_get_forces_body_frame( VEC* X, VEC* F , MAT* dcm, VEC* omega_square) { +static VEC* booz_get_forces_body_frame( VEC* X, VEC* F , MAT* dcm, VEC* omega_square, VEC* rate_body, VEC* speed_body) { // propeller thrust F->ve[AXIS_X] = 0; F->ve[AXIS_Y] = 0; @@ -93,6 +138,12 @@ static VEC* booz_get_forces_body_frame( VEC* X, VEC* F , MAT* dcm, VEC* omega_sq v_body->ve[AXIS_Z] = X->ve[BFMS_W]; double norm_speed = v_norm2(v_body); F = v_mltadd(F, v_body, - norm_speed * C_d_body, F); + // non inertial forces + static VEC *fict_f = VNULL; + fict_f = v_resize(fict_f, AXIS_NB); + fict_f = out_prod(speed_body, rate_body, fict_f); + fict_f = sv_mlt(bfm.mass, fict_f, fict_f); + F = v_add(F, fict_f, F); return F; } @@ -129,11 +180,23 @@ static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot) { static MAT *dcm_t = MNULL; dcm_t = m_resize(dcm_t,AXIS_NB, AXIS_NB); dcm_t = m_transp(dcm, dcm_t); + /* extract body_speeds_from state */ + static VEC *speed_body = VNULL; + speed_body = v_resize(speed_body, AXIS_NB); + speed_body->ve[AXIS_X] = X->ve[BFMS_U]; + speed_body->ve[AXIS_Y] = X->ve[BFMS_V]; + speed_body->ve[AXIS_Z] = X->ve[BFMS_W]; + /* extracts body rates from state */ + static VEC *rate_body = VNULL; + rate_body = v_resize(rate_body, AXIS_NB); + rate_body->ve[AXIS_P] = X->ve[BFMS_P]; + rate_body->ve[AXIS_Q] = X->ve[BFMS_Q]; + rate_body->ve[AXIS_R] = X->ve[BFMS_R]; /* compute external forces */ static VEC *f_body = VNULL; f_body = v_resize(f_body, AXIS_NB); - f_body = booz_get_forces_body_frame( X, f_body , dcm, omega_square); + f_body = booz_get_forces_body_frame( X, f_body , dcm, omega_square, rate_body, speed_body); /* compute external moments */ static VEC *m_body = VNULL; @@ -141,11 +204,6 @@ static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot) { m_body = booz_get_moments_body_frame( X, m_body, omega_square); /* derivatives of position */ - static VEC *speed_body = VNULL; - speed_body = v_resize(speed_body, AXIS_NB); - speed_body->ve[AXIS_X] = X->ve[BFMS_U]; - speed_body->ve[AXIS_Y] = X->ve[BFMS_V]; - speed_body->ve[AXIS_Z] = X->ve[BFMS_W]; static VEC *speed_earth = VNULL; speed_earth = v_resize(speed_earth, AXIS_NB); speed_earth = mv_mlt(dcm_t, speed_body,speed_earth); @@ -154,19 +212,9 @@ static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot) { Xdot->ve[BFMS_Z] = speed_earth->ve[AXIS_Z]; /* derivatives of speed */ - /* extracts body rates from state */ - static VEC *rate_body = VNULL; - rate_body = v_resize(rate_body, AXIS_NB); - rate_body->ve[AXIS_P] = X->ve[BFMS_P]; - rate_body->ve[AXIS_Q] = X->ve[BFMS_Q]; - rate_body->ve[AXIS_R] = X->ve[BFMS_R]; - /* Newton in body frame */ - static VEC *fict_f = VNULL; - fict_f = v_resize(fict_f, AXIS_NB); - fict_f = out_prod(speed_body, rate_body, fict_f); - Xdot->ve[BFMS_U] = 1./MASS * f_body->ve[AXIS_X] + fict_f->ve[AXIS_X]; - Xdot->ve[BFMS_V] = 1./MASS * f_body->ve[AXIS_Y] + fict_f->ve[AXIS_Y]; - Xdot->ve[BFMS_W] = 1./MASS * f_body->ve[AXIS_Z] + fict_f->ve[AXIS_Z]; + Xdot->ve[BFMS_U] = 1./MASS * f_body->ve[AXIS_X]; + Xdot->ve[BFMS_V] = 1./MASS * f_body->ve[AXIS_Y]; + Xdot->ve[BFMS_W] = 1./MASS * f_body->ve[AXIS_Z]; /* derivatives of eulers */ double sinPHI = sin(X->ve[BFMS_PHI]); diff --git a/sw/simulator/booz_flight_model.h b/sw/simulator/booz_flight_model.h index 5e0d95bf09..b7b8af7cac 100644 --- a/sw/simulator/booz_flight_model.h +++ b/sw/simulator/booz_flight_model.h @@ -41,6 +41,8 @@ struct BoozFlightModel { double torque_factor; /* Matrix used to compute the moments produced by props */ MAT* props_moment_matrix; + /* */ + double mass; /* inertia matrix */ MAT* Inert; /* invert of inertia matrix */ @@ -51,7 +53,7 @@ extern struct BoozFlightModel bfm; extern void booz_flight_model_init( void ); extern void booz_flight_model_run( double t, double* commands ); - +extern VEC* booz_flight_model_get_forces_body_frame(VEC* forces); #endif /* BOOZ_FLIGHT_MODEL_H */ diff --git a/sw/simulator/booz_sensors_model.c b/sw/simulator/booz_sensors_model.c index dd97c666db..03958fcc46 100644 --- a/sw/simulator/booz_sensors_model.c +++ b/sw/simulator/booz_sensors_model.c @@ -13,6 +13,26 @@ void booz_sensors_model_init(void) { bsm.accel->ve[AXIS_Y] = 0.; bsm.accel->ve[AXIS_Z] = 0.; + bsm.accel_sensitivity = m_get(AXIS_NB, AXIS_NB); + m_zero(bsm.accel_sensitivity); + bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = -(1024 * 32) / (6. * 9.81); + bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = (1024. * 32) / (6. * 9.81); + bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = (1024. * 32) / (6. * 9.81); + + bsm.accel_neutral = v_get(AXIS_NB); + bsm.accel_neutral->ve[AXIS_X] = 538 * 32; + bsm.accel_neutral->ve[AXIS_Y] = 506 * 32; + bsm.accel_neutral->ve[AXIS_Z] = 506 * 32; + + bsm.accel_noise_std_dev = v_get(AXIS_NB); + bsm.accel_noise_std_dev->ve[AXIS_X] = 1e-1 * bsm.accel_sensitivity->me[AXIS_X][AXIS_X]; + bsm.accel_noise_std_dev->ve[AXIS_Y] = 1e-1 * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y]; + bsm.accel_noise_std_dev->ve[AXIS_Z] = 1e-1 * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z]; + + bsm.accel_bias = v_get(AXIS_NB); + bsm.accel_bias->ve[AXIS_P] = 1e-3 * bsm.accel_sensitivity->me[AXIS_X][AXIS_X]; + bsm.accel_bias->ve[AXIS_Q] = 1e-3 * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y]; + bsm.accel_bias->ve[AXIS_R] = 1e-3 * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z]; bsm.gyro = v_get(AXIS_NB); bsm.gyro->ve[AXIS_P] = 0.; @@ -30,11 +50,73 @@ void booz_sensors_model_init(void) { bsm.gyro_neutral->ve[AXIS_Q] = 40910; bsm.gyro_neutral->ve[AXIS_R] = 39552; + bsm.gyro_noise_std_dev = v_get(AXIS_NB); + bsm.gyro_noise_std_dev->ve[AXIS_P] = 5e-3 * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P]; + bsm.gyro_noise_std_dev->ve[AXIS_Q] = 5e-3 * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q]; + bsm.gyro_noise_std_dev->ve[AXIS_R] = 5e-3 * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R]; + + bsm.gyro_bias = v_get(AXIS_NB); + bsm.gyro_bias->ve[AXIS_P] = 1e-5 * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P]; + bsm.gyro_bias->ve[AXIS_Q] = 1e-5 * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q]; + bsm.gyro_bias->ve[AXIS_R] = 1e-5 * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R]; } void booz_sensors_model_run(void) { - + + /* compute forces */ + static VEC *accel_body = VNULL; + accel_body = v_resize(accel_body, AXIS_NB); +#if 0 + accel_body = booz_flight_model_get_forces_body_frame(accel_body); + /* divide by mass */ + accel_body = sv_mlt(1./bfm.mass, accel_body, accel_body); + /* get g in body frame */ + /* extract eulers angles from state */ + static VEC *eulers = VNULL; + eulers = v_resize(eulers, AXIS_NB); + eulers->ve[EULER_PHI] = bfm.state->ve[BFMS_PHI]; + eulers->ve[EULER_THETA] = bfm.state->ve[BFMS_THETA]; + eulers->ve[EULER_PSI] = bfm.state->ve[BFMS_PSI]; + /* direct cosine matrix ( inertial to body )*/ + static MAT *dcm = MNULL; + dcm = m_resize(dcm,AXIS_NB, AXIS_NB); + dcm = dcm_of_eulers(eulers, dcm); + static VEC *g_body = VNULL; + g_body = v_resize(g_body, AXIS_NB); + g_body = mv_mlt(dcm, bfm.g_earth, g_body); + accel_body = v_sub(g_body, accel_body, accel_body); +#else + accel_body->ve[AXIS_X] = -9.81 * sin(bfm.state->ve[BFMS_THETA]); + accel_body->ve[AXIS_Y] = 9.81 * sin(bfm.state->ve[BFMS_PHI]) * cos(bfm.state->ve[BFMS_THETA]); + accel_body->ve[AXIS_Z] = 9.81 * cos(bfm.state->ve[BFMS_PHI]) * cos(bfm.state->ve[BFMS_THETA]); +#endif + + // printf("sim accel %f %f %f\n",accel_body->ve[AXIS_X] ,accel_body->ve[AXIS_Y] ,accel_body->ve[AXIS_Z]); + /* 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 */ + /* gaussian noise */ + static VEC *accel_error = VNULL; + accel_error = v_resize(accel_error, AXIS_NB); + accel_error = v_rand(accel_error); + static VEC *one = VNULL; + one = v_resize(one, AXIS_NB); + one = v_ones(one); + accel_error = v_mltadd(one, accel_error, -2., accel_error); + accel_error = v_star(accel_error, bsm.accel_noise_std_dev, accel_error); + /* constant bias */ + accel_error = v_add(accel_error, bsm.accel_bias, accel_error); + /* add per accel error reading */ + bsm.accel = v_add(bsm.accel, accel_error, bsm.accel); + /* round signal to account for adc discretisation */ + bsm.accel->ve[AXIS_X] = round( bsm.accel->ve[AXIS_X]); + bsm.accel->ve[AXIS_Y] = round( bsm.accel->ve[AXIS_Y]); + bsm.accel->ve[AXIS_Z] = round( bsm.accel->ve[AXIS_Z]); + + // printf("sim adc %f %f %f\n",bsm.accel->ve[AXIS_X] ,bsm.accel->ve[AXIS_Y] ,bsm.accel->ve[AXIS_Z]); + /* extract rotational speed from flight model state */ static VEC *rate_body = VNULL; rate_body = v_resize(rate_body, AXIS_NB); @@ -42,9 +124,22 @@ void booz_sensors_model_run(void) { rate_body->ve[AXIS_Q] = bfm.state->ve[BFMS_Q]; rate_body->ve[AXIS_R] = bfm.state->ve[BFMS_R]; - /* compute sensor readings */ + /* 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 readinf*/ + /* gaussian noise */ + static VEC *gyro_error = VNULL; + gyro_error = v_resize(gyro_error, AXIS_NB); + gyro_error = v_rand(gyro_error); + gyro_error = v_mltadd(one, gyro_error, -2., gyro_error); + gyro_error = v_star(gyro_error, bsm.gyro_noise_std_dev, gyro_error); + /* constant bias */ + gyro_error = v_add(bsm.gyro_bias, gyro_error, gyro_error); + /* add per gyro error reading */ + bsm.gyro = v_add(bsm.gyro, gyro_error, bsm.gyro); + /* round signal to account for adc discretisation */ bsm.gyro->ve[AXIS_P] = round( bsm.gyro->ve[AXIS_P]); bsm.gyro->ve[AXIS_Q] = round( bsm.gyro->ve[AXIS_Q]); bsm.gyro->ve[AXIS_R] = round( bsm.gyro->ve[AXIS_R]); diff --git a/sw/simulator/booz_sensors_model.h b/sw/simulator/booz_sensors_model.h index 1557067ecd..c608ced9ee 100644 --- a/sw/simulator/booz_sensors_model.h +++ b/sw/simulator/booz_sensors_model.h @@ -8,10 +8,19 @@ extern void booz_sensors_model_init(void); extern void booz_sensors_model_run(void); struct BoozSensorsModel { + VEC* accel; + MAT* accel_sensitivity; + VEC* accel_neutral; + VEC* accel_noise_std_dev; + VEC* accel_bias; + VEC* gyro; MAT* gyro_sensitivity; VEC* gyro_neutral; + VEC* gyro_noise_std_dev; + VEC* gyro_bias; + }; extern struct BoozSensorsModel bsm; diff --git a/sw/simulator/main_booz_sim.c b/sw/simulator/main_booz_sim.c index c8306ab800..a9dd0212b5 100644 --- a/sw/simulator/main_booz_sim.c +++ b/sw/simulator/main_booz_sim.c @@ -56,7 +56,8 @@ static gboolean booz_sim_periodic(gpointer data) { /* read actuators positions */ booz_sim_read_actuators(); - booz_flight_model_run(DT, booz_sim_actuators_values); + if (sim_time > 3.) + booz_flight_model_run(DT, booz_sim_actuators_values); booz_sensors_model_run(); sim_time += DT; @@ -64,19 +65,21 @@ static gboolean booz_sim_periodic(gpointer data) { /* the sim implementation of max1167_read ( called by ImuPeriodic() ) */ /* will post a ImuEvent */ booz_filter_main_periodic_task(); + /* process the ImuEvent */ /* it will run the filter and the inter-process communication which */ /* will post a BoozLinkMcuEvent in the Controller process */ booz_filter_main_event_task(); /* process the BoozLinkMcuEvent */ - /* this will update the controller stimator */ + /* this will update the controller estimator */ booz_controller_main_event_task(); - /* post a radio control event */ booz_sim_set_ppm_from_joystick(); ppm_valid = TRUE; /* and let the controller process it */ booz_controller_main_event_task(); + // printf("ppm_pulses %d\n", ppm_pulses[RADIO_THROTTLE]); + // printf("rc_value %d\n", rc_values[RADIO_THROTTLE]); /* call the controller periodic task to run control loops */ booz_controller_main_periodic_task(); @@ -132,22 +135,43 @@ static inline void booz_sim_display(void) { if (sim_time >= disp_time) { disp_time+= DT_DISPLAY; booz_flightgear_send(); - IvySendMsg("148 BOOZ_RPMS %f %f %f %f", + IvySendMsg("148 BOOZ_SIM_RPMS %f %f %f %f", RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_F]), RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_B]), RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_L]), RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_R]) ); + IvySendMsg("148 BOOZ_SIM_BODY_RATE %f %f %f", + DegOfRad(bfm.state->ve[BFMS_P]), + DegOfRad(bfm.state->ve[BFMS_Q]), + DegOfRad(bfm.state->ve[BFMS_R])); + IvySendMsg("148 BOOZ_SIM_ATTITUDE %f %f %f", + DegOfRad(bfm.state->ve[BFMS_PHI]), + DegOfRad(bfm.state->ve[BFMS_THETA]), + DegOfRad(bfm.state->ve[BFMS_PSI])); } } static void booz_sim_set_ppm_from_joystick( void ) { + // printf("joystick_value %f\n", booz_joystick_value[JS_THROTTLE]); ppm_pulses[RADIO_THROTTLE] = 1223 + booz_joystick_value[JS_THROTTLE] * (2050-1223); ppm_pulses[RADIO_PITCH] = 1498 + booz_joystick_value[JS_PITCH] * (2050-950); ppm_pulses[RADIO_ROLL] = 1500 + booz_joystick_value[JS_ROLL] * (2050-950); - ppm_pulses[RADIO_YAW] = 1500 + booz_joystick_value[JS_YAW] * (2050-950); + ppm_pulses[RADIO_YAW] = 1493 + booz_joystick_value[JS_YAW] * (2050-950); ppm_pulses[RADIO_MODE] = 1500 + booz_joystick_value[JS_MODE] * (2050-950); + + int foo = sim_time/5; + ppm_pulses[RADIO_THROTTLE] = 1223 + 0.7 * (2050-1223); + ppm_pulses[RADIO_MODE] = 1500 + 0. * (2050-950); + if (foo%2) { + ppm_pulses[RADIO_ROLL] = 1500 + 0.2 * (2050-950); + ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950); + } + else { + ppm_pulses[RADIO_ROLL] = 1500 - 0.2 * (2050-950); + ppm_pulses[RADIO_PITCH] = 1500 - 0.2 * (2050-950); + } } @@ -157,12 +181,12 @@ static void booz_sim_read_actuators( void ) { booz_sim_actuators_values[SERVO_MOTOR_FRONT] = (actuators[SERVO_MOTOR_FRONT] - 1200)/(double)(1850-1200); booz_sim_actuators_values[SERVO_MOTOR_RIGHT] = (actuators[SERVO_MOTOR_RIGHT] - 1200)/(double)(1850-1200); booz_sim_actuators_values[SERVO_MOTOR_LEFT] = (actuators[SERVO_MOTOR_LEFT] - 1200)/(double)(1850-1200); -#endif - +#else booz_sim_actuators_values[SERVO_MOTOR_BACK] = (actuators[SERVO_MOTOR_BACK] - 0)/(double)(255); booz_sim_actuators_values[SERVO_MOTOR_FRONT] = (actuators[SERVO_MOTOR_FRONT] - 0)/(double)(255); booz_sim_actuators_values[SERVO_MOTOR_RIGHT] = (actuators[SERVO_MOTOR_RIGHT] - 0)/(double)(255); booz_sim_actuators_values[SERVO_MOTOR_LEFT] = (actuators[SERVO_MOTOR_LEFT] - 0)/(double)(255); +#endif }