diff --git a/conf/autopilot/conf_booz.makefile b/conf/autopilot/conf_booz.makefile index 3a67a1b8a3..d4114c772a 100644 --- a/conf/autopilot/conf_booz.makefile +++ b/conf/autopilot/conf_booz.makefile @@ -169,9 +169,13 @@ sim.srcs += imu_v3.c $(SRC_ARCH)/imu_v3_hw.c sim.CFLAGS += -DFLOAT_T=float sim.srcs += booz_ahrs.c + sim.CFLAGS += -DBOOZ_AHRS_TYPE=BOOZ_AHRS_MULTITILT sim.srcs += multitilt.c + #sim.CFLAGS += -DBOOZ_AHRS_TYPE=BOOZ_AHRS_QUATERNION #sim.CFLAGS += -DEKF_UPDATE_DISCRETE #sim.CFLAGS += -DEKF_PREDICT_ONLY -#sim.srcs += ahrs_quat_fast_ekf.c \ No newline at end of file +#sim.srcs += ahrs_quat_fast_ekf.c + +sim.srcs += comp_flt.c diff --git a/sw/airborne/ahrs_quat_fast_ekf.h b/sw/airborne/ahrs_quat_fast_ekf.h index 2e8697a474..ec26797719 100644 --- a/sw/airborne/ahrs_quat_fast_ekf.h +++ b/sw/airborne/ahrs_quat_fast_ekf.h @@ -4,9 +4,6 @@ #include #include "6dof.h" -//#define FLOAT_T double -//#define FLOAT_T float - /* ekf state : quaternion and gyro biases */ extern FLOAT_T afe_q0, afe_q1, afe_q2, afe_q3; //extern FLOAT_T afe_bias_p, afe_bias_q, afe_bias_r; diff --git a/sw/airborne/booz_filter_main.c b/sw/airborne/booz_filter_main.c index acba42dfdd..590217710d 100644 --- a/sw/airborne/booz_filter_main.c +++ b/sw/airborne/booz_filter_main.c @@ -75,17 +75,18 @@ STATIC_INLINE void booz_filter_main_periodic_task( void ) { booz_filter_telemetry_periodic_task(); break; #ifndef SITL - case 1: + // case 1: /* triger measurements */ - ami601_periodic(); + // ami601_periodic(); // Z Y X - DOWNLINK_SEND_IMU_MAG_RAW(&ami601_val[0], &ami601_val[4], &ami601_val[2]); + // DOWNLINK_SEND_IMU_MAG_RAW(&ami601_val[0], &ami601_val[4], &ami601_val[2]); #endif } } static inline void on_imu_event( void ) { + switch (booz_ahrs_status) { case BOOZ_AHRS_STATUS_UNINIT : @@ -95,7 +96,7 @@ static inline void on_imu_event( void ) { booz_ahrs_start(imu_accel, imu_gyro, imu_mag); } break; - + case BOOZ_AHRS_STATUS_RUNNING : // t0 = T0TC; booz_ahrs_run(imu_accel, imu_gyro_prev, imu_mag); diff --git a/sw/airborne/imu_v3.h b/sw/airborne/imu_v3.h index 1c87c6afd7..a26541dd3a 100644 --- a/sw/airborne/imu_v3.h +++ b/sw/airborne/imu_v3.h @@ -47,10 +47,10 @@ extern struct adc_buf buf_bat; #define IMU_ACCEL_X_NEUTRAL (538 * 32) #define IMU_ACCEL_Y_NEUTRAL (506 * 32) #define IMU_ACCEL_Z_NEUTRAL (506 * 32) - -#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)) +// FIXME ??? why 0.5 +#define IMU_ACCEL_X_GAIN ( (0.5 * 6. * 9.81) / (1024*32)) +#define IMU_ACCEL_Y_GAIN (-(0.5 * 6. * 9.81) / (1024*32)) +#define IMU_ACCEL_Z_GAIN (-(0.5 * 6. * 9.81) / (1024*32)) #define ImuUpdateAccels() { \ ImuReadAdcs(); \ diff --git a/sw/airborne/multitilt.c b/sw/airborne/multitilt.c index 16649af620..35a5d225a1 100644 --- a/sw/airborne/multitilt.c +++ b/sw/airborne/multitilt.c @@ -15,10 +15,10 @@ float mtt_P_psi[2][2]; /* process covariance noise */ //static const float Q_angle = 0.0005; static const float Q_angle = 0.00025; -static const float Q_bias = 0.0015; +static const float Q_bias = 0.0005; /* Measurement covariance */ //static const float R_accel = 0.3; -static const float R_accel = 0.4; +static const float R_accel = 0.5; static inline void multitilt_update( const float* accel, const int16_t* mag ); static inline void mtt_update_axis(float _err, float _P[2][2], float* angle, float* bias); @@ -29,7 +29,7 @@ static inline void mtt_predict_axis(float* angle, float angle_dot, float P[2][2] #define WRAP(x,a) { while (x > a) x -= 2 * a; while (x <= -a) x += 2 * a;} static inline float phi_of_accel( const float* accel) { - return atan2(accel[AXIS_Y], accel[AXIS_Z]); + return atan2(-accel[AXIS_Y], -accel[AXIS_Z]); } static inline float theta_of_accel( const float* accel) { @@ -37,7 +37,7 @@ static inline float theta_of_accel( const float* accel) { accel[AXIS_X]*accel[AXIS_X] + accel[AXIS_Y]*accel[AXIS_Y] + accel[AXIS_Z]*accel[AXIS_Z]; - return -asin( accel[AXIS_X] / sqrt( g2 ) ); + return asin( accel[AXIS_X] / sqrt( g2 ) ); } static inline float psi_of_mag( const int16_t* mag) { diff --git a/sw/simulator/booz_flight_model.c b/sw/simulator/booz_flight_model.c index 80d2e7aa1e..94f57048e5 100644 --- a/sw/simulator/booz_flight_model.c +++ b/sw/simulator/booz_flight_model.c @@ -4,7 +4,6 @@ #include "booz_flight_model_params.h" #include "booz_flight_model_utils.h" -#include "airframe.h" #include "6dof.h" @@ -13,11 +12,13 @@ struct BoozFlightModel bfm; //static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot); -static VEC* booz_get_forces_body_frame(VEC* M, MAT* dcm, VEC* omega_square, VEC* speed_body); +//static VEC* booz_get_forces_body_frame(VEC* M, MAT* dcm, VEC* omega_square, VEC* speed_body); static VEC* booz_get_moments_body_frame(VEC* M, VEC* omega_square ); static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot); void booz_flight_model_init( void ) { + bfm.on_ground = TRUE; + bfm.time = 0.; bfm.bat_voltage = BAT_VOLTAGE; bfm.mot_voltage = v_get(SERVOS_NB); @@ -90,20 +91,33 @@ void booz_flight_model_run( double dt, double* commands ) { 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* F , MAT* dcm, VEC* omega_square, VEC* speed_body) { - // propeller thrust - F->ve[AXIS_X] = 0; - F->ve[AXIS_Y] = 0; - F->ve[AXIS_Z] = -v_sum(omega_square) * bfm.thrust_factor; - // gravity - static VEC *g_body = VNULL; - g_body = v_resize(g_body, AXIS_NB); - g_body = mv_mlt(dcm, bfm.g_earth, g_body); - F = v_mltadd(F, g_body, MASS, F); - // body drag - double norm_speed = v_norm2(speed_body); - F = v_mltadd(F, speed_body, - norm_speed * C_d_body, F); +VEC* booz_get_forces_body_frame(VEC* F , MAT* dcm, VEC* omega_square, VEC* speed_body, int exclude_weight) { + // FIXME : nimporte koi ! + if (bfm.on_ground) { + F->ve[AXIS_X] = 0; + F->ve[AXIS_Y] = 0; + if (!exclude_weight) + F->ve[AXIS_Z] = 0; + else + F->ve[AXIS_Z] = -9.81*MASS; + } + else { + // propeller thrust + F->ve[AXIS_X] = 0; + F->ve[AXIS_Y] = 0; + F->ve[AXIS_Z] = -v_sum(omega_square) * bfm.thrust_factor; + if (!exclude_weight) { + // gravity + static VEC *g_body = VNULL; + g_body = v_resize(g_body, AXIS_NB); + g_body = mv_mlt(dcm, bfm.g_earth, g_body); + F = v_mltadd(F, g_body, MASS, F); + } + // body drag + double norm_speed = v_norm2(speed_body); + F = v_mltadd(F, speed_body, - norm_speed * C_d_body, F); + } return F; } @@ -112,7 +126,12 @@ static VEC* booz_get_forces_body_frame(VEC* F , MAT* dcm, VEC* omega_square, VEC assumes that omega_square is already precomputed from X */ static VEC* booz_get_moments_body_frame(VEC* M, VEC* omega_square ) { - M = mv_mlt(bfm.props_moment_matrix, omega_square, M); + if (bfm.on_ground) { + M = v_zero(M); + } + else { + M = mv_mlt(bfm.props_moment_matrix, omega_square, M); + } return M; } @@ -157,7 +176,7 @@ static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot) { /* compute external forces */ static VEC *f_body = VNULL; f_body = v_resize(f_body, AXIS_NB); - f_body = booz_get_forces_body_frame(f_body , dcm, omega_square, speed_body); + f_body = booz_get_forces_body_frame(f_body , dcm, omega_square, speed_body, FALSE); /* add non inertial forces */ static VEC *fict_f = VNULL; diff --git a/sw/simulator/booz_flight_model.h b/sw/simulator/booz_flight_model.h index 774126e958..f76462bfec 100644 --- a/sw/simulator/booz_flight_model.h +++ b/sw/simulator/booz_flight_model.h @@ -2,6 +2,8 @@ #define BOOZ_FLIGHT_MODEL_H #include +#include "airframe.h" + #define BFMS_X 0 #define BFMS_Y 1 @@ -23,6 +25,9 @@ struct BoozFlightModel { + /* are we flying ? */ + int on_ground; + double time; /* battery voltage in V */ double bat_voltage; @@ -56,6 +61,8 @@ extern struct BoozFlightModel bfm; extern void booz_flight_model_init( void ); extern void booz_flight_model_run( double t, double* commands ); +extern VEC* booz_get_forces_body_frame(VEC* M, MAT* dcm, VEC* omega_square, VEC* speed_body, int exclude_weight); + #define BoozFlighModelGetPos(_dest) { \ _dest->ve[AXIS_X] = bfm.state->ve[BFMS_X]; \ _dest->ve[AXIS_Y] = bfm.state->ve[BFMS_Y]; \ diff --git a/sw/simulator/booz_sensors_model.c b/sw/simulator/booz_sensors_model.c index 0e2c52f725..c3e135b5c4 100644 --- a/sw/simulator/booz_sensors_model.c +++ b/sw/simulator/booz_sensors_model.c @@ -238,37 +238,34 @@ static void booz_sensors_model_accel_run( MAT* dcm ) { accel_body = v_resize(accel_body, AXIS_NB); accel_body = v_zero(accel_body); #if 1 - /* get g in body frame */ - static VEC *g_body = VNULL; - g_body = v_resize(g_body, AXIS_NB); - g_body = mv_mlt(dcm, bfm.g_earth, g_body); + /* compute sum of forces in body frame except gravity */ - /* add non inertial forces */ + /* 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); - 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]; - 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); + BoozFlighModelGetSpeed(speed_body); + + accel_body = booz_get_forces_body_frame(accel_body , dcm, omega_square, speed_body, TRUE); + + /* divide by mass */ - // accel_body = sv_mlt(1./bfm.mass, accel_body, accel_body); - accel_body = v_add(g_body, fict_f, 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]); - 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]); - printf(" accel_body ~ %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]); + accel_body = sv_mlt(1./bfm.mass, accel_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]); + static VEC* accel_inert = VNULL; + accel_inert = v_resize(accel_inert, AXIS_NB); + accel_inert->ve[AXIS_X] = 0; + accel_inert->ve[AXIS_Y] = 0; + accel_inert->ve[AXIS_Z] = -9.81; + accel_body = mv_mlt(dcm, accel_inert, 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 */ diff --git a/sw/simulator/booz_sensors_model_params.h b/sw/simulator/booz_sensors_model_params.h index 90d27736c8..cd108d8124 100644 --- a/sw/simulator/booz_sensors_model_params.h +++ b/sw/simulator/booz_sensors_model_params.h @@ -6,16 +6,16 @@ */ #define BSM_ACCEL_RESOLUTION (1024 * 32) /* ms-2 */ -#define BSM_ACCEL_SENSITIVITY_XX -(1024. * 32.)/(2 * 6. * 9.81) -#define BSM_ACCEL_SENSITIVITY_YY (1024. * 32.)/(2 * 6. * 9.81) -#define BSM_ACCEL_SENSITIVITY_ZZ (1024. * 32.)/(2 * 6. * 9.81) +#define BSM_ACCEL_SENSITIVITY_XX (1024. * 32.)/(0.5 * 6. * 9.81) +#define BSM_ACCEL_SENSITIVITY_YY -(1024. * 32.)/(0.5 * 6. * 9.81) +#define BSM_ACCEL_SENSITIVITY_ZZ -(1024. * 32.)/(0.5 * 6. * 9.81) #define BSM_ACCEL_NEUTRAL_X (538. * 32.) #define BSM_ACCEL_NEUTRAL_Y (506. * 32.) #define BSM_ACCEL_NEUTRAL_Z (506. * 32.) /* m2s-4 */ -#define BSM_ACCEL_NOISE_STD_DEV_X 2e-1 -#define BSM_ACCEL_NOISE_STD_DEV_Y 2e-1 -#define BSM_ACCEL_NOISE_STD_DEV_Z 2e-1 +#define BSM_ACCEL_NOISE_STD_DEV_X 1e-1 +#define BSM_ACCEL_NOISE_STD_DEV_Y 1e-1 +#define BSM_ACCEL_NOISE_STD_DEV_Z 1e-1 /* ms-2 */ #define BSM_ACCEL_BIAS_X 1e-3 #define BSM_ACCEL_BIAS_Y 1e-3 diff --git a/sw/simulator/main_booz_sim.c b/sw/simulator/main_booz_sim.c index 6071062d95..46afcea9d1 100644 --- a/sw/simulator/main_booz_sim.c +++ b/sw/simulator/main_booz_sim.c @@ -61,9 +61,10 @@ static gboolean booz_sim_periodic(gpointer data __attribute__ ((unused))) { /* run our models */ if (sim_time > 3.) + bfm.on_ground = FALSE; /* no fdm at start to allow for filter initialisation */ /* it sucks, I know */ - booz_flight_model_run(DT, booz_sim_actuators_values); + booz_flight_model_run(DT, booz_sim_actuators_values); booz_sensors_model_run(DT);