diff --git a/conf/airframes/booz2_a1.xml b/conf/airframes/booz2_a1.xml index e97437db50..7e858cc719 100644 --- a/conf/airframes/booz2_a1.xml +++ b/conf/airframes/booz2_a1.xml @@ -61,12 +61,14 @@ - - - + + - + + + + + diff --git a/conf/autopilot/booz2_simulator.makefile b/conf/autopilot/booz2_simulator.makefile index e924f31421..61a3085a63 100644 --- a/conf/autopilot/booz2_simulator.makefile +++ b/conf/autopilot/booz2_simulator.makefile @@ -17,7 +17,12 @@ sim.CFLAGS += -DSITL sim.CFLAGS += `pkg-config glib-2.0 --cflags` -I /usr/include/meschach sim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lmeschach -lpcre -lglibivy -sim.CFLAGS += -DBYPASS_AHRS +#sim.CFLAGS += -DBYPASS_AHRS +#sim.CFLAGS += -DBYPASS_INS +sim.CFLAGS += -DINIT_WIND_X=-5.0 +sim.CFLAGS += -DINIT_WIND_Y=-0.0 +sim.CFLAGS += -DINIT_WIND_Z=-0.0 + sim.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ_SIM) -I../simulator -DFLOAT_T=float @@ -26,7 +31,9 @@ sim.srcs = $(SIMDIR)/booz2_sim_main.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_r250.c \ + $(SIMDIR)/booz_randlcg.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 \ diff --git a/conf/messages.xml b/conf/messages.xml index 451866162c..8580a1add8 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -458,17 +458,17 @@ - + - - - - - - + + + + + + @@ -654,9 +654,9 @@ - - - + + + @@ -666,9 +666,9 @@ - - - + + + diff --git a/conf/settings/settings_booz2.xml b/conf/settings/settings_booz2.xml index 457d02cb96..19e51d32e7 100644 --- a/conf/settings/settings_booz2.xml +++ b/conf/settings/settings_booz2.xml @@ -5,7 +5,7 @@ - + diff --git a/conf/settings/settings_booz2_fms_ts.xml b/conf/settings/settings_booz2_fms_ts.xml index 3b59584d03..0c7844655a 100644 --- a/conf/settings/settings_booz2_fms_ts.xml +++ b/conf/settings/settings_booz2_fms_ts.xml @@ -3,8 +3,8 @@ - - + + diff --git a/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c b/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c index 7206010323..3147bbae4a 100644 --- a/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c +++ b/sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c @@ -174,25 +174,25 @@ void booz2_filter_attitude_update(void) { } -// FIXME +// FIXME : make a real frame change and rotate rates too static inline void apply_alignment(void) { #if 0 booz2_filter_attitude_euler_aligned.phi = - booz2_filter_attitude_euler.phi - + (FILTER_ALIGNMENT_DPSI * booz2_filter_attitude_euler.theta) >> IANGLE_RES - - (FILTER_ALIGNMENT_DTHETA * booz2_filter_attitude_euler.psi) >> IANGLE_RES; + booz2_filter_attitude_euler.phi + + (((int32_t)FILTER_ALIGNMENT_DPSI * booz2_filter_attitude_euler.theta) >> IANGLE_RES) + - (((int32_t)FILTER_ALIGNMENT_DTHETA * booz2_filter_attitude_euler.psi) >> IANGLE_RES); booz2_filter_attitude_euler_aligned.theta = - - (FILTER_ALIGNMENT_DPSI * booz2_filter_attitude_euler.phi) >> IANGLE_RES - + booz2_filter_attitude_euler.theta - + (FILTER_ALIGNMENT_DPHI * booz2_filter_attitude_euler.psi) >> IANGLE_RES; + - (((int32_t)FILTER_ALIGNMENT_DPSI * booz2_filter_attitude_euler.phi) >> IANGLE_RES) + + booz2_filter_attitude_euler.theta + + (((int32_t)FILTER_ALIGNMENT_DPHI * booz2_filter_attitude_euler.psi) >> IANGLE_RES); booz2_filter_attitude_euler_aligned.psi = - (FILTER_ALIGNMENT_DTHETA * booz2_filter_attitude_euler.phi) >> IANGLE_RES - - (FILTER_ALIGNMENT_DPHI * booz2_filter_attitude_euler.theta) >> IANGLE_RES - + booz2_filter_attitude_euler.psi; + (((int32_t)FILTER_ALIGNMENT_DTHETA * booz2_filter_attitude_euler.phi) >> IANGLE_RES) + - (((int32_t)FILTER_ALIGNMENT_DPHI * booz2_filter_attitude_euler.theta) >> IANGLE_RES) + + booz2_filter_attitude_euler.psi; #else booz2_filter_attitude_euler_aligned.phi = booz2_filter_attitude_euler.phi - FILTER_ALIGNMENT_DPHI; diff --git a/sw/airborne/booz/booz2_fms.c b/sw/airborne/booz/booz2_fms.c index cabd626e24..85ea0e8b48 100644 --- a/sw/airborne/booz/booz2_fms.c +++ b/sw/airborne/booz/booz2_fms.c @@ -38,8 +38,11 @@ void booz_fms_periodic(void) { booz_fms_impl_periodic(); } +void booz_fms_set_on_off(bool_t state) { +} + void booz_fms_update_info(void) { PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.gyro, booz_imu_state.gyro); diff --git a/sw/airborne/booz/booz2_fms.h b/sw/airborne/booz/booz2_fms.h index 1858f04217..0b8e570b9a 100644 --- a/sw/airborne/booz/booz2_fms.h +++ b/sw/airborne/booz/booz2_fms.h @@ -80,6 +80,7 @@ extern struct Booz_fms_info booz_fms_info; extern struct Booz_fms_command booz_fms_input; extern void booz_fms_init(void); +extern void booz_fms_set_on_off(bool_t state); extern void booz_fms_periodic(void); extern void booz_fms_update_info(void); @@ -109,6 +110,11 @@ extern void booz_fms_update_info(void); booz_fms_input.h_sp.pos.z = _psi_sp; \ } +#define booz2_fms_SetOnOff(_val) { \ + booz_fms_on = _val; \ + booz_fms_set_on_off(booz_fms_on); \ + } + #endif /* BOOZ2_FMS_H */ diff --git a/sw/airborne/booz/booz2_fms_test_signal.c b/sw/airborne/booz/booz2_fms_test_signal.c index f5bc137746..6abf072677 100644 --- a/sw/airborne/booz/booz2_fms_test_signal.c +++ b/sw/airborne/booz/booz2_fms_test_signal.c @@ -17,24 +17,19 @@ uint32_t booz_fms_test_signal_counter; uint32_t booz_fms_test_signal_start_z; void booz_fms_impl_init(void) { - booz_fms_test_signal_mode = BOOZ_FMS_TEST_SIGNAL_MODE_DISABLED; + booz_fms_test_signal_mode = BOOZ_FMS_TEST_SIGNAL_MODE_ATTITUDE; booz_fms_test_signal_period = BOOZ2_FMS_TEST_SIGNAL_DEFAULT_PERIOD; booz_fms_test_signal_amplitude = BOOZ2_FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE; booz_fms_test_signal_axe = BOOZ2_FMS_TEST_SIGNAL_DEFAULT_AXE; booz_fms_test_signal_counter = 0; booz_fms_input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE; - booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_RC_DIRECT; + booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_HOVER; } void booz_fms_impl_periodic(void) { switch (booz_fms_test_signal_mode) { - case BOOZ_FMS_TEST_SIGNAL_MODE_DISABLED: { - PPRZ_INT32_EULER_ASSIGN(booz_fms_input.h_sp.attitude, 0, 0, 0); - } - break; - case BOOZ_FMS_TEST_SIGNAL_MODE_ATTITUDE: { if (booz_fms_test_signal_counter < booz_fms_test_signal_period) { PPRZ_INT32_EULER_ASSIGN(booz_fms_input.h_sp.attitude, booz_fms_test_signal_amplitude, 0, 0); diff --git a/sw/airborne/booz/booz2_fms_test_signal.h b/sw/airborne/booz/booz2_fms_test_signal.h index efb194fa41..b22ea77263 100644 --- a/sw/airborne/booz/booz2_fms_test_signal.h +++ b/sw/airborne/booz/booz2_fms_test_signal.h @@ -3,9 +3,8 @@ #include "std.h" -#define BOOZ_FMS_TEST_SIGNAL_MODE_DISABLED 0 -#define BOOZ_FMS_TEST_SIGNAL_MODE_ATTITUDE 1 -#define BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL 2 +#define BOOZ_FMS_TEST_SIGNAL_MODE_ATTITUDE 0 +#define BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL 1 extern uint8_t booz_fms_test_signal_mode; @@ -24,11 +23,7 @@ extern void booz_fms_impl_periodic(void); #define booz2_fms_test_signal_SetTsMode(_val) { \ booz_fms_test_signal_mode = _val; \ - if (booz_fms_test_signal_mode == BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL) \ - booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_HOVER; \ - else \ - booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_RC_DIRECT; \ -} + } #endif /* BOOZ_FMS_TEST_SIGNAL_H */ diff --git a/sw/simulator/booz2_sim_main.c b/sw/simulator/booz2_sim_main.c index d48683f81d..5f5f1513a2 100644 --- a/sw/simulator/booz2_sim_main.c +++ b/sw/simulator/booz2_sim_main.c @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -40,9 +41,14 @@ char* fg_host = "10.31.4.107"; unsigned int fg_port = 5501; char* joystick_dev = "/dev/input/js0"; +/* rate of the host mainloop */ +#define HOST_TIMEOUT_PERIOD 4 +struct timeval host_time_start; +double host_time_elapsed; +double host_time_factor = 1.; + /* 250Hz <-> 4ms */ -//#define TIMEOUT_PERIOD 4 -#define DT (1./512.) +#define SIM_DT (1./512.) double sim_time; #define DT_DISPLAY 0.04 @@ -50,7 +56,13 @@ double disp_time; double booz_sim_actuators_values[] = {0., 0., 0., 0.}; -static void sim_bypass_ahrs(void); +static void sim_run_one_step(void); +#ifdef BYPASS_AHRS +static void sim_overwrite_ahrs(void); +#endif +#ifdef BYPASS_INS +static void sim_overwrite_ins(void); +#endif static void sim_gps_feed_data(void); static void sim_mag_feed_data(void); @@ -68,6 +80,7 @@ static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)), static void booz2_sim_init(void) { + gettimeofday (&host_time_start, NULL); sim_time = 0.; disp_time = 0.; @@ -87,80 +100,124 @@ static void booz2_sim_init(void) { #include "booz2_imu.h" static gboolean booz2_sim_periodic(gpointer data __attribute__ ((unused))) { - /* read actuators positions */ - booz_sim_read_actuators(); - /* run our models */ - if (sim_time > 13.) - bfm.on_ground = FALSE; - - booz_wind_model_run(DT); - - booz_flight_model_run(DT, booz_sim_actuators_values); - - booz_sensors_model_run(sim_time); + struct timeval host_time_now; + gettimeofday (&host_time_now, NULL); + host_time_elapsed = host_time_factor * + ((host_time_now.tv_sec - host_time_start.tv_sec) + + (host_time_now.tv_usec - host_time_start.tv_usec)*1e-6); - sim_time += DT; - - /* outputs models state */ - booz2_sim_display(); - - /* run the airborne code */ + while (sim_time <= host_time_elapsed) { + sim_run_one_step(); + sim_time += SIM_DT; + } + - // feed a rc frame and signal event - BoozRcSimFeed(sim_time); - // process it - booz2_main_event(); - - if (booz_sensors_model_baro_available()) { - Booz2BaroISRHandler(bsm.baro); - booz2_main_event(); - } - if (booz_sensors_model_gyro_available()) { - booz2_imu_feed_data(); - booz2_main_event(); -#ifdef BYPASS_AHRS - sim_bypass_ahrs(); -#endif /* BYPASS_AHRS */ - } - - if (booz_sensors_model_gps_available()) { - sim_gps_feed_data(); - booz2_main_event(); - } - - if (booz_sensors_model_mag_available()) { - sim_mag_feed_data(); - booz2_main_event(); - } - - booz2_main_periodic(); - - - + return TRUE; } + -#include "booz_geometry_mixed.h" -#include "booz2_filter_attitude.h" -static void sim_bypass_ahrs(void) { - booz2_filter_attitude_euler_aligned.phi = BOOZ_ANGLE_I_OF_F(bfm.state->ve[BFMS_PHI]); - booz2_filter_attitude_euler_aligned.theta = BOOZ_ANGLE_I_OF_F(bfm.state->ve[BFMS_THETA]); - booz2_filter_attitude_euler_aligned.psi = BOOZ_ANGLE_I_OF_F(bfm.state->ve[BFMS_PSI]); +static void sim_run_one_step(void) { + + /* read actuators positions */ + booz_sim_read_actuators(); + + /* run our models */ + if (sim_time > 13.) + bfm.on_ground = FALSE; + + booz_wind_model_run(SIM_DT); + + booz_flight_model_run(SIM_DT, booz_sim_actuators_values); + + booz_sensors_model_run(sim_time); + + /* outputs models state */ + booz2_sim_display(); + + /* run the airborne code */ + + // feed a rc frame and signal event + BoozRcSimFeed(sim_time); + // process it + booz2_main_event(); + + if (booz_sensors_model_baro_available()) { + Booz2BaroISRHandler(bsm.baro); + booz2_main_event(); +#ifdef BYPASS_INS + sim_overwrite_ins(); +#endif /* BYPASS_INS */ + } + if (booz_sensors_model_gyro_available()) { + booz2_imu_feed_data(); + booz2_main_event(); +#ifdef BYPASS_AHRS + sim_overwrite_ahrs(); +#endif /* BYPASS_AHRS */ +#ifdef BYPASS_INS + sim_overwrite_ins(); +#endif /* BYPASS_INS */ + } + + if (booz_sensors_model_gps_available()) { + sim_gps_feed_data(); + booz2_main_event(); + } + + if (booz_sensors_model_mag_available()) { + sim_mag_feed_data(); + booz2_main_event(); +#ifdef BYPASS_AHRS + sim_overwrite_ahrs(); +#endif /* BYPASS_AHRS */ + } + + booz2_main_periodic(); - booz2_filter_attitude_rate.x = BOOZ_RATE_I_OF_F(bfm.state->ve[BFMS_P]); - booz2_filter_attitude_rate.y = BOOZ_RATE_I_OF_F(bfm.state->ve[BFMS_Q]); - booz2_filter_attitude_rate.z = BOOZ_RATE_I_OF_F(bfm.state->ve[BFMS_R]); } + + + + +#ifdef BYPASS_AHRS +#include "booz_geometry_mixed.h" +#include "booz2_filter_attitude.h" +static void sim_overwrite_ahrs(void) { + booz2_filter_attitude_euler_aligned.phi = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_X]); + booz2_filter_attitude_euler_aligned.theta = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_Y]); + booz2_filter_attitude_euler_aligned.psi = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_Z]); + + booz2_filter_attitude_rate.x = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_X]); + booz2_filter_attitude_rate.y = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_Y]); + booz2_filter_attitude_rate.z = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_Z]); + +} +#endif /* BYPASS_AHRS */ + + +#ifdef BYPASS_INS +#include "booz2_ins.h" +static void sim_overwrite_ins(void) { + booz_ins_position.z = BOOZ_POS_I_OF_F(bfm.pos_ltp->ve[AXIS_Z]); + booz_ins_speed_earth.z = BOOZ_SPEED_I_OF_F(bfm.speed_ltp->ve[AXIS_Z]); + booz_ins_accel_earth.z = BOOZ_ACCEL_I_OF_F(bfm.accel_ltp->ve[AXIS_Z]); +} +#endif /* BYPASS_INS */ + + + + #include "booz2_gps.h" static void sim_gps_feed_data(void) { booz2_gps_lat = bsm.gps_pos_lla.lat; booz2_gps_lon = bsm.gps_pos_lla.lon; // speed? - booz2_gps_vel_n = rint(bsm.gps_speed->ve[AXIS_Y] * 100.); - booz2_gps_vel_e = rint(bsm.gps_speed->ve[AXIS_X] * 100.); + booz2_gps_vel_n = rint(bsm.gps_speed->ve[AXIS_X] * 100.); + booz2_gps_vel_e = rint(bsm.gps_speed->ve[AXIS_Y] * 100.); booz_gps_state.fix = BOOZ2_GPS_FIX_3D; } @@ -169,16 +226,20 @@ static void sim_mag_feed_data(void) { #ifdef IMU_MAG_45_HACK // booz2_imu_mag.x = msx - msy; // booz2_imu_mag.y = msx + msy; - int32_t foo_x = bsm.mag->ve[AXIS_X] * cos(M_PI_4); - int32_t foo_y = bsm.mag->ve[AXIS_Y] * cos(M_PI_4); - ami601_val[IMU_MAG_X_CHAN] = foo_x + foo_y; - ami601_val[IMU_MAG_Y_CHAN] = -foo_x + foo_y; + // double cos_pi_4 = cos(M_PI_4); + // int32_t foo_x = bsm.mag->ve[AXIS_X];// / cos(M_PI_4); + // int32_t foo_y = bsm.mag->ve[AXIS_Y];// / cos(M_PI_4); + ami601_val[IMU_MAG_X_CHAN] = bsm.mag->ve[AXIS_X]; + ami601_val[IMU_MAG_Y_CHAN] = bsm.mag->ve[AXIS_Y]; + // ami601_val[IMU_MAG_X_CHAN] = foo_x + foo_y; + // ami601_val[IMU_MAG_Y_CHAN] = -foo_x + foo_y; ami601_val[IMU_MAG_Z_CHAN] = bsm.mag->ve[AXIS_Z]; #else ami601_val[IMU_MAG_X_CHAN] = bsm.mag->ve[AXIS_X]; ami601_val[IMU_MAG_Y_CHAN] = bsm.mag->ve[AXIS_Y]; ami601_val[IMU_MAG_Z_CHAN] = bsm.mag->ve[AXIS_Z]; #endif + ami601_status = AMI601_DATA_AVAILABLE; } @@ -190,26 +251,26 @@ static void booz2_sim_display(void) { // booz_flightgear_send(); IvySendMsg("%d BOOZ_SIM_RPMS %f %f %f %f", AC_ID, - 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]) ); + RPM_OF_RAD_S(bfm.omega->ve[SERVO_FRONT]), + RPM_OF_RAD_S(bfm.omega->ve[SERVO_BACK]), + RPM_OF_RAD_S(bfm.omega->ve[SERVO_RIGHT]), + RPM_OF_RAD_S(bfm.omega->ve[SERVO_LEFT]) ); IvySendMsg("%d BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f", AC_ID, - DegOfRad(bfm.state->ve[BFMS_P]), - DegOfRad(bfm.state->ve[BFMS_Q]), - DegOfRad(bfm.state->ve[BFMS_R]), - DegOfRad(bfm.state->ve[BFMS_PHI]), - DegOfRad(bfm.state->ve[BFMS_THETA]), - DegOfRad(bfm.state->ve[BFMS_PSI])); + DegOfRad(bfm.ang_rate_body->ve[AXIS_X]), + DegOfRad(bfm.ang_rate_body->ve[AXIS_Y]), + DegOfRad(bfm.ang_rate_body->ve[AXIS_Z]), + DegOfRad(bfm.eulers->ve[AXIS_X]), + DegOfRad(bfm.eulers->ve[AXIS_Y]), + DegOfRad(bfm.eulers->ve[AXIS_Z])); IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f", AC_ID, - (bfm.state->ve[BFMS_U]), - (bfm.state->ve[BFMS_V]), - (bfm.state->ve[BFMS_W]), - (bfm.state->ve[BFMS_X]), - (bfm.state->ve[BFMS_Y]), - (bfm.state->ve[BFMS_Z])); + (bfm.speed_ltp->ve[AXIS_X]), + (bfm.speed_ltp->ve[AXIS_Y]), + (bfm.speed_ltp->ve[AXIS_Z]), + (bfm.pos_ltp->ve[AXIS_X]), + (bfm.pos_ltp->ve[AXIS_Y]), + (bfm.pos_ltp->ve[AXIS_Z])); IvySendMsg("%d BOOZ_SIM_WIND %f %f %f", AC_ID, bwm.velocity->ve[AXIS_X], @@ -226,7 +287,7 @@ int main ( int argc, char** argv) { GMainLoop *ml = g_main_loop_new(NULL, FALSE); - g_timeout_add(4, booz2_sim_periodic, NULL); + g_timeout_add(HOST_TIMEOUT_PERIOD, booz2_sim_periodic, NULL); g_main_loop_run(ml); diff --git a/sw/simulator/booz_flight_model.c b/sw/simulator/booz_flight_model.c index e9e9770113..716eb04015 100644 --- a/sw/simulator/booz_flight_model.c +++ b/sw/simulator/booz_flight_model.c @@ -1,9 +1,60 @@ #include "booz_flight_model.h" +#define BFMS_X 0 +#define BFMS_Y 1 +#define BFMS_Z 2 +#define BFMS_XD 3 +#define BFMS_YD 4 +#define BFMS_ZD 5 +#define BFMS_PHI 6 +#define BFMS_THETA 7 +#define BFMS_PSI 8 +#define BFMS_P 9 +#define BFMS_Q 10 +#define BFMS_R 11 +#define BFMS_OM_B 12 +#define BFMS_OM_F 13 +#define BFMS_OM_R 14 +#define BFMS_OM_L 15 +#define BFMS_SIZE 16 + +#define BoozFlighModelGetPos(_dest) { \ + _dest->ve[AXIS_X] = bfm.state->ve[BFMS_X]; \ + _dest->ve[AXIS_Y] = bfm.state->ve[BFMS_Y]; \ + _dest->ve[AXIS_Z] = bfm.state->ve[BFMS_Z]; \ + } + +#define BoozFlighModelGetSpeedLtp(_dest) { \ + _dest->ve[AXIS_X] = bfm.state->ve[BFMS_XD]; \ + _dest->ve[AXIS_Y] = bfm.state->ve[BFMS_YD]; \ + _dest->ve[AXIS_Z] = bfm.state->ve[BFMS_ZD]; \ + } + +#define BoozFlighModelGetAngles(_dest) { \ + _dest->ve[EULER_PHI] = bfm.state->ve[BFMS_PHI]; \ + _dest->ve[EULER_THETA] = bfm.state->ve[BFMS_THETA]; \ + _dest->ve[EULER_PSI] = bfm.state->ve[BFMS_PSI]; \ + } + +#define BoozFlighModelGetRate(_dest) { \ + _dest->ve[AXIS_P] = bfm.state->ve[BFMS_P]; \ + _dest->ve[AXIS_Q] = bfm.state->ve[BFMS_Q]; \ + _dest->ve[AXIS_R] = bfm.state->ve[BFMS_R]; \ + } + +#define BoozFlighModelGetRPMS(_dest) { \ + _dest->ve[SERVO_BACK] = bfm.state->ve[BFMS_OM_B]; \ + _dest->ve[SERVO_FRONT] = bfm.state->ve[BFMS_OM_F]; \ + _dest->ve[SERVO_RIGHT] = bfm.state->ve[BFMS_OM_R]; \ + _dest->ve[SERVO_LEFT] = bfm.state->ve[BFMS_OM_L]; \ + } + + #include #include "booz_flight_model_params.h" #include "booz_flight_model_utils.h" +#include "booz_wind_model.h" #include "6dof.h" @@ -12,7 +63,8 @@ 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 void booz_flight_model_update_byproducts(void); +static VEC* booz_get_forces_ltp(VEC* F , VEC* speed_ltp, MAT* dcm_t, VEC* omega_square); 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); @@ -24,18 +76,37 @@ void booz_flight_model_init( void ) { bfm.mot_voltage = v_get(SERVOS_NB); bfm.state = v_get(BFMS_SIZE); + + bfm.pos_ltp = v_get(AXIS_NB); + bfm.speed_ltp = v_get(AXIS_NB); + bfm.accel_ltp = v_get(AXIS_NB); + + bfm.speed_body = v_get(AXIS_NB); + bfm.accel_body = v_get(AXIS_NB); + + bfm.eulers = v_get(AXIS_NB); + bfm.ang_rate_body = v_get(AXIS_NB); + bfm.ang_accel_body = v_get(AXIS_NB); + + bfm.dcm = m_get(AXIS_NB, AXIS_NB); + bfm.dcm_t = m_get(AXIS_NB, AXIS_NB); + bfm.quat = v_get(AXIS_NB); + + bfm.omega = v_get(SERVOS_NB); + bfm.omega_square = v_get(SERVOS_NB); + /* constants */ - bfm.g_earth = v_get(AXIS_NB); - bfm.g_earth->ve[AXIS_X] = 0.; - bfm.g_earth->ve[AXIS_Y] = 0.; - bfm.g_earth->ve[AXIS_Z] = G; + bfm.g_ltp = v_get(AXIS_NB); + bfm.g_ltp->ve[AXIS_X] = 0.; + bfm.g_ltp->ve[AXIS_Y] = 0.; + bfm.g_ltp->ve[AXIS_Z] = G; /* FIXME */ - bfm.h_earth = v_get(AXIS_NB); - bfm.h_earth->ve[AXIS_X] = 1.; - bfm.h_earth->ve[AXIS_Y] = 0.; - bfm.h_earth->ve[AXIS_Z] = 1.; + bfm.h_ltp = v_get(AXIS_NB); + bfm.h_ltp->ve[AXIS_X] = 1.; + bfm.h_ltp->ve[AXIS_Y] = 0.; + bfm.h_ltp->ve[AXIS_Z] = 1.; bfm.thrust_factor = 0.5 * RHO * PROP_AREA * C_t * PROP_RADIUS * PROP_RADIUS; bfm.torque_factor = 0.5 * RHO * PROP_AREA * C_q * PROP_RADIUS * PROP_RADIUS; @@ -82,36 +153,79 @@ void booz_flight_model_run( double dt, double* commands ) { 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); - + booz_flight_model_update_byproducts(); bfm.time += dt; } +static void booz_flight_model_update_byproducts(void) { + + /* extract eulers angles from state */ + BoozFlighModelGetAngles( bfm.eulers); + /* extract body rotational rates from state */ + BoozFlighModelGetRate( bfm.ang_rate_body); + + /* direct cosine matrix ( inertial to body )*/ + dcm_of_eulers(bfm.eulers, bfm.dcm); + /* transpose of dcm ( body to inertial ) */ + m_transp(bfm.dcm, bfm.dcm_t); + + BoozFlighModelGetPos(bfm.pos_ltp); + /* extract speed in ltp frame from state */ + BoozFlighModelGetSpeedLtp(bfm.speed_ltp); + /* extract prop rotational speeds from state */ + BoozFlighModelGetRPMS(bfm.omega); + /* compute square */ + v_star(bfm.omega, bfm.omega, bfm.omega_square); + /* compute ltp accelerations */ + static VEC *f_ltp = VNULL; + f_ltp = v_resize(f_ltp, AXIS_NB); + f_ltp = booz_get_forces_ltp(f_ltp , bfm.speed_ltp, bfm.dcm_t, bfm.omega_square); + sv_mlt( 1./bfm.mass, f_ltp, bfm.accel_ltp); + /* rotate speed and accel to body frame */ + mv_mlt(bfm.dcm, bfm.speed_ltp, bfm.speed_body); + mv_mlt(bfm.dcm, bfm.accel_ltp, bfm.accel_body); + + + /* rotational accelerations */ + /* quaternion */ + + +} + + + /* compute the sum of external forces. assumes that dcm and omega_square are already precomputed from X */ -VEC* booz_get_forces_body_frame(VEC* F , MAT* dcm, VEC* omega_square, VEC* speed_body) { +static VEC* booz_get_forces_ltp(VEC* F , VEC* speed_ltp, MAT* dcm_t, VEC* omega_square) { // FIXME : nimporte koi ! - if (bfm.on_ground) { - F->ve[AXIS_X] = 0; - F->ve[AXIS_Y] = 0; - F->ve[AXIS_Z] = 0; - } - else { + F = v_zero(F); + if (!bfm.on_ground) { + // propeller thrust - F->ve[AXIS_X] = 0; - F->ve[AXIS_Y] = 0; - F->ve[AXIS_Z] = -v_sum(omega_square) * bfm.thrust_factor; + static VEC *prop_thrust_body = VNULL; + prop_thrust_body = v_resize(prop_thrust_body, AXIS_NB); + prop_thrust_body->ve[AXIS_X] = 0; + prop_thrust_body->ve[AXIS_Y] = 0; + prop_thrust_body->ve[AXIS_Z] = -v_sum(omega_square) * bfm.thrust_factor; + static VEC *prop_thrust_ltp = VNULL; + prop_thrust_ltp = v_resize(prop_thrust_ltp, AXIS_NB); + prop_thrust_ltp = mv_mlt(dcm_t, prop_thrust_body, prop_thrust_ltp); + F = v_add(F, prop_thrust_ltp, F); + // 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, bfm.mass, F); - // body drag - double norm_speed = v_norm2(speed_body); - F = v_mltadd(F, speed_body, -norm_speed * C_d_body, F); + F = v_mltadd(F, bfm.g_ltp, bfm.mass, F); + + // drag + static VEC *airspeed_ltp = VNULL; + airspeed_ltp = v_resize(airspeed_ltp, AXIS_NB); + airspeed_ltp = v_sub(speed_ltp, bwm.velocity, airspeed_ltp); + double norm_speed = v_norm2(airspeed_ltp); + F = v_mltadd(F, airspeed_ltp, -norm_speed * C_d_body, F); + } return F; } @@ -149,40 +263,27 @@ 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); - BoozFlighModelGetSpeed(speed_body); + /* extract ltp_speeds_from state */ + static VEC *speed_ltp = VNULL; + speed_ltp = v_resize(speed_ltp, AXIS_NB); + BoozFlighModelGetSpeedLtp(speed_ltp); /* extracts body rates from state */ static VEC *rate_body = VNULL; rate_body = v_resize(rate_body, AXIS_NB); BoozFlighModelGetRate(rate_body); /* derivatives of position */ - static VEC *speed_earth = VNULL; - speed_earth = v_resize(speed_earth, AXIS_NB); - speed_earth = mv_mlt(dcm_t, speed_body,speed_earth); - Xdot->ve[BFMS_X] = speed_earth->ve[AXIS_X]; - Xdot->ve[BFMS_Y] = speed_earth->ve[AXIS_Y]; - Xdot->ve[BFMS_Z] = speed_earth->ve[AXIS_Z]; + Xdot->ve[BFMS_X] = speed_ltp->ve[AXIS_X]; + Xdot->ve[BFMS_Y] = speed_ltp->ve[AXIS_Y]; + Xdot->ve[BFMS_Z] = speed_ltp->ve[AXIS_Z]; /* derivatives of speed */ - - /* 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); - - /* add 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_body = v_add(f_body, fict_f, f_body); - - Xdot->ve[BFMS_U] = 1./bfm.mass * f_body->ve[AXIS_X]; - Xdot->ve[BFMS_V] = 1./bfm.mass * f_body->ve[AXIS_Y]; - Xdot->ve[BFMS_W] = 1./bfm.mass * f_body->ve[AXIS_Z]; + static VEC *f_ltp = VNULL; + f_ltp = v_resize(f_ltp, AXIS_NB); + f_ltp = booz_get_forces_ltp(f_ltp , speed_ltp, dcm_t, omega_square); + Xdot->ve[BFMS_XD] = 1./bfm.mass * f_ltp->ve[AXIS_X]; + Xdot->ve[BFMS_YD] = 1./bfm.mass * f_ltp->ve[AXIS_Y]; + Xdot->ve[BFMS_ZD] = 1./bfm.mass * f_ltp->ve[AXIS_Z]; /* derivatives of eulers */ double sinPHI = sin(eulers->ve[EULER_PHI]); diff --git a/sw/simulator/booz_flight_model.h b/sw/simulator/booz_flight_model.h index dbf14fc282..41935faefa 100644 --- a/sw/simulator/booz_flight_model.h +++ b/sw/simulator/booz_flight_model.h @@ -5,24 +5,6 @@ #include "airframe.h" -#define BFMS_X 0 -#define BFMS_Y 1 -#define BFMS_Z 2 -#define BFMS_U 3 -#define BFMS_V 4 -#define BFMS_W 5 -#define BFMS_PHI 6 -#define BFMS_THETA 7 -#define BFMS_PSI 8 -#define BFMS_P 9 -#define BFMS_Q 10 -#define BFMS_R 11 -#define BFMS_OM_B 12 -#define BFMS_OM_F 13 -#define BFMS_OM_R 14 -#define BFMS_OM_L 15 -#define BFMS_SIZE 16 - struct BoozFlightModel { /* are we flying ? */ @@ -34,14 +16,31 @@ struct BoozFlightModel { /* motors supply voltage in V */ VEC* mot_voltage; - /* state : see define above for fields */ + /* private state : see defines in .c for fields */ VEC* state; + /* user products */ + VEC* pos_ltp; + VEC* speed_ltp; + VEC* accel_ltp; + VEC* speed_body; + VEC* accel_body; + + VEC* eulers; + VEC* ang_rate_body; + VEC* ang_accel_body; + MAT* dcm; + MAT* dcm_t; + VEC* quat; + + VEC* omega; + VEC* omega_square; + /* constants used in derivative computation */ /* magnetic field in earth frame */ - VEC* h_earth; + VEC* h_ltp; /* gravitation in earth frame */ - VEC* g_earth; + VEC* g_ltp; /* propeller thrust factor */ double thrust_factor; /* propeller torque factor */ @@ -61,37 +60,5 @@ 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* F, MAT* dcm, VEC* omega_square, VEC* speed_body); - -#define BoozFlighModelGetPos(_dest) { \ - _dest->ve[AXIS_X] = bfm.state->ve[BFMS_X]; \ - _dest->ve[AXIS_Y] = bfm.state->ve[BFMS_Y]; \ - _dest->ve[AXIS_Z] = bfm.state->ve[BFMS_Z]; \ - } - -#define BoozFlighModelGetSpeed(_dest) { \ - _dest->ve[AXIS_X] = bfm.state->ve[BFMS_U]; \ - _dest->ve[AXIS_Y] = bfm.state->ve[BFMS_V]; \ - _dest->ve[AXIS_Z] = bfm.state->ve[BFMS_W]; \ - } - -#define BoozFlighModelGetAngles(_dest) { \ - _dest->ve[EULER_PHI] = bfm.state->ve[BFMS_PHI]; \ - _dest->ve[EULER_THETA] = bfm.state->ve[BFMS_THETA]; \ - _dest->ve[EULER_PSI] = bfm.state->ve[BFMS_PSI]; \ - } - -#define BoozFlighModelGetRate(_dest) { \ - _dest->ve[AXIS_P] = bfm.state->ve[BFMS_P]; \ - _dest->ve[AXIS_Q] = bfm.state->ve[BFMS_Q]; \ - _dest->ve[AXIS_R] = bfm.state->ve[BFMS_R]; \ - } - -#define BoozFlighModelGetRPMS(_dest) { \ - _dest->ve[SERVO_BACK] = bfm.state->ve[BFMS_OM_B]; \ - _dest->ve[SERVO_FRONT] = bfm.state->ve[BFMS_OM_F]; \ - _dest->ve[SERVO_RIGHT] = bfm.state->ve[BFMS_OM_R]; \ - _dest->ve[SERVO_LEFT] = bfm.state->ve[BFMS_OM_L]; \ - } #endif /* BOOZ_FLIGHT_MODEL_H */ diff --git a/sw/simulator/booz_flight_model_params.h b/sw/simulator/booz_flight_model_params.h index cd20bd28bc..ae80ccf655 100644 --- a/sw/simulator/booz_flight_model_params.h +++ b/sw/simulator/booz_flight_model_params.h @@ -2,7 +2,7 @@ #define BOOZ_FLIGHT_MODEL_PARAMS_H /* body drag coefficient */ -#define C_d_body .0005 +#define C_d_body .0075 /* propeller thrust aerodynamic coefficient */ #define C_t 0.297 /* propeller moment aerodynamic coefficient */ diff --git a/sw/simulator/booz_r250.c b/sw/simulator/booz_r250.c new file mode 100644 index 0000000000..528d0eb12d --- /dev/null +++ b/sw/simulator/booz_r250.c @@ -0,0 +1,245 @@ +/* r250.c the r250 uniform random number algorithm + + Kirkpatrick, S., and E. Stoll, 1981; "A Very Fast + Shift-Register Sequence Random Number Generator", + Journal of Computational Physics, V.40 + + also: + + see W.L. Maier, DDJ May 1991 + + + +*/ + +#include + +#include "booz_r250.h" + +/* set the following if you trust rand(), otherwise the minimal standard + generator is used +*/ +/* #define TRUST_RAND */ + + +#ifndef TRUST_RAND +#include "booz_randlcg.h" +#endif + +/* defines to allow for 16 or 32 bit integers */ +#define BITS 31 + + +#if WORD_BIT == 32 +#ifndef BITS +#define BITS 32 +#endif +#else +#ifndef BITS +#define BITS 16 +#endif +#endif + +#if BITS == 31 +#define MSB 0x40000000L +#define ALL_BITS 0x7fffffffL +#define HALF_RANGE 0x20000000L +#define STEP 7 +#endif + +#if BITS == 32 +#define MSB 0x80000000L +#define ALL_BITS 0xffffffffL +#define HALF_RANGE 0x40000000L +#define STEP 7 +#endif + +#if BITS == 16 +#define MSB 0x8000 +#define ALL_BITS 0xffff +#define HALF_RANGE 0x4000 +#define STEP 11 +#endif + +static unsigned int r250_buffer[ 250 ]; +static int r250_index; + +#ifdef NO_PROTO +void r250_init(sd) +int seed; +#else +void r250_init(int sd) +#endif +{ + int j, k; + unsigned int mask, msb; + +#ifdef TRUST_RAND + +#if BITS == 32 || BITS == 31 + srand48( sd ); +#else + srand( sd ); +#endif + + +#else + set_seed( sd ); +#endif + + r250_index = 0; + for (j = 0; j < 250; j++) /* fill r250 buffer with BITS-1 bit values */ +#ifdef TRUST_RAND +#if BITS == 32 || BITS == 31 + r250_buffer[j] = (unsigned int)lrand48(); +#else + r250_buffer[j] = rand(); +#endif +#else + r250_buffer[j] = randlcg(); +#endif + + + for (j = 0; j < 250; j++) /* set some MSBs to 1 */ +#ifdef TRUST_RAND + if ( rand() > HALF_RANGE ) + r250_buffer[j] |= MSB; +#else + if ( randlcg() > HALF_RANGE ) + r250_buffer[j] |= MSB; +#endif + + + msb = MSB; /* turn on diagonal bit */ + mask = ALL_BITS; /* turn off the leftmost bits */ + + for (j = 0; j < BITS; j++) + { + k = STEP * j + 3; /* select a word to operate on */ + r250_buffer[k] &= mask; /* turn off bits left of the diagonal */ + r250_buffer[k] |= msb; /* turn on the diagonal bit */ + mask >>= 1; + msb >>= 1; + } + +} + +unsigned int r250() /* returns a random unsigned integer */ +{ + register int j; + register unsigned int new_rand; + + if ( r250_index >= 147 ) + j = r250_index - 147; /* wrap pointer around */ + else + j = r250_index + 103; + + new_rand = r250_buffer[ r250_index ] ^ r250_buffer[ j ]; + r250_buffer[ r250_index ] = new_rand; + + if ( r250_index >= 249 ) /* increment pointer for next time */ + r250_index = 0; + else + r250_index++; + + return new_rand; + +} + + +double dr250() /* returns a random double in range 0..1 */ +{ + register int j; + register unsigned int new_rand; + + if ( r250_index >= 147 ) + j = r250_index - 147; /* wrap pointer around */ + else + j = r250_index + 103; + + new_rand = r250_buffer[ r250_index ] ^ r250_buffer[ j ]; + r250_buffer[ r250_index ] = new_rand; + + if ( r250_index >= 249 ) /* increment pointer for next time */ + r250_index = 0; + else + r250_index++; + + return (double)new_rand / ALL_BITS; + +} + +#ifdef MAIN + +/* test driver prints out either NMR_RAND values or a histogram */ + +#include + +#define NMR_RAND 5000 +#define MAX_BINS 500 + +#ifdef NO_PROTO +void main(argc, argv) +int argc; +char **argv; +#else +void main(int argc, char **argv) +#endif +{ + int j,k,nmr_bins,seed; + int bins[MAX_BINS]; + double randm, bin_inc; + double bin_limit[MAX_BINS]; + + if ( argc != 3 ) + { + printf("Usage -- %s nmr_bins seed\n", argv[0]); + exit(1); + } + + nmr_bins = atoi( argv[1] ); + if ( nmr_bins > MAX_BINS ) + { + printf("ERROR -- maximum number of bins is %d\n", MAX_BINS); + exit(1); + } + + seed = atoi( argv[2] ); + + r250_init( seed ); + + if ( nmr_bins < 1 ) /* just print out the numbers */ + { + for (j = 0; j < NMR_RAND; j++) + printf("%f\n", dr250() ); + exit(0); + } + + bin_inc = 1.0 / nmr_bins; + for (j = 0; j < nmr_bins; j++) /* initialize bins to zero */ + { + bins[j] = 0; + bin_limit[j] = (j + 1) * bin_inc; + } + + bin_limit[nmr_bins-1] = 1.0e7; /* make sure all others are in last bin */ + + for (j = 0; j < NMR_RAND; j++) + { + randm = r250() / (double)ALL_BITS; + for (k = 0; k < nmr_bins; k++) + if ( randm < bin_limit[k] ) + { + bins[k]++; + break; + } + } + + + for (j = 0; j < nmr_bins; j++) + printf("%d\n", bins[j]); + +} + +#endif + diff --git a/sw/simulator/booz_r250.h b/sw/simulator/booz_r250.h new file mode 100644 index 0000000000..8455e534b8 --- /dev/null +++ b/sw/simulator/booz_r250.h @@ -0,0 +1,21 @@ +/* r250.h prototypes for r250 random number generator, + + Kirkpatrick, S., and E. Stoll, 1981; "A Very Fast + Shift-Register Sequence Random Number Generator", + Journal of Computational Physics, V.40 + + also: + + see W.L. Maier, DDJ May 1991 + + +*/ + +#ifndef _R250_H_ +#define _R250_H_ 1.2 + +void r250_init(int seed); +unsigned int r250( void ); +double dr250( void ); + +#endif diff --git a/sw/simulator/booz_randlcg.c b/sw/simulator/booz_randlcg.c new file mode 100644 index 0000000000..ae28970b62 --- /dev/null +++ b/sw/simulator/booz_randlcg.c @@ -0,0 +1,51 @@ +/* rndlcg Linear Congruential Method, the "minimal standard generator" + Park & Miller, 1988, Comm of the ACM, 31(10), pp. 1192-1201 + +*/ + +#include +#include + +#define ALL_BITS 0xffffffff + +static long int quotient = LONG_MAX / 16807L; +static long int my_remainder = LONG_MAX % 16807L; + +static long int seed_val = 1L; + +long set_seed(long int sd) +{ + return seed_val = sd; +} + +long get_seed() +{ + return seed_val; +} + + +unsigned long int randlcg() /* returns a random unsigned integer */ +{ + if ( seed_val <= quotient ) + seed_val = (seed_val * 16807L) % LONG_MAX; + else + { + long int high_part = seed_val / quotient; + long int low_part = seed_val % quotient; + + long int test = 16807L * low_part - my_remainder * high_part; + + if ( test > 0 ) + seed_val = test; + else + seed_val = test + LONG_MAX; + + } + + return seed_val; +} + + + + + diff --git a/sw/simulator/booz_randlcg.h b/sw/simulator/booz_randlcg.h new file mode 100644 index 0000000000..7093ff3cfa --- /dev/null +++ b/sw/simulator/booz_randlcg.h @@ -0,0 +1,19 @@ + +/* randlcg.h prototypes for the minimal standard random number generator, + + Linear Congruential Method, the "minimal standard generator" + Park & Miller, 1988, Comm of the ACM, 31(10), pp. 1192-1201 + + + rcsid: @(#)randlcg.h 1.1 15:48:09 11/21/94 EFC + +*/ + +#ifndef _RANDLCG_H_ +#define _RANDLCG_H_ 1.1 + +long set_seed(long); +long get_seed(long); +unsigned long int randlcg(); + +#endif diff --git a/sw/simulator/booz_rc_sim.h b/sw/simulator/booz_rc_sim.h index fb9d39083b..c74bf84cb0 100644 --- a/sw/simulator/booz_rc_sim.h +++ b/sw/simulator/booz_rc_sim.h @@ -22,7 +22,7 @@ else { \ ppm_pulses[RADIO_YAW] = 1500 + 0. * (2050-950); \ ppm_pulses[RADIO_THROTTLE] = 1223 + 0.99 * (2050-1223); \ - ppm_pulses[RADIO_MODE] = MODE_SWITCH_AUTO1; \ + ppm_pulses[RADIO_MODE] = MODE_SWITCH_AUTO2; \ } \ ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \ ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \ diff --git a/sw/simulator/booz_sensors_model.c b/sw/simulator/booz_sensors_model.c index 5684785a5c..9bd0223111 100644 --- a/sw/simulator/booz_sensors_model.c +++ b/sw/simulator/booz_sensors_model.c @@ -1,5 +1,5 @@ #include "booz_sensors_model.h" -#include "booz_sensors_model_params.h" +#include BSM_PARAMS #include @@ -10,13 +10,13 @@ struct BoozSensorsModel bsm; extern void booz_sensors_model_accel_init(double time); -extern void booz_sensors_model_accel_run(double time, MAT* dcm); +extern void booz_sensors_model_accel_run(double time); extern void booz_sensors_model_gyro_init(double time); extern void booz_sensors_model_gyro_run(double time); extern void booz_sensors_model_mag_init(double time); -extern void booz_sensors_model_mag_run(double time, MAT* dcm); +extern void booz_sensors_model_mag_run(double time); extern void booz_sensors_model_rangemeter_init(double time); extern void booz_sensors_model_rangemeter_run(double time); @@ -25,41 +25,36 @@ extern void booz_sensors_model_baro_init(double time); extern void booz_sensors_model_baro_run(double time); extern void booz_sensors_model_gps_init(double time); -extern void booz_sensors_model_gps_run(double time, MAT* dcm_t); +extern void booz_sensors_model_gps_run(double time); void booz_sensors_model_init(double time) { + + VEC* tmp_eulers = v_get(AXIS_NB); + tmp_eulers->ve[EULER_PHI] = BSM_BODY_TO_IMU_PHI; + tmp_eulers->ve[EULER_THETA] = BSM_BODY_TO_IMU_THETA; + tmp_eulers->ve[EULER_PSI] = BSM_BODY_TO_IMU_PSI; + bsm.body_to_imu = m_get(AXIS_NB, AXIS_NB); + dcm_of_eulers (tmp_eulers, bsm.body_to_imu ); + 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 time) { - /* 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); - /* transpose of dcm ( body to inertial ) */ - static MAT *dcm_t = MNULL; - dcm_t = m_resize(dcm_t,AXIS_NB, AXIS_NB); - dcm_t = m_transp(dcm, dcm_t); - - booz_sensors_model_accel_run(time, dcm); + booz_sensors_model_accel_run(time); booz_sensors_model_gyro_run(time); - booz_sensors_model_mag_run(time, dcm); + booz_sensors_model_mag_run(time); booz_sensors_model_rangemeter_run(time); booz_sensors_model_baro_run(time); - booz_sensors_model_gps_run(time, dcm_t); + booz_sensors_model_gps_run(time); + } diff --git a/sw/simulator/booz_sensors_model.h b/sw/simulator/booz_sensors_model.h index a2b7878db0..37477e96b8 100644 --- a/sw/simulator/booz_sensors_model.h +++ b/sw/simulator/booz_sensors_model.h @@ -43,6 +43,9 @@ extern bool_t booz_sensors_model_gps_available(); struct BoozSensorsModel { + /* our imu alignement */ + MAT* body_to_imu; + /* Accelerometer */ VEC* accel; unsigned int accel_resolution; @@ -69,7 +72,7 @@ struct BoozSensorsModel { /* Magnetometer */ VEC* mag; - unsigned int mag_resolution; + MAT* mag_imu_to_sensor; MAT* mag_sensitivity; VEC* mag_neutral; VEC* mag_noise_std_dev; diff --git a/sw/simulator/booz_sensors_model_accel.c b/sw/simulator/booz_sensors_model_accel.c index d8805e1571..d87165c2fd 100644 --- a/sw/simulator/booz_sensors_model_accel.c +++ b/sw/simulator/booz_sensors_model_accel.c @@ -49,60 +49,30 @@ void booz_sensors_model_accel_init(double time) { } -void booz_sensors_model_accel_run( double time, MAT* dcm ) { +void booz_sensors_model_accel_run( double time ) { if (time < bsm.accel_next_update) return; - /* */ + static VEC* accel_ltp = VNULL; + accel_ltp = v_resize(accel_ltp, AXIS_NB); + /* substract gravity to acceleration in ltp frame */ + accel_ltp = v_sub(bfm.accel_ltp, bfm.g_ltp, accel_ltp); + /* convert to body frame */ 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 */ - - /* 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); - - /* divide by mass */ - accel_body = sv_mlt(1./bfm.mass, accel_body, accel_body); + mv_mlt(bfm.dcm, accel_ltp, accel_body); + /* convert to imu frame */ + static VEC* accel_imu = VNULL; + accel_imu = v_resize(accel_imu, AXIS_NB); + mv_mlt(bsm.body_to_imu, accel_body, accel_imu); - 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); - -#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]); - - - accel_body = mv_mlt(dcm, g_inert, accel_body); - accel_body = sv_mlt(-1., accel_body, accel_body); -#endif // 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); + mv_mlt(bsm.accel_sensitivity, accel_imu, bsm.accel); + v_add(bsm.accel, bsm.accel_neutral, bsm.accel); /* compute accel error readings */ static VEC *accel_error = VNULL; diff --git a/sw/simulator/booz_sensors_model_baro.c b/sw/simulator/booz_sensors_model_baro.c index b4ac3ac0ae..afc1c9b86e 100644 --- a/sw/simulator/booz_sensors_model_baro.c +++ b/sw/simulator/booz_sensors_model_baro.c @@ -28,7 +28,7 @@ void booz_sensors_model_baro_run( double time ) { if (time < 12.5) bsm.baro = 840; else { - double z = bfm.state->ve[BFMS_Z]; + double z = bfm.pos_ltp->ve[AXIS_Z] + get_gaussian_noise()*BSM_BARO_NOISE_STD_DEV; // double p = ( z / 0.084 ) + BSM_BARO_QNH; // double baro_reading = p * BSM_BARO_SENSITIVITY; double baro_reading = BSM_BARO_QNH + z * BSM_BARO_SENSITIVITY; diff --git a/sw/simulator/booz_sensors_model_gps.c b/sw/simulator/booz_sensors_model_gps.c index 9f497657bb..fe287a239a 100644 --- a/sw/simulator/booz_sensors_model_gps.c +++ b/sw/simulator/booz_sensors_model_gps.c @@ -58,21 +58,16 @@ void booz_sensors_model_gps_init( double time ) { } /* We store our positions like the fdm and convert to UTM and funcky course, gspeed, climb */ -void booz_sensors_model_gps_run( double time, MAT* dcm_t ) { +void booz_sensors_model_gps_run( double time) { 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); + v_copy(bfm.speed_ltp, 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); @@ -90,12 +85,8 @@ void booz_sensors_model_gps_run( double time, MAT* dcm_t ) { /* * 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 */ + /* compute position error */ static VEC *pos_error = VNULL; pos_error = v_resize(pos_error, AXIS_NB); pos_error = v_zero(pos_error); @@ -106,10 +97,13 @@ void booz_sensors_model_gps_run( double time, MAT* dcm_t ) { 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); + /* add it */ 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); - + /* sum true pos and error reading */ + static VEC *cur_pos_reading = VNULL; + cur_pos_reading = v_resize(cur_pos_reading, AXIS_NB); + v_add(bfm.pos_ltp, pos_error, cur_pos_reading); + /* store that for later and retrieve a previously stored data */ UpdateSensorLatency(time, cur_pos_reading, bsm.gps_pos_history, BSM_GPS_POS_LATENCY, bsm.gps_pos); /* UTM conversion */ @@ -126,9 +120,9 @@ void booz_sensors_model_gps_run( double time, MAT* dcm_t ) { #define LON0 1. #define GROUND_ALT 180. - bsm.gps_pos_lla.lat = (bsm.gps_pos->ve[AXIS_Y] * 9e-6 + LAT0) * 1e7; + bsm.gps_pos_lla.lat = (bsm.gps_pos->ve[AXIS_X] * 9e-6 + LAT0) * 1e7; bsm.gps_pos_lla.lat = rint(bsm.gps_pos_lla.lat); - bsm.gps_pos_lla.lon = (bsm.gps_pos->ve[AXIS_X] * 9e-6 + LON0) * 1e7; + bsm.gps_pos_lla.lon = (bsm.gps_pos->ve[AXIS_Y] * 9e-6 + LON0) * 1e7; bsm.gps_pos_lla.lon = rint(bsm.gps_pos_lla.lon); bsm.gps_pos_lla.alt = (bsm.gps_pos->ve[AXIS_Z] + GROUND_ALT)* 100.; bsm.gps_pos_lla.alt = rint(bsm.gps_pos_lla.alt); diff --git a/sw/simulator/booz_sensors_model_gyro.c b/sw/simulator/booz_sensors_model_gyro.c index f5138d21c2..eb59941d16 100644 --- a/sw/simulator/booz_sensors_model_gyro.c +++ b/sw/simulator/booz_sensors_model_gyro.c @@ -60,15 +60,13 @@ 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]; - + /* rotate to IMU frame */ + /* convert to imu frame */ + static VEC* rate_imu = VNULL; + rate_imu = v_resize(rate_imu, AXIS_NB); + mv_mlt(bsm.body_to_imu, bfm.ang_rate_body, rate_imu); /* compute gyros readings */ - bsm.gyro = mv_mlt(bsm.gyro_sensitivity, rate_body, bsm.gyro); + bsm.gyro = mv_mlt(bsm.gyro_sensitivity, rate_imu, bsm.gyro); bsm.gyro = v_add(bsm.gyro, bsm.gyro_neutral, bsm.gyro); /* compute gyro error readings */ diff --git a/sw/simulator/booz_sensors_model_mag.c b/sw/simulator/booz_sensors_model_mag.c index f0b384fa11..3bb91d83d8 100644 --- a/sw/simulator/booz_sensors_model_mag.c +++ b/sw/simulator/booz_sensors_model_mag.c @@ -3,6 +3,7 @@ #include BSM_PARAMS #include "booz_sensors_model_utils.h" #include "booz_flight_model.h" +#include "booz_flight_model_utils.h" bool_t booz_sensors_model_mag_available() { if (bsm.mag_available) { @@ -19,7 +20,14 @@ void booz_sensors_model_mag_init( double time ) { bsm.mag->ve[AXIS_X] = 0.; bsm.mag->ve[AXIS_Y] = 0.; bsm.mag->ve[AXIS_Z] = 0.; - bsm.mag_resolution = BSM_MAG_RESOLUTION; + // bsm.mag_resolution = BSM_MAG_RESOLUTION; + + bsm.mag_imu_to_sensor = m_get(AXIS_NB, AXIS_NB); + VEC* tmp_eulers = v_get(AXIS_NB); + tmp_eulers->ve[EULER_PHI] = BSM_MAG_IMU_TO_SENSOR_PHI; + tmp_eulers->ve[EULER_THETA] = BSM_MAG_IMU_TO_SENSOR_THETA; + tmp_eulers->ve[EULER_PSI] = BSM_MAG_IMU_TO_SENSOR_PSI; + dcm_of_eulers (tmp_eulers, bsm.mag_imu_to_sensor ); bsm.mag_sensitivity = m_get(AXIS_NB, AXIS_NB); m_zero(bsm.mag_sensitivity); @@ -42,21 +50,30 @@ void booz_sensors_model_mag_init( double time ) { } -void booz_sensors_model_mag_run( double time, MAT* dcm ) { +void booz_sensors_model_mag_run( double time ) { 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); + mv_mlt(bfm.dcm, bfm.h_ltp, h_body); + /* rotate to imu frame */ + static VEC *h_imu = VNULL; + h_imu = v_resize(h_imu, AXIS_NB); + mv_mlt(bsm.body_to_imu, h_body, h_imu); + /* rotate to sensor frame */ + static VEC *h_sensor = VNULL; + h_sensor = v_resize(h_sensor, AXIS_NB); + mv_mlt(bsm.mag_imu_to_sensor, h_imu, h_sensor); + + mv_mlt(bsm.mag_sensitivity, h_sensor, 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); + /* add hard iron now ? */ 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); @@ -66,7 +83,7 @@ void booz_sensors_model_mag_run( double time, MAT* dcm ) { 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); + 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]); diff --git a/sw/simulator/booz_sensors_model_params.h b/sw/simulator/booz_sensors_model_params.h deleted file mode 100644 index 53d3387147..0000000000 --- a/sw/simulator/booz_sensors_model_params.h +++ /dev/null @@ -1,133 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2008 Antoine Drouin - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ - -#ifndef BOOZ_SENSORS_MODEL_PARAMS_H -#define BOOZ_SENSORS_MODEL_PARAMS_H - -/* - * Accelerometer - */ -#define BSM_ACCEL_RESOLUTION (1024 * 32) -/* ms-2 */ -#define BSM_ACCEL_SENSITIVITY_XX 827.294562 -#define BSM_ACCEL_SENSITIVITY_YY -823.507909 -#define BSM_ACCEL_SENSITIVITY_ZZ -824.835155 -#define BSM_ACCEL_NEUTRAL_X 17595 -#define BSM_ACCEL_NEUTRAL_Y 16427 -#define BSM_ACCEL_NEUTRAL_Z 16458 -/* m2s-4 */ -#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 -#define BSM_ACCEL_BIAS_Z 1e-3 -/* s */ -#define BSM_ACCEL_DT (1./250.) - - - -/* - * Gyrometer - */ -#define BSM_GYRO_RESOLUTION 65536 -/* degres/s - nominal 300 */ -//#define BSM_GYRO_SENSITIVITY_PP 65536. / (2.*RadOfDeg(-413.41848)); -//#define BSM_GYRO_SENSITIVITY_QQ 65536. / (2.*RadOfDeg(-403.65564)); -//#define BSM_GYRO_SENSITIVITY_RR 65536. / (2.*RadOfDeg( 395.01929)); - -#define BSM_GYRO_SENSITIVITY_PP (-4541.3261) -#define BSM_GYRO_SENSITIVITY_QQ (-4651.1628) -#define BSM_GYRO_SENSITIVITY_RR ( 4752.8517) - -//#define BSM_GYRO_NEUTRAL_P 65536. * 0.6238556; -//#define BSM_GYRO_NEUTRAL_Q 65536. * 0.6242371; -//#define BSM_GYRO_NEUTRAL_R 65536. * 0.6035156; - -#define BSM_GYRO_NEUTRAL_P 40852 -#define BSM_GYRO_NEUTRAL_Q 40916 -#define BSM_GYRO_NEUTRAL_R 39619 - -#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(.5) -#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(.5) -#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(.5) - -#define BSM_GYRO_BIAS_INITIAL_P 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_R RadOfDeg(5.e-1) - -#define BSM_GYRO_DT (1./250.) - - - -/* - * Magnetometer - */ - -#define BSM_MAG_DT (1./20.) - - -/* - * Range meter - */ -#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_POS_INITIAL_UTM_EAST 37728000 -#define BSM_GPS_POS_INITIAL_UTM_NORTH 482464300 -#define BSM_GPS_POS_INITIAL_UTM_ALT 15200 - -#define BSM_GPS_DT (1./4.) - -#endif /* BOOZ_SENSORS_MODEL_PARAMS_H */ diff --git a/sw/simulator/booz_sensors_model_params_booz2_a1.h b/sw/simulator/booz_sensors_model_params_booz2_a1.h index c777e445fb..275b1b604b 100644 --- a/sw/simulator/booz_sensors_model_params_booz2_a1.h +++ b/sw/simulator/booz_sensors_model_params_booz2_a1.h @@ -25,6 +25,13 @@ #ifndef BOOZ_SENSORS_MODEL_PARAMS_H #define BOOZ_SENSORS_MODEL_PARAMS_H +#include "airframe.h" + + +#define BSM_BODY_TO_IMU_PHI RadOfDeg(4.) +#define BSM_BODY_TO_IMU_THETA RadOfDeg(3.) +#define BSM_BODY_TO_IMU_PSI RadOfDeg(0.) + /* * Accelerometer */ @@ -38,13 +45,13 @@ #define BSM_ACCEL_NEUTRAL_Y 33738 #define BSM_ACCEL_NEUTRAL_Z 32441 /* m2s-4 */ -#define BSM_ACCEL_NOISE_STD_DEV_X 0 -#define BSM_ACCEL_NOISE_STD_DEV_Y 0 -#define BSM_ACCEL_NOISE_STD_DEV_Z 0 +//#define BSM_ACCEL_NOISE_STD_DEV_X 0 +//#define BSM_ACCEL_NOISE_STD_DEV_Y 0 +//#define BSM_ACCEL_NOISE_STD_DEV_Z 0 -//#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 +#define BSM_ACCEL_NOISE_STD_DEV_X 1.e-1 +#define BSM_ACCEL_NOISE_STD_DEV_Y 1.e-1 +#define BSM_ACCEL_NOISE_STD_DEV_Z 1.1e-1 /* ms-2 */ #define BSM_ACCEL_BIAS_X 0 @@ -88,11 +95,15 @@ /* * Magnetometer */ -#define BSM_MAG_RESOLUTION 65536 +//#define BSM_MAG_RESOLUTION 65536 -#define BSM_MAG_SENSITIVITY_XX (1./-3.4936416) -#define BSM_MAG_SENSITIVITY_YY (1./ 3.607713) -#define BSM_MAG_SENSITIVITY_ZZ (1./-4.90788848) +#define BSM_MAG_IMU_TO_SENSOR_PHI 0. +#define BSM_MAG_IMU_TO_SENSOR_THETA 0. +#define BSM_MAG_IMU_TO_SENSOR_PSI RadOfDeg(45.) + +#define BSM_MAG_SENSITIVITY_XX (1.*(1<<11)/-4.94075530) +#define BSM_MAG_SENSITIVITY_YY (1.*(1<<11)/ 5.10207664) +#define BSM_MAG_SENSITIVITY_ZZ (1.*(1<<11)/-4.90788848) #define BSM_MAG_NEUTRAL_X 2358 #define BSM_MAG_NEUTRAL_Y 2362 @@ -102,9 +113,9 @@ //#define BSM_MAG_NOISE_STD_DEV_Y 0 //#define BSM_MAG_NOISE_STD_DEV_Z 0 -#define BSM_MAG_NOISE_STD_DEV_X 2e-2 -#define BSM_MAG_NOISE_STD_DEV_Y 2e-2 -#define BSM_MAG_NOISE_STD_DEV_Z 2e-2 +#define BSM_MAG_NOISE_STD_DEV_X 2e-3 +#define BSM_MAG_NOISE_STD_DEV_Y 2e-3 +#define BSM_MAG_NOISE_STD_DEV_Z 2e-3 #define BSM_MAG_DT (1./100.) @@ -126,7 +137,7 @@ #define BSM_BARO_QNH 900. #define BSM_BARO_SENSITIVITY 17.066667 #define BSM_BARO_DT (1./100.) - +#define BSM_BARO_NOISE_STD_DEV 5.e-2 /* * GPS @@ -134,7 +145,7 @@ #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_NOISE_STD_DEV 3e-2 #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 diff --git a/sw/simulator/booz_sensors_model_rangemeter.c b/sw/simulator/booz_sensors_model_rangemeter.c index 58d27a72f0..f2c951829c 100644 --- a/sw/simulator/booz_sensors_model_rangemeter.c +++ b/sw/simulator/booz_sensors_model_rangemeter.c @@ -28,10 +28,10 @@ void booz_sensors_model_rangemeter_run( double time ) { return; /* compute dist from ground */ - double dz = bfm.state->ve[BFMS_Z]; + double dz = bfm.pos_ltp->ve[AXIS_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 dx = dz * tan(bfm.eulers->ve[EULER_THETA]); + double dy = dz * tan(bfm.eulers->ve[EULER_PHI]); double dist = sqrt( dx*dx + dy*dy + dz*dz); dist *= BSM_RANGEMETER_SENSITIVITY; /* add gaussian noise */ diff --git a/sw/simulator/booz_sensors_model_utils.c b/sw/simulator/booz_sensors_model_utils.c index 6fd655777b..d5ee76723b 100644 --- a/sw/simulator/booz_sensors_model_utils.c +++ b/sw/simulator/booz_sensors_model_utils.c @@ -1,6 +1,7 @@ #include "booz_sensors_model_utils.h" #include "6dof.h" +#include void UpdateSensorLatency(double time, VEC* cur_reading, GSList* history, double latency, VEC* sensor_reading) { @@ -8,7 +9,7 @@ void UpdateSensorLatency(double time, VEC* cur_reading, GSList* history, 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); + v_copy(cur_reading, cur_read->value); history = g_slist_prepend(history, cur_read); /* remove old readings */ GSList* last = g_slist_last(history); @@ -21,7 +22,7 @@ void UpdateSensorLatency(double time, VEC* cur_reading, GSList* history, last = g_slist_last(history); } /* update sensor */ - CopyVect(sensor_reading, ((struct BoozDatedSensor*)last->data)->value); + v_copy(((struct BoozDatedSensor*)last->data)->value, sensor_reading); } @@ -35,13 +36,42 @@ VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* 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); + tmp = v_resize(tmp, in->dim); + unsigned int i; + for (i=0; idim; i++) + tmp->ve[i] = get_gaussian_noise() * std_dev->ve[i]; + v_add(in, tmp, out); return out; } + + +/* + http://www.taygeta.com/random/gaussian.html +*/ + +#include "booz_r250.h" + +double get_gaussian_noise(void) { + + static int nb_call = 0; + static double x1, x2, w; + if (nb_call==0) r250_init(0); + nb_call++; + if (nb_call%2) { + do { + x1 = 2.0 * dr250() - 1.0; + x2 = 2.0 * dr250() - 1.0; + w = x1 * x1 + x2 * x2; + } while ( w >= 1.0 ); + + w = sqrt( (-2.0 * log( w ) ) / w ); + return x1 * w; + } + else + return x2 * w; +} + + + + + diff --git a/sw/simulator/booz_sensors_model_utils.h b/sw/simulator/booz_sensors_model_utils.h index 007640ed27..f842034781 100644 --- a/sw/simulator/booz_sensors_model_utils.h +++ b/sw/simulator/booz_sensors_model_utils.h @@ -13,7 +13,7 @@ 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); - +extern double get_gaussian_noise(); #define RoundSensor(_sensor) { \ _sensor->ve[AXIS_X] = rint(_sensor->ve[AXIS_X]); \ @@ -30,12 +30,6 @@ extern VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out); 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/booz_wind_model.c b/sw/simulator/booz_wind_model.c index f8becdcf98..c1bbd09c33 100644 --- a/sw/simulator/booz_wind_model.c +++ b/sw/simulator/booz_wind_model.c @@ -22,6 +22,10 @@ void booz_wind_model_init( void ) { bwm.velocity = v_get(AXIS_NB); v_zero(bwm.velocity); + bwm.velocity->ve[AXIS_X] = INIT_WIND_X; + bwm.velocity->ve[AXIS_Y] = INIT_WIND_Y; + bwm.velocity->ve[AXIS_Z] = INIT_WIND_Z; + bwm.state = v_get(BWM_STATE_SIZE); v_zero(bwm.state);