diff --git a/sw/simulator/old_booz/booz2_sim_main.c b/sw/simulator/old_booz/booz2_sim_main.c deleted file mode 100644 index 4161279adb..0000000000 --- a/sw/simulator/old_booz/booz2_sim_main.c +++ /dev/null @@ -1,449 +0,0 @@ -/* - * 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. - * - */ - -#include -#include -#include - -#include -#include - -#include "booz_flight_model.h" -#include "booz_sensors_model.h" -#include "booz_wind_model.h" -#include "booz_rc_sim.h" -#include "firmwares/rotorcraft/battery.h" - -#include "main.h" - - -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 SIM_DT (1./512.) -double sim_time; - -#define DT_DISPLAY 0.04 -double disp_time; - -double booz_sim_actuators_values[] = {0., 0., 0., 0.}; - -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); - -static void booz2_sim_parse_options(int argc, char** argv); -static void booz2_sim_init(void); -static gboolean booz2_sim_periodic(gpointer data); -static void booz2_sim_display(void); -static void booz_sim_read_actuators(void); - -static void ivy_transport_init(void); -static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), - int argc __attribute__ ((unused)), char *argv[]); -static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), - int argc __attribute__ ((unused)), char *argv[]); -static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), - int argc __attribute__ ((unused)), char *argv[]); - - -static void booz2_sim_init(void) { - - gettimeofday (&host_time_start, NULL); - sim_time = 0.; - disp_time = 0.; - - booz_flight_model_init(); - - booz_sensors_model_init(sim_time); - - booz_wind_model_init(); - - ivy_transport_init(); - - main_init(); - -} - -#include "booz2_analog_baro.h" -#include "imu.h" - -static gboolean booz2_sim_periodic(gpointer data __attribute__ ((unused))) { - - 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); - - while (sim_time <= host_time_elapsed) { - sim_run_one_step(); - sim_time += SIM_DT; - } - - - - return TRUE; -} - - -#include "subsystems/ahrs.h" - -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); - battery_voltage = bfm.bat_voltage * 10; - - /* outputs models state */ - booz2_sim_display(); - - /* run the airborne code */ - - // feed a rc frame and signal event - BoozRcSimFeed(sim_time); - // process it - main_event(); - - if (booz_sensors_model_baro_available()) { - Booz2BaroISRHandler(bsm.baro); - main_event(); -#ifdef BYPASS_INS - sim_overwrite_ins(); -#endif /* BYPASS_INS */ - } - if (booz_sensors_model_gyro_available()) { - imu_feed_data(); - 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(); - main_event(); - } - - if (booz_sensors_model_mag_available()) { - sim_mag_feed_data(); - main_event(); -#ifdef BYPASS_AHRS - sim_overwrite_ahrs(); -#endif /* BYPASS_AHRS */ - } - - main_periodic(); - - -} - - - - - -#ifdef BYPASS_AHRS -#include "booz_geometry_mixed.h" -#include "subsystems/ahrs.h" -static void sim_overwrite_ahrs(void) { - ahrs.ltp_to_body_euler.phi = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_X]); - ahrs.ltp_to_body_euler.theta = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_Y]); - ahrs.ltp_to_body_euler.psi = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_Z]); - - ahrs.ltp_to_body_quat.qi = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QI], IQUAT_RES); - ahrs.ltp_to_body_quat.qx = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QX], IQUAT_RES); - ahrs.ltp_to_body_quat.qy = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QY], IQUAT_RES); - ahrs.ltp_to_body_quat.qz = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QZ], IQUAT_RES); - - ahrs.body_rate.p = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_X]); - ahrs.body_rate.q = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_Y]); - ahrs.body_rate.r = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_Z]); - -} -#endif /* BYPASS_AHRS */ - - -#ifdef BYPASS_INS -#include "subsystems/ins.h" -static void sim_overwrite_ins(void) { - ins_position.z = BOOZ_POS_I_OF_F(bfm.pos_ltp->ve[AXIS_Z]); - ins_speed_earth.z = BOOZ_SPEED_I_OF_F(bfm.speed_ltp->ve[AXIS_Z]); - 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_X] * 100.); - // booz2_gps_vel_e = rint(bsm.gps_speed->ve[AXIS_Y] * 100.); - - - booz_gps_state.ecef_pos.x = rint(bsm.gps_pos_ecef.x); - booz_gps_state.ecef_pos.y = rint(bsm.gps_pos_ecef.y); - booz_gps_state.ecef_pos.z = rint(bsm.gps_pos_ecef.z); - // VECT3_COPY(booz_gps_state.ecef_pos, bsm.gps_pos_ecef); - VECT3_COPY(booz_gps_state.ecef_speed, bsm.gps_speed_ecef); - booz_gps_state.fix = BOOZ2_GPS_FIX_3D; - - -} - -#include "AMI601.h" -static void sim_mag_feed_data(void) { - 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]; - ami601_status = AMI601_DATA_AVAILABLE; -} - - - -#define RPM_OF_RAD_S(a) ((a)*60./M_PI) -static void booz2_sim_display(void) { - if (sim_time >= disp_time) { - disp_time+= DT_DISPLAY; - // booz_flightgear_send(); - IvySendMsg("%d NPS_RPMS %f %f %f %f", - AC_ID, - 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 NPS_RATE_ATTITUDE %f %f %f %f %f %f", - AC_ID, - 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 NPS_SPEED_POS %f %f %f %f %f %f", - AC_ID, - (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])); -#if 0 - IvySendMsg("%d NPS_WIND %f %f %f", - AC_ID, - bwm.velocity->ve[AXIS_X], - bwm.velocity->ve[AXIS_Y], - bwm.velocity->ve[AXIS_Z]); -#endif - IvySendMsg("%d NPS_ACCEL_LTP %f %f %f", - AC_ID, - bfm.accel_ltp->ve[AXIS_X], - bfm.accel_ltp->ve[AXIS_Y], - bfm.accel_ltp->ve[AXIS_Z]); - - } -} - -int main ( int argc, char** argv) { - - booz2_sim_parse_options(argc, argv); - - booz2_sim_init(); - - GMainLoop *ml = g_main_loop_new(NULL, FALSE); - - g_timeout_add(HOST_TIMEOUT_PERIOD, booz2_sim_periodic, NULL); - - g_main_loop_run(ml); - - return 0; -} - - -static void ivy_transport_init(void) { - IvyInit ("BoozSim", "BoozSim READY", NULL, NULL, NULL, NULL); - IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)"); - IvyBindMsg(on_DL_BLOCK, NULL, "^(\\S*) BLOCK (\\S*) (\\S*)"); - IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); - IvyStart("127.255.255.255"); -} - -#include "std.h" -#include "generated/settings.h" -#include "dl_protocol.h" -#include "subsystems/datalink/downlink.h" -static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), - int argc __attribute__ ((unused)), char *argv[]){ - uint8_t index = atoi(argv[2]); - float value = atof(argv[3]); - DlSetting(index, value); - DOWNLINK_SEND_DL_VALUE(&index, &value); - // printf("setting %d %f\n", index, value); -} - -static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), - int argc __attribute__ ((unused)), char *argv[]){ - int block = atoi(argv[1]); - nav_goto_block(block); -} - -#include "pprz_geodetic_int.h" -#include "stdio.h" -static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), - int argc __attribute__ ((unused)), char *argv[]){ - int wp_id = atoi(argv[1]); - //int ac_id = atoi(argv[1]); - struct LlaCoor_i lla; - struct EnuCoor_i enu; - //printf("move deg %d %d %d\n",atoi(argv[3]),atoi(argv[4]),atoi(argv[5])); - int lat = atoi(argv[3]); - int lon = atoi(argv[4]); - int alt = atoi(argv[5]); - lla.lat = INT32_RAD_OF_DEG(lat); - lla.lon = INT32_RAD_OF_DEG(lon); - lla.alt = alt+ins_ltp_def.lla.alt; - //printf("move rad %d %d %d (%d)\n",lla.lat,lla.lon,lla.alt,ins_ltp_def.lla.alt); - enu_of_lla_point_i(&enu,&ins_ltp_def,&lla); - enu.x = POS_BFP_OF_REAL(enu.x)/100; - enu.y = POS_BFP_OF_REAL(enu.y)/100; - enu.z = POS_BFP_OF_REAL(enu.z)/100; - //printf("enu_of_lla %d %d %d -> %d %d %d (%f %f %f)\n",waypoints[wp_id].x,waypoints[wp_id].y,waypoints[wp_id].z,enu.x,enu.y,enu.z, - // POS_FLOAT_OF_BFP(enu.x),POS_FLOAT_OF_BFP(enu.y),POS_FLOAT_OF_BFP(enu.z)); - VECT3_ASSIGN(waypoints[wp_id],enu.x,enu.y,enu.z); - DOWNLINK_SEND_WP_MOVED_LTP(&wp_id, &enu.x, &enu.y, &enu.z); - //DOWNLINK_SEND_WP_MOVED_LLA(&wp_id, &lat, &lon, &alt); - //printf("WP_MOVED\n"); - //fflush(stdout); -} - -#include "actuators.h" -static void booz_sim_read_actuators(void) { - - -// printf("actatuors %d %d %d %d\n", -// Actuator(SERVO_FRONT), Actuator(SERVO_BACK), Actuator(SERVO_RIGHT), Actuator(SERVO_LEFT)); - int32_t ut_front = Actuator(SERVO_FRONT) - TRIM_FRONT; - int32_t ut_back = Actuator(SERVO_BACK) - TRIM_BACK; - int32_t ut_right = Actuator(SERVO_RIGHT) - TRIM_RIGHT; - int32_t ut_left = Actuator(SERVO_LEFT) - TRIM_LEFT; -#if 1 - booz_sim_actuators_values[0] = (double)ut_front / SUPERVISION_MAX_MOTOR; - booz_sim_actuators_values[1] = (double)ut_back / SUPERVISION_MAX_MOTOR; - booz_sim_actuators_values[2] = (double)ut_right / SUPERVISION_MAX_MOTOR; - booz_sim_actuators_values[3] = (double)ut_left / SUPERVISION_MAX_MOTOR; -#else - // double foo = 0.33; - double foo = 0.35; - booz_sim_actuators_values[0] = foo; - booz_sim_actuators_values[1] = foo; - booz_sim_actuators_values[2] = foo; - booz_sim_actuators_values[3] = foo; -#endif - - - // printf("%f %f %f %f\n", booz_sim_actuators_values[0], booz_sim_actuators_values[1], booz_sim_actuators_values[2], booz_sim_actuators_values[3]); - -} - -static void booz2_sim_parse_options(int argc, char** argv) { - - static const char* usage = -"Usage: %s [options]\n" -" Options :\n" -" -j --js_dev joystick device\n" -" --fg_host flight gear host\n" -" --fg_port flight gear port\n"; - - - while (1) { - - static struct option long_options[] = { - {"fg_host", 1, NULL, 0}, - {"fg_port", 1, NULL, 0}, - {"js_dev", 1, NULL, 0}, - {0, 0, 0, 0} - }; - int option_index = 0; - int c = getopt_long(argc, argv, "j:", - long_options, &option_index); - if (c == -1) - break; - - switch (c) { - case 0: - switch (option_index) { - case 0: - fg_host = strdup(optarg); break; - case 1: - fg_port = atoi(optarg); break; - case 2: - joystick_dev = strdup(optarg); break; - } - break; - - case 'j': - joystick_dev = strdup(optarg); - break; - - default: /* ’?’ */ - printf("?? getopt returned character code 0%o ??\n", c); - fprintf(stderr, usage, argv[0]); - exit(EXIT_FAILURE); - } - } -} diff --git a/sw/simulator/old_booz/booz_flight_model.c b/sw/simulator/old_booz/booz_flight_model.c deleted file mode 100644 index 5aa0029238..0000000000 --- a/sw/simulator/old_booz/booz_flight_model.c +++ /dev/null @@ -1,362 +0,0 @@ -#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" - - -struct BoozFlightModel bfm; - -//static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot); - -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); - -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); - - bfm.state = v_get(BFMS_SIZE); - v_zero(bfm.state); - - 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_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_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; - - bfm.props_moment_matrix = m_get(AXIS_NB, SERVOS_NB); - m_zero(bfm.props_moment_matrix); - bfm.props_moment_matrix->me[AXIS_X][SERVO_LEFT] = L * bfm.thrust_factor; - bfm.props_moment_matrix->me[AXIS_X][SERVO_RIGHT] = -L * bfm.thrust_factor; - bfm.props_moment_matrix->me[AXIS_Y][SERVO_BACK] = -L * bfm.thrust_factor; - bfm.props_moment_matrix->me[AXIS_Y][SERVO_FRONT] = L * bfm.thrust_factor; - bfm.props_moment_matrix->me[AXIS_Z][SERVO_LEFT] = bfm.torque_factor; - bfm.props_moment_matrix->me[AXIS_Z][SERVO_RIGHT] = bfm.torque_factor; - bfm.props_moment_matrix->me[AXIS_Z][SERVO_BACK] = -bfm.torque_factor; - bfm.props_moment_matrix->me[AXIS_Z][SERVO_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; - bfm.Inert->me[AXIS_Y][AXIS_Y] = Iy; - bfm.Inert->me[AXIS_Z][AXIS_Z] = Iz; - - bfm.Inert_inv = m_get(AXIS_NB, AXIS_NB); - m_zero(bfm.Inert_inv); - bfm.Inert_inv->me[AXIS_X][AXIS_X] = 1./Ix; - bfm.Inert_inv->me[AXIS_Y][AXIS_Y] = 1./Iy; - bfm.Inert_inv->me[AXIS_Z][AXIS_Z] = 1./Iz; - -} - - -#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 ) { - - - int i; - for (i=0; ive[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); - 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); - /* quaternion */ - quat_of_eulers(bfm.quat, bfm.eulers); - - 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 */ - - -} - - - -/* - compute the sum of external forces. - assumes that dcm and omega_square are already precomputed from X -*/ -static VEC* booz_get_forces_ltp(VEC* F , VEC* speed_ltp, MAT* dcm_t, VEC* omega_square) { - - // FIXME : nimporte koi ! - F = v_zero(F); - if (!bfm.on_ground) { - - // propeller thrust - 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 - 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; -} - -/* - compute the sum of external moments. - assumes that omega_square is already precomputed from X -*/ -static VEC* booz_get_moments_body_frame(VEC* M, VEC* omega_square ) { - if (bfm.on_ground) { - M = v_zero(M); - } - else { - M = mv_mlt(bfm.props_moment_matrix, omega_square, M); - } - return M; -} - -static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot) { - - /* 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 eulers angles from state */ - static VEC *eulers = VNULL; - eulers = v_resize(eulers, AXIS_NB); - BoozFlighModelGetAngles(eulers); - /* 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); - /* 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 */ - 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 */ - 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]); - double cosPHI = cos(eulers->ve[EULER_PHI]); - double cosTHETA = cos(eulers->ve[EULER_THETA]); - double tanTHETA = tan(eulers->ve[EULER_THETA]); - static MAT *euler_dot_of_pqr = MNULL; - euler_dot_of_pqr = m_resize(euler_dot_of_pqr,AXIS_NB, AXIS_NB); - euler_dot_of_pqr->me[EULER_PHI][AXIS_P] = 1.; - euler_dot_of_pqr->me[EULER_PHI][AXIS_Q] = sinPHI*tanTHETA; - euler_dot_of_pqr->me[EULER_PHI][AXIS_R] = cosPHI*tanTHETA; - euler_dot_of_pqr->me[EULER_THETA][AXIS_P] = 0.; - euler_dot_of_pqr->me[EULER_THETA][AXIS_Q] = cosPHI; - euler_dot_of_pqr->me[EULER_THETA][AXIS_R] = -sinPHI; - euler_dot_of_pqr->me[EULER_PSI][AXIS_P] = 0.; - euler_dot_of_pqr->me[EULER_PSI][AXIS_Q] = sinPHI/cosTHETA; - euler_dot_of_pqr->me[EULER_PSI][AXIS_R] = cosPHI/cosTHETA; - static VEC *euler_dot = VNULL; - euler_dot = v_resize(euler_dot, AXIS_NB); - euler_dot = mv_mlt(euler_dot_of_pqr, rate_body, euler_dot); - Xdot->ve[BFMS_PHI] = euler_dot->ve[EULER_PHI]; - Xdot->ve[BFMS_THETA] = euler_dot->ve[EULER_THETA]; - Xdot->ve[BFMS_PSI] = euler_dot->ve[EULER_PSI]; - - /* derivatives of rates */ - /* compute external moments */ - static VEC *m_body = VNULL; - m_body = v_resize(m_body, AXIS_NB); - m_body = booz_get_moments_body_frame(m_body, omega_square); - /* Newton in body frame */ - static VEC *i_omega = VNULL; - i_omega = v_resize(i_omega, AXIS_NB); - i_omega = mv_mlt(bfm.Inert, rate_body, i_omega); - static VEC *omega_i_omega = VNULL; - omega_i_omega = v_resize(omega_i_omega, AXIS_NB); - omega_i_omega = out_prod(rate_body, i_omega, omega_i_omega); - static VEC *m_tot = VNULL; - m_tot = v_resize(m_tot, AXIS_NB); - m_tot = v_sub(m_body, omega_i_omega, m_tot); - static VEC *I_inv_m_tot = VNULL; - I_inv_m_tot = v_resize(I_inv_m_tot, AXIS_NB); - I_inv_m_tot = mv_mlt(bfm.Inert_inv, m_tot, I_inv_m_tot); - Xdot->ve[BFMS_P] = I_inv_m_tot->ve[AXIS_P]; - Xdot->ve[BFMS_Q] = I_inv_m_tot->ve[AXIS_Q]; - Xdot->ve[BFMS_R] = I_inv_m_tot->ve[AXIS_R]; - - /* derivatives of motors rpm */ - /* omega_dot = -1/THAU*omega - Kq*omega^2 + Kv/THAU * V */ - Xdot->ve[BFMS_OM_B] = -1./THAU * X->ve[BFMS_OM_B] - Kq * omega_square->ve[SERVO_BACK] + Kv/THAU * u->ve[SERVO_BACK]; - Xdot->ve[BFMS_OM_F] = -1./THAU * X->ve[BFMS_OM_F] - Kq * omega_square->ve[SERVO_FRONT] + Kv/THAU * u->ve[SERVO_FRONT]; - Xdot->ve[BFMS_OM_R] = -1./THAU * X->ve[BFMS_OM_R] - Kq * omega_square->ve[SERVO_RIGHT] + Kv/THAU * u->ve[SERVO_RIGHT]; - Xdot->ve[BFMS_OM_L] = -1./THAU * X->ve[BFMS_OM_L] - Kq * omega_square->ve[SERVO_LEFT] + Kv/THAU * u->ve[SERVO_LEFT]; - -} - - - - - - -#if 0 -static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot) { - static VEC *temp1 = VNULL; - static VEC *temp2 = VNULL; - temp1 = v_resize(temp1,SERVOS_NB); - temp2 = v_resize(temp2,SERVOS_NB); - - // omega_dot = -1/THAU*omega - Kq*omega^2 + Kv/THAU * V; - temp1 = sv_mlt(-1./THAU, x, temp1); /* temp1 = -1/THAU * x */ - temp2 = v_star(x, x, temp2); /* temp2 = x^2 */ - xdot = v_mltadd(temp1, temp2, -Kq, xdot); /* xdot = temp1 - Kq*temp2 */ - xdot = v_mltadd(xdot, u, Kv/THAU, xdot); /* xdot = xdot + Kv/THAU * u */ -} -#endif diff --git a/sw/simulator/old_booz/booz_flight_model.h b/sw/simulator/old_booz/booz_flight_model.h deleted file mode 100644 index 57e2d0a66c..0000000000 --- a/sw/simulator/old_booz/booz_flight_model.h +++ /dev/null @@ -1,65 +0,0 @@ -#ifndef BOOZ_FLIGHT_MODEL_H -#define BOOZ_FLIGHT_MODEL_H - -#include -#include "generated/airframe.h" - - - -struct BoozFlightModel { - /* are we flying ? */ - int on_ground; - - double time; - /* battery voltage in V */ - double bat_voltage; - /* motors supply voltage in V */ - VEC* mot_voltage; - - /* private state : see defines in .c for fields */ - VEC* state; - - /* user products */ - VEC* pos_ecef; - 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_ltp; - /* gravitation in earth frame */ - VEC* g_ltp; - /* propeller thrust factor */ - double thrust_factor; - /* propeller torque factor */ - 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 */ - MAT* Inert_inv; -}; - -extern struct BoozFlightModel bfm; - -extern void booz_flight_model_init( void ); -extern void booz_flight_model_run( double t, double* commands ); - - -#endif /* BOOZ_FLIGHT_MODEL_H */ diff --git a/sw/simulator/old_booz/booz_flight_model_params.h b/sw/simulator/old_booz/booz_flight_model_params.h deleted file mode 100644 index 964b9287ff..0000000000 --- a/sw/simulator/old_booz/booz_flight_model_params.h +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef BOOZ_FLIGHT_MODEL_PARAMS_H -#define BOOZ_FLIGHT_MODEL_PARAMS_H - -/* body drag coefficient */ -#define C_d_body .0075 -/* propeller thrust aerodynamic coefficient */ -#define C_t 0.297 -/* propeller moment aerodynamic coefficient */ -#define C_q 0.0276 -/* propeller radius in m */ -#define PROP_RADIUS 0.125 -/* propeller area in m2 */ -#define PROP_AREA 0.005 -/* air density in kg/m3 */ -#define RHO 1.225 -/* gravity in m/s2 */ -#define G 9.81 -/* mass in kg */ -#define MASS 0.724 -/* inertia on x axis in kg * m2 */ -#define Ix .007 -/* inertia on y axis in kg * m2 */ -#define Iy .0073 -/* inertia on z axis in kg * m2 */ -#define Iz .0137 -/* lenght between centers of vehicle and prop in m */ -#define L 0.25 - -/* motors parameters - - from http://cherokee.stanford.edu/~starmac/docs/DynamicsSummary - - omega_dot = -1/thau * omega - Kq * omega^2 + Kv/thau * V - -*/ -#define BAT_VOLTAGE 11. - -#define THAU 0.05 -#define Kq 0.12 -#define Kv 304. - - - -#endif /* BOOZ_FLIGHT_MODEL_PARAMS_H */ diff --git a/sw/simulator/old_booz/booz_flight_model_utils.c b/sw/simulator/old_booz/booz_flight_model_utils.c deleted file mode 100644 index 6a648227c6..0000000000 --- a/sw/simulator/old_booz/booz_flight_model_utils.c +++ /dev/null @@ -1,100 +0,0 @@ -#include "booz_flight_model_utils.h" - -#include -#include "6dof.h" - -void rk4(ode_fun f, VEC* x, VEC* u, double dt) { - - static VEC *v1=VNULL, *v2=VNULL, *v3=VNULL, *v4=VNULL; - static VEC *temp=VNULL; - - v1 = v_resize(v1,x->dim); - v2 = v_resize(v2,x->dim); - v3 = v_resize(v3,x->dim); - v4 = v_resize(v4,x->dim); - temp = v_resize(temp,x->dim); - - // MEM_STAT_REG(v1,TYPE_VEC); - // MEM_STAT_REG(v2,TYPE_VEC); - // MEM_STAT_REG(v3,TYPE_VEC); - // MEM_STAT_REG(v4,TYPE_VEC); - // MEM_STAT_REG(temp,TYPE_VEC); - - f(x, u, v1); - v_mltadd(x,v1,0.5*dt,temp); /* temp = x + .5*dt*v1 */ - f(temp, u, v2); - v_mltadd(x,v2,0.5*dt,temp); /* temp = x + .5*dt*v2 */ - f(temp,u, v3); - v_mltadd(x,v3,dt,temp); /* temp = x + dt*v3 */ - f(temp, u, v4); - - /* now add: v1+2*v2+2*v3+v4 */ - v_copy(v1,temp); /* temp = v1 */ - v_mltadd(temp,v2,2.0,temp); /* temp = v1+2*v2 */ - v_mltadd(temp,v3,2.0,temp); /* temp = v1+2*v2+2*v3 */ - v_add(temp,v4,temp); /* temp = v1+2*v2+2*v3+v4 */ - - /* adjust x */ - v_mltadd(x,temp,dt/6.0,x); /* x = x+(h/6) * temp */ - -} - - -MAT* dcm_of_eulers (VEC* eulers, MAT* dcm ) { - - dcm = m_resize(dcm, 3,3); - - double sinPHI = sin(eulers->ve[EULER_PHI]); - double cosPHI = cos(eulers->ve[EULER_PHI]); - double sinTHETA = sin(eulers->ve[EULER_THETA]); - double cosTHETA = cos(eulers->ve[EULER_THETA]); - double sinPSI = sin(eulers->ve[EULER_PSI]); - double cosPSI = cos(eulers->ve[EULER_PSI]); - - dcm->me[0][0] = cosTHETA * cosPSI; - dcm->me[0][1] = cosTHETA * sinPSI; - dcm->me[0][2] = -sinTHETA; - dcm->me[1][0] = sinPHI * sinTHETA * cosPSI - cosPHI * sinPSI; - dcm->me[1][1] = sinPHI * sinTHETA * sinPSI + cosPHI * cosPSI; - dcm->me[1][2] = sinPHI * cosTHETA; - dcm->me[2][0] = cosPHI * sinTHETA * cosPSI + sinPHI * sinPSI; - dcm->me[2][1] = cosPHI * sinTHETA * sinPSI - sinPHI * cosPSI; - dcm->me[2][2] = cosPHI * cosTHETA; - - return dcm; -} - - -VEC* quat_of_eulers(VEC* quat, VEC* eulers) { - - double phi2 = eulers->ve[EULER_PHI] / 2.0; - double theta2 = eulers->ve[EULER_THETA] / 2.0; - double psi2 = eulers->ve[EULER_PSI] / 2.0; - - double sinphi2 = sin( phi2 ); - double cosphi2 = cos( phi2 ); - double sintheta2 = sin( theta2 ); - double costheta2 = cos( theta2 ); - double sinpsi2 = sin( psi2 ); - double cospsi2 = cos( psi2 ); - - quat->ve[QUAT_QI] = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2; - quat->ve[QUAT_QX] = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2; - quat->ve[QUAT_QY] = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2; - quat->ve[QUAT_QZ] = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2; - - return quat; -} - - - -VEC* out_prod( VEC* a, VEC* b, VEC* out) { - if ( a->dim != 3 || b->dim != 3 ) - error(E_SIZES,"out_prod"); - if ( out==(VEC *)NULL || out->dim != a->dim ) - out = v_resize(out,a->dim); - out->ve[0] = a->ve[1]*b->ve[2] - a->ve[2]*b->ve[1]; - out->ve[1] = a->ve[2]*b->ve[0] - a->ve[0]*b->ve[2]; - out->ve[2] = a->ve[0]*b->ve[1] - a->ve[1]*b->ve[0]; - return out; -} diff --git a/sw/simulator/old_booz/booz_flight_model_utils.h b/sw/simulator/old_booz/booz_flight_model_utils.h deleted file mode 100644 index a53798608c..0000000000 --- a/sw/simulator/old_booz/booz_flight_model_utils.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef BOOZ_FM_UTILS_H -#define BOOZ_FM_UTILS_H - -#include - -typedef void (*ode_fun)(VEC* x, VEC* u, VEC* xdot); - -extern void rk4(ode_fun f, VEC* x, VEC* u, double dt); - -extern MAT* dcm_of_eulers (VEC* eulers, MAT* dcm ); -extern VEC* quat_of_eulers(VEC* quat, VEC* eulers); - -extern VEC* out_prod( VEC* a, VEC* b, VEC* out); - - - - -#endif /* BOOZ_FM_UTILS_H */ diff --git a/sw/simulator/old_booz/booz_flightgear.c b/sw/simulator/old_booz/booz_flightgear.c deleted file mode 100644 index 2a58ea4529..0000000000 --- a/sw/simulator/old_booz/booz_flightgear.c +++ /dev/null @@ -1,88 +0,0 @@ -#include "booz_flightgear.h" -#include "flight_gear.h" - -#include -#include -#include -#include - -#include -#include -#include - -static int fg_socket; -static struct sockaddr_in fg_addr; - -void net_gui_init (struct FGNetGUI* gui); - -void booz_flightgear_init(const char* host, unsigned int port) { - - printf("connecting to %s on port %d\n", host, port); - - int so_reuseaddr = 1; - struct protoent * pte = getprotobyname("UDP"); - fg_socket = socket( PF_INET, SOCK_DGRAM, pte->p_proto); - setsockopt(fg_socket, SOL_SOCKET, SO_REUSEADDR, - &so_reuseaddr, sizeof(so_reuseaddr)); - - fg_addr.sin_family = PF_INET; - fg_addr.sin_port = htons(port); - fg_addr.sin_addr.s_addr = inet_addr(host); - -} - - -void booz_flightgear_send() { - - const double earth_radius = 6372795.; - - double lat = 0.656480 + asin((bfm.state->ve[BFMS_X] - 90)/earth_radius); - double lon = -2.135537 + asin((bfm.state->ve[BFMS_Y] - 45)/earth_radius); - - struct FGNetGUI gui; - net_gui_init(&gui); - - gui.latitude = lat; - gui.longitude = lon; - gui.altitude = 1.1 - bfm.state->ve[BFMS_Z]; - - gui.phi = bfm.state->ve[BFMS_PHI]; - gui.theta = bfm.state->ve[BFMS_THETA]; - gui.psi = bfm.state->ve[BFMS_PSI]; - - gui.cur_time += (unsigned long)bfm.time; - - if (sendto(fg_socket, (char*)(&gui), sizeof(gui), 0, - (struct sockaddr*)&fg_addr, sizeof(fg_addr)) == -1) - printf("error sending\n"); -} - -void net_gui_init (struct FGNetGUI* gui) { - gui->version = FG_NET_GUI_VERSION; - gui->latitude = 0.656480; - gui->longitude = -2.135537; - gui->altitude = 0.807609; - gui->agl = 1.111652; - - gui->phi = 0.; - gui->theta = 0.; - gui->psi = 5.20; - - gui->vcas = 0.; - gui->climb_rate = 0.; - - gui->num_tanks = 1; - gui->fuel_quantity[0] = 0.; - - gui->cur_time = 3198060679ul; - gui->warp = 1122474394ul; - - gui->ground_elev = 0.; - - gui->tuned_freq = 125.65; - gui->nav_radial = 90.; - gui->in_range = 1; - gui->dist_nm = 10.; - gui->course_deviation_deg = 0.; - gui->gs_deviation_deg = 0.; -} diff --git a/sw/simulator/old_booz/booz_flightgear.h b/sw/simulator/old_booz/booz_flightgear.h deleted file mode 100644 index bbb5ea3033..0000000000 --- a/sw/simulator/old_booz/booz_flightgear.h +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef BOOZ_FLIGHTGEAR_H -#define BOOZ_FLIGHTGEAR_H - -#include "booz_flight_model.h" - -extern void booz_flightgear_init(const char* host, unsigned int port); -extern void booz_flightgear_send(); - -#endif /* BOOZ_FLIGHTGEAR_H */ diff --git a/sw/simulator/old_booz/booz_joystick.c b/sw/simulator/old_booz/booz_joystick.c deleted file mode 100644 index 731c7a5305..0000000000 --- a/sw/simulator/old_booz/booz_joystick.c +++ /dev/null @@ -1,84 +0,0 @@ -#include "booz_joystick.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -double booz_joystick_value[JS_NB_AXIS]; -//const double booz_joystick_neutral[JS_NB_AXIS] = { 112., 113., 112., 112., 112., 112., 30.}; -//const double booz_joystick_max[JS_NB_AXIS] = { 1., 224., 224., 224., 224., 224., 223.}; -//const double booz_joystick_min[JS_NB_AXIS] = { 224., 1., 1., 1., 1., 1., 30.}; - -const double booz_joystick_neutral[JS_NB_AXIS] = { 113., 113., 112., 112., 112., 112., 30.}; -const double booz_joystick_max[JS_NB_AXIS] = { 46., 178., 224., 224., 224., 179., 223.}; -const double booz_joystick_min[JS_NB_AXIS] = { 179., 46., 1., 1., 1., 46., 30.}; - - - -static gboolean on_data_received(GIOChannel *source, GIOCondition condition, gpointer data); - -void booz_joystick_init(const char* device) { - int i; - for (i=0; i - -#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/old_booz/booz_r250.h b/sw/simulator/old_booz/booz_r250.h deleted file mode 100644 index 8455e534b8..0000000000 --- a/sw/simulator/old_booz/booz_r250.h +++ /dev/null @@ -1,21 +0,0 @@ -/* 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/old_booz/booz_randlcg.c b/sw/simulator/old_booz/booz_randlcg.c deleted file mode 100644 index 31170292e1..0000000000 --- a/sw/simulator/old_booz/booz_randlcg.c +++ /dev/null @@ -1,51 +0,0 @@ -/* 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/old_booz/booz_randlcg.h b/sw/simulator/old_booz/booz_randlcg.h deleted file mode 100644 index 7093ff3cfa..0000000000 --- a/sw/simulator/old_booz/booz_randlcg.h +++ /dev/null @@ -1,19 +0,0 @@ - -/* 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/old_booz/booz_rc_sim.h b/sw/simulator/old_booz/booz_rc_sim.h deleted file mode 100644 index 7a6522d189..0000000000 --- a/sw/simulator/old_booz/booz_rc_sim.h +++ /dev/null @@ -1,33 +0,0 @@ -#ifndef BOOZ_RC_SIM_H -#define BOOZ_RC_SIM_H - -#include "subsystems/radio_control.h" - -#define MODE_SWITCH_MANUAL 1900 -#define MODE_SWITCH_AUTO1 1500 -#define MODE_SWITCH_AUTO2 1200 - -#define BoozRcSimFeed(_time) { \ - /* starts motors */ \ - if (_time < 0.93) { \ - ppm_pulses[RADIO_YAW] = 1500 - 0.5 * (2050-950); \ - ppm_pulses[RADIO_THROTTLE] = 1223 + 0.0 * (2050-1223); \ - ppm_pulses[RADIO_MODE] = MODE_SWITCH_MANUAL; \ - } \ - else if (_time < 3.5) { \ - ppm_pulses[RADIO_YAW] = 1500 + 0. * (2050-950); \ - ppm_pulses[RADIO_THROTTLE] = 1223 + 0.4 * (2050-1223); \ - ppm_pulses[RADIO_MODE] = MODE_SWITCH_MANUAL; \ - } \ - else { \ - ppm_pulses[RADIO_YAW] = 1500 + 0. * (2050-950); \ - ppm_pulses[RADIO_THROTTLE] = 1223 + 0.99 * (2050-1223); \ - ppm_pulses[RADIO_MODE] = MODE_SWITCH_AUTO2; \ - } \ - ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \ - ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \ - ppm_valid = TRUE; \ - } - - -#endif /* BOOZ_RC_SIM_H */ diff --git a/sw/simulator/old_booz/booz_sensors_model.c b/sw/simulator/old_booz/booz_sensors_model.c deleted file mode 100644 index 8b4ac35086..0000000000 --- a/sw/simulator/old_booz/booz_sensors_model.c +++ /dev/null @@ -1,64 +0,0 @@ -#include "booz_sensors_model.h" -#include BSM_PARAMS - -#include - -#include "std.h" -#include "booz_flight_model.h" -#include "booz_flight_model_utils.h" - -struct BoozSensorsModel bsm; - -extern void booz_sensors_model_accel_init(double time); -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); - -extern void booz_sensors_model_rangemeter_init(double time); -extern void booz_sensors_model_rangemeter_run(double time); - -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); - - -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) { - - booz_sensors_model_accel_run(time); - booz_sensors_model_gyro_run(time); - booz_sensors_model_mag_run(time); - booz_sensors_model_rangemeter_run(time); - booz_sensors_model_baro_run(time); - booz_sensors_model_gps_run(time); - -} - - - - - - diff --git a/sw/simulator/old_booz/booz_sensors_model.h b/sw/simulator/old_booz/booz_sensors_model.h deleted file mode 100644 index ee33e998a4..0000000000 --- a/sw/simulator/old_booz/booz_sensors_model.h +++ /dev/null @@ -1,126 +0,0 @@ -/* - * 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_H -#define BOOZ_SENSORS_MODEL_H - -#include -#include - -#include "pprz_algebra_float.h" -#include "pprz_geodetic_double.h" -#include "booz_geometry_int.h" -#include "6dof.h" -#include "std.h" - -extern void booz_sensors_model_init(double time); -extern void booz_sensors_model_run( double time); -extern bool_t booz_sensors_model_accel_available(); -extern bool_t booz_sensors_model_gyro_available(); -extern bool_t booz_sensors_model_mag_available(); -extern bool_t booz_sensors_model_baro_available(); -extern bool_t booz_sensors_model_gps_available(); - - -struct BoozSensorsModel { - - /* our imu alignement */ - MAT* body_to_imu; - - /* Accelerometer */ - VEC* accel; - unsigned int accel_resolution; - MAT* accel_sensitivity; - VEC* accel_neutral; - VEC* accel_noise_std_dev; - VEC* accel_bias; - double accel_next_update; - int accel_available; - - - /* Gyrometer */ - VEC* gyro; - unsigned int gyro_resolution; - MAT* gyro_sensitivity; - VEC* gyro_neutral; - VEC* gyro_noise_std_dev; - VEC* gyro_bias_initial; - VEC* gyro_bias_random_walk_std_dev; - VEC* gyro_bias_random_walk_value; - double gyro_next_update; - int gyro_available; - - - /* Magnetometer */ - VEC* mag; - MAT* mag_imu_to_sensor; - MAT* mag_sensitivity; - VEC* mag_neutral; - VEC* mag_noise_std_dev; - double mag_next_update; - int mag_available; - - - /* Rangemeter */ - double rangemeter; - double rangemeter_next_update; - int rangemeter_available; - - - /* Barometer */ - double baro; - unsigned int baro_resolution; - double baro_next_update; - int baro_available; - - - /* GPS */ - /* state */ - VEC* gps_speed; - VEC* gps_speed_noise_std_dev; - GSList* gps_speed_history; - VEC* gps_pos; - VEC* gps_pos_noise_std_dev; - VEC* gps_pos_bias_initial; - VEC* gps_pos_bias_random_walk_std_dev; - VEC* gps_pos_bias_random_walk_value; - GSList* gps_pos_history; - /* outputs */ - struct LtpDef_d gps_ltp_def; - struct EcefCoor_d gps_speed_ecef; - struct EcefCoor_d gps_pos_ecef; - double gps_pos_utm_north; - double gps_pos_utm_east; - double gps_pos_utm_alt; - // struct Pprz_double_lla gps_pos_lla; - double gps_speed_course; - double gps_speed_gspeed; - double gps_speed_climb; - double gps_next_update; - int gps_available; - -}; - -extern struct BoozSensorsModel bsm; - - -#endif /* BOOZ_SENSORS_MODEL_H */ diff --git a/sw/simulator/old_booz/booz_sensors_model_accel.c b/sw/simulator/old_booz/booz_sensors_model_accel.c deleted file mode 100644 index 77d834bf2f..0000000000 --- a/sw/simulator/old_booz/booz_sensors_model_accel.c +++ /dev/null @@ -1,100 +0,0 @@ -#include "booz_sensors_model.h" - -#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_accel_available() { - if (bsm.accel_available) { - bsm.accel_available = FALSE; - return TRUE; - } - return FALSE; -} - - -void booz_sensors_model_accel_init(double time) { - - bsm.accel = v_get(AXIS_NB); - bsm.accel->ve[AXIS_X] = 0.; - bsm.accel->ve[AXIS_Y] = 0.; - bsm.accel->ve[AXIS_Z] = 0.; - bsm.accel_resolution = BSM_ACCEL_RESOLUTION; - - bsm.accel_sensitivity = m_get(AXIS_NB, AXIS_NB); - m_zero(bsm.accel_sensitivity); - bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = BSM_ACCEL_SENSITIVITY_XX; - bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = BSM_ACCEL_SENSITIVITY_YY; - bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = BSM_ACCEL_SENSITIVITY_ZZ; - - bsm.accel_neutral = v_get(AXIS_NB); - bsm.accel_neutral->ve[AXIS_X] = BSM_ACCEL_NEUTRAL_X; - bsm.accel_neutral->ve[AXIS_Y] = BSM_ACCEL_NEUTRAL_Y; - bsm.accel_neutral->ve[AXIS_Z] = BSM_ACCEL_NEUTRAL_Z; - - bsm.accel_noise_std_dev = v_get(AXIS_NB); - bsm.accel_noise_std_dev->ve[AXIS_X] = BSM_ACCEL_NOISE_STD_DEV_X; - bsm.accel_noise_std_dev->ve[AXIS_Y] = BSM_ACCEL_NOISE_STD_DEV_Y; - bsm.accel_noise_std_dev->ve[AXIS_Z] = BSM_ACCEL_NOISE_STD_DEV_Z; - - bsm.accel_bias = v_get(AXIS_NB); - bsm.accel_bias->ve[AXIS_P] = BSM_ACCEL_BIAS_X; - bsm.accel_bias->ve[AXIS_Q] = BSM_ACCEL_BIAS_Y; - bsm.accel_bias->ve[AXIS_R] = BSM_ACCEL_BIAS_Z; - - bsm.accel_next_update = time; - bsm.accel_available = FALSE; - -} - -void booz_sensors_model_accel_run( double time ) { - 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); - 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); - - - - // printf(" accel_body ~ %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]); - - /* compute accel reading */ - 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; - accel_error = v_resize(accel_error, AXIS_NB); - accel_error = v_zero(accel_error); - /* add a gaussian noise */ - accel_error = v_add_gaussian_noise(accel_error, bsm.accel_noise_std_dev, accel_error); - /* constant bias */ - accel_error = v_add(accel_error, bsm.accel_bias, accel_error); - /* scale to adc units FIXME : should use full adc gain ? sum ? */ - accel_error->ve[AXIS_X] = accel_error->ve[AXIS_X] * bsm.accel_sensitivity->me[AXIS_X][AXIS_X]; - accel_error->ve[AXIS_Y] = accel_error->ve[AXIS_Y] * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y]; - accel_error->ve[AXIS_Z] = accel_error->ve[AXIS_Z] * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z]; - /* add per accel error reading */ - bsm.accel = v_add(bsm.accel, accel_error, bsm.accel); - /* round signal to account for adc discretisation */ - RoundSensor(bsm.accel); - /* saturation */ - BoundSensor(bsm.accel, 0, bsm.accel_resolution); - - // printf("sim adc %f %f %f\n",bsm.accel->ve[AXIS_X] ,bsm.accel->ve[AXIS_Y] ,bsm.accel->ve[AXIS_Z]); - bsm.accel_next_update += BSM_ACCEL_DT; - bsm.accel_available = TRUE; -} - diff --git a/sw/simulator/old_booz/booz_sensors_model_baro.c b/sw/simulator/old_booz/booz_sensors_model_baro.c deleted file mode 100644 index e16e3d1c12..0000000000 --- a/sw/simulator/old_booz/booz_sensors_model_baro.c +++ /dev/null @@ -1,42 +0,0 @@ -#include "booz_sensors_model.h" - -#include BSM_PARAMS -#include "booz_sensors_model_utils.h" -#include "booz_flight_model.h" - -bool_t booz_sensors_model_baro_available() { - if (bsm.baro_available) { - bsm.baro_available = FALSE; - return TRUE; - } - return FALSE; -} - - -void booz_sensors_model_baro_init( double time ) { - bsm.baro = 0.; - - bsm.baro_next_update = time; - bsm.baro_available = FALSE; - -} - -void booz_sensors_model_baro_run( double time ) { - if (time < bsm.baro_next_update) - return; - - if (time < 12.5) - bsm.baro = 840; - else { - 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; - - /* FIXME : add noise and random walk */ - baro_reading = rint(baro_reading); - bsm.baro = baro_reading; - } - bsm.baro_next_update += BSM_BARO_DT; - bsm.baro_available = TRUE; -} diff --git a/sw/simulator/old_booz/booz_sensors_model_gps.c b/sw/simulator/old_booz/booz_sensors_model_gps.c deleted file mode 100644 index c77d2e47c4..0000000000 --- a/sw/simulator/old_booz/booz_sensors_model_gps.c +++ /dev/null @@ -1,151 +0,0 @@ -#include "booz_sensors_model.h" - -#include BSM_PARAMS -#include "booz_sensors_model_utils.h" -#include "booz_flight_model.h" -#include "pprz_geodetic_double.h" - -bool_t booz_sensors_model_gps_available() { - if (bsm.gps_available) { - bsm.gps_available = FALSE; - return TRUE; - } - return FALSE; -} - - - - -void booz_sensors_model_gps_init( double time ) { - - bsm.gps_speed = v_get(AXIS_NB); - v_zero(bsm.gps_speed); - - bsm.gps_speed_noise_std_dev = v_get(AXIS_NB); - bsm.gps_speed_noise_std_dev->ve[AXIS_X] = BSM_GPS_SPEED_NOISE_STD_DEV; - bsm.gps_speed_noise_std_dev->ve[AXIS_Y] = BSM_GPS_SPEED_NOISE_STD_DEV; - bsm.gps_speed_noise_std_dev->ve[AXIS_Z] = BSM_GPS_SPEED_NOISE_STD_DEV; - - bsm.gps_speed_history = NULL; - - bsm.gps_pos = v_get(AXIS_NB); - v_zero(bsm.gps_pos); - - bsm.gps_pos_noise_std_dev = v_get(AXIS_NB); - bsm.gps_pos_noise_std_dev->ve[AXIS_X] = BSM_GPS_POS_NOISE_STD_DEV; - bsm.gps_pos_noise_std_dev->ve[AXIS_Y] = BSM_GPS_POS_NOISE_STD_DEV; - bsm.gps_pos_noise_std_dev->ve[AXIS_Z] = BSM_GPS_POS_NOISE_STD_DEV; - - bsm.gps_pos_bias_initial = v_get(AXIS_NB); - bsm.gps_pos_bias_initial->ve[AXIS_X] = BSM_GPS_POS_BIAS_INITIAL_X; - bsm.gps_pos_bias_initial->ve[AXIS_Y] = BSM_GPS_POS_BIAS_INITIAL_Y; - bsm.gps_pos_bias_initial->ve[AXIS_Z] = BSM_GPS_POS_BIAS_INITIAL_Z; - - bsm.gps_pos_bias_random_walk_std_dev = v_get(AXIS_NB); - bsm.gps_pos_bias_random_walk_std_dev->ve[AXIS_X] = BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X; - bsm.gps_pos_bias_random_walk_std_dev->ve[AXIS_Y] = BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y; - bsm.gps_pos_bias_random_walk_std_dev->ve[AXIS_Z] = BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z; - - bsm.gps_pos_bias_random_walk_value = v_get(AXIS_NB); - bsm.gps_pos_bias_random_walk_value->ve[AXIS_X] = bsm.gps_pos_bias_initial->ve[AXIS_X]; - bsm.gps_pos_bias_random_walk_value->ve[AXIS_Y] = bsm.gps_pos_bias_initial->ve[AXIS_Y]; - bsm.gps_pos_bias_random_walk_value->ve[AXIS_Z] = bsm.gps_pos_bias_initial->ve[AXIS_Z]; - - bsm.gps_pos_history = NULL; - - /* Toulouse */ - struct EcefCoor_d ref_coor = { 4624497.0 , 116475.0, 4376563.0}; - // struct EcefCoor_d ref_coor = { 6368189. , 0., 0.}; - - ltp_def_from_ecef_d(&bsm.gps_ltp_def, &ref_coor); - - bsm.gps_next_update = time; - bsm.gps_available = FALSE; - -} - -/* We store our positions like the fdm and convert to UTM and funcky course, gspeed, climb */ -void booz_sensors_model_gps_run( double time) { - if (time < bsm.gps_next_update) - return; - - /* - * simulate speed sensor - */ - static VEC *cur_speed_reading = VNULL; - cur_speed_reading = v_resize(cur_speed_reading, AXIS_NB); - 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); - UpdateSensorLatency(time, cur_speed_reading, bsm.gps_speed_history, BSM_GPS_SPEED_LATENCY, bsm.gps_speed); - - /* course, gspeed, climb convertion */ - bsm.gps_speed_course = DegOfRad(atan2(bsm.gps_speed->ve[AXIS_Y], bsm.gps_speed->ve[AXIS_X]))*10.; - bsm.gps_speed_course = rint(bsm.gps_speed_course); - bsm.gps_speed_gspeed = sqrt(bsm.gps_speed->ve[AXIS_X] * bsm.gps_speed->ve[AXIS_X] + - bsm.gps_speed->ve[AXIS_Y] * bsm.gps_speed->ve[AXIS_Y]) * 100.; - bsm.gps_speed_gspeed = rint(bsm.gps_speed_gspeed); - bsm.gps_speed_climb = rint(-bsm.gps_speed->ve[AXIS_Z] * 100); - - - /* - * simulate position sensor - */ - - /* compute position error */ - static VEC *pos_error = VNULL; - pos_error = v_resize(pos_error, AXIS_NB); - pos_error = v_zero(pos_error); - /* add a gaussian noise */ - pos_error = v_add_gaussian_noise(pos_error, bsm.gps_pos_noise_std_dev, pos_error); - /* update random walk bias */ - bsm.gps_pos_bias_random_walk_value = - v_update_random_walk(bsm.gps_pos_bias_random_walk_value, - bsm.gps_pos_bias_random_walk_std_dev, BSM_GPS_DT, - bsm.gps_pos_bias_random_walk_value); - /* add it */ - pos_error = v_add(pos_error, bsm.gps_pos_bias_random_walk_value, pos_error); - /* 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 */ - bsm.gps_pos_utm_north = bsm.gps_pos->ve[AXIS_X] * 100. + BSM_GPS_POS_INITIAL_UTM_EAST; - bsm.gps_pos_utm_north = rint(bsm.gps_pos_utm_north); - bsm.gps_pos_utm_east = bsm.gps_pos->ve[AXIS_Y] * 100. + BSM_GPS_POS_INITIAL_UTM_NORTH; - bsm.gps_pos_utm_east = rint(bsm.gps_pos_utm_east); - bsm.gps_pos_utm_alt = bsm.gps_pos->ve[AXIS_Z] * 100. + BSM_GPS_POS_INITIAL_UTM_ALT; - bsm.gps_pos_utm_alt = rint(bsm.gps_pos_utm_alt); - - /* LLA conversion */ - -#if 0 -#define LAT0 40. -#define LON0 1. -#define GROUND_ALT 180. - - 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_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); -#endif - - /* ECEF Conversion */ - struct NedCoor_d pos_ned = {bsm.gps_pos->ve[AXIS_X], bsm.gps_pos->ve[AXIS_Y], bsm.gps_pos->ve[AXIS_Z]}; - ecef_of_ned_point_d(&bsm.gps_pos_ecef, &bsm.gps_ltp_def, &pos_ned); - VECT3_SMUL(bsm.gps_pos_ecef, bsm.gps_pos_ecef, (double)1e2); - struct NedCoor_d speed_ned = {bsm.gps_speed->ve[AXIS_X], bsm.gps_speed->ve[AXIS_Y], bsm.gps_speed->ve[AXIS_Z]}; - ecef_of_ned_vect_d(&bsm.gps_speed_ecef, &bsm.gps_ltp_def , &speed_ned); - VECT3_SMUL(bsm.gps_speed_ecef, bsm.gps_speed_ecef, (double)1e2); - - bsm.gps_next_update += BSM_GPS_DT; - bsm.gps_available = TRUE; - -} - diff --git a/sw/simulator/old_booz/booz_sensors_model_gyro.c b/sw/simulator/old_booz/booz_sensors_model_gyro.c deleted file mode 100644 index f8f607ca0e..0000000000 --- a/sw/simulator/old_booz/booz_sensors_model_gyro.c +++ /dev/null @@ -1,102 +0,0 @@ -#include "booz_sensors_model.h" - -#include BSM_PARAMS -#include "booz_sensors_model_utils.h" -#include "booz_flight_model.h" - -bool_t booz_sensors_model_gyro_available() { - if (bsm.gyro_available) { - bsm.gyro_available = FALSE; - return TRUE; - } - return FALSE; -} - -void booz_sensors_model_gyro_init(double time) { - - bsm.gyro = v_get(AXIS_NB); - bsm.gyro->ve[AXIS_P] = 0.; - bsm.gyro->ve[AXIS_Q] = 0.; - bsm.gyro->ve[AXIS_R] = 0.; - bsm.gyro_resolution = BSM_GYRO_RESOLUTION; - - bsm.gyro_sensitivity = m_get(AXIS_NB, AXIS_NB); - m_zero(bsm.gyro_sensitivity); - bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] = BSM_GYRO_SENSITIVITY_PP; - bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] = BSM_GYRO_SENSITIVITY_QQ; - bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] = BSM_GYRO_SENSITIVITY_RR; - - bsm.gyro_neutral = v_get(AXIS_NB); - bsm.gyro_neutral->ve[AXIS_P] = BSM_GYRO_NEUTRAL_P; - bsm.gyro_neutral->ve[AXIS_Q] = BSM_GYRO_NEUTRAL_Q; - bsm.gyro_neutral->ve[AXIS_R] = BSM_GYRO_NEUTRAL_R; - - bsm.gyro_noise_std_dev = v_get(AXIS_NB); - bsm.gyro_noise_std_dev->ve[AXIS_P] = BSM_GYRO_NOISE_STD_DEV_P; - bsm.gyro_noise_std_dev->ve[AXIS_Q] = BSM_GYRO_NOISE_STD_DEV_Q; - bsm.gyro_noise_std_dev->ve[AXIS_R] = BSM_GYRO_NOISE_STD_DEV_R; - - bsm.gyro_bias_initial = v_get(AXIS_NB); - bsm.gyro_bias_initial->ve[AXIS_P] = BSM_GYRO_BIAS_INITIAL_P; - bsm.gyro_bias_initial->ve[AXIS_Q] = BSM_GYRO_BIAS_INITIAL_Q; - bsm.gyro_bias_initial->ve[AXIS_R] = BSM_GYRO_BIAS_INITIAL_R; - - bsm.gyro_bias_random_walk_std_dev = v_get(AXIS_NB); - bsm.gyro_bias_random_walk_std_dev->ve[AXIS_P] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P; - bsm.gyro_bias_random_walk_std_dev->ve[AXIS_Q] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q; - bsm.gyro_bias_random_walk_std_dev->ve[AXIS_R] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R; - - bsm.gyro_bias_random_walk_value = v_get(AXIS_NB); - bsm.gyro_bias_random_walk_value->ve[AXIS_P] = bsm.gyro_bias_initial->ve[AXIS_P]; - bsm.gyro_bias_random_walk_value->ve[AXIS_Q] = bsm.gyro_bias_initial->ve[AXIS_Q]; - bsm.gyro_bias_random_walk_value->ve[AXIS_R] = bsm.gyro_bias_initial->ve[AXIS_R]; - - bsm.gyro_next_update = time; - bsm.gyro_available = FALSE; - -} - -void booz_sensors_model_gyro_run( double time ) { - if (time < bsm.gyro_next_update) - return; - - /* 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_imu, bsm.gyro); - bsm.gyro = v_add(bsm.gyro, bsm.gyro_neutral, bsm.gyro); - - /* compute gyro error readings */ - static VEC *gyro_error = VNULL; - gyro_error = v_resize(gyro_error, AXIS_NB); - gyro_error = v_zero(gyro_error); - /* add a gaussian noise */ - gyro_error = v_add_gaussian_noise(gyro_error, bsm.gyro_noise_std_dev, gyro_error); - /* update random walk bias */ - bsm.gyro_bias_random_walk_value = - v_update_random_walk(bsm.gyro_bias_random_walk_value, - bsm.gyro_bias_random_walk_std_dev, BSM_GYRO_DT, - bsm.gyro_bias_random_walk_value); - gyro_error = v_add(gyro_error, bsm.gyro_bias_random_walk_value, gyro_error); - /* scale to adc units FIXME : should use full adc gain ? sum ? */ - gyro_error->ve[AXIS_P] = - gyro_error->ve[AXIS_P] * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P]; - gyro_error->ve[AXIS_Q] = - gyro_error->ve[AXIS_Q] * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q]; - gyro_error->ve[AXIS_R] = - gyro_error->ve[AXIS_R] * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R]; - /* add per gyro error reading */ - bsm.gyro = v_add(bsm.gyro, gyro_error, bsm.gyro); - /* round signal to account for adc discretisation */ - RoundSensor(bsm.gyro); - /* saturation */ - BoundSensor(bsm.gyro, 0, bsm.gyro_resolution); - - bsm.gyro_next_update += BSM_GYRO_DT; - bsm.gyro_available = TRUE; -} - - diff --git a/sw/simulator/old_booz/booz_sensors_model_mag.c b/sw/simulator/old_booz/booz_sensors_model_mag.c deleted file mode 100644 index 4ad6b65157..0000000000 --- a/sw/simulator/old_booz/booz_sensors_model_mag.c +++ /dev/null @@ -1,96 +0,0 @@ -#include "booz_sensors_model.h" - -#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) { - bsm.mag_available = FALSE; - return TRUE; - } - return FALSE; -} - - -void booz_sensors_model_mag_init( double time ) { - - bsm.mag = v_get(AXIS_NB); - bsm.mag->ve[AXIS_X] = 0.; - bsm.mag->ve[AXIS_Y] = 0.; - bsm.mag->ve[AXIS_Z] = 0.; - // bsm.mag_resolution = 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); - bsm.mag_sensitivity->me[AXIS_X][AXIS_X] = BSM_MAG_SENSITIVITY_XX; - bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y] = BSM_MAG_SENSITIVITY_YY; - bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z] = BSM_MAG_SENSITIVITY_ZZ; - - bsm.mag_neutral = v_get(AXIS_NB); - bsm.mag_neutral->ve[AXIS_X] = BSM_MAG_NEUTRAL_X; - bsm.mag_neutral->ve[AXIS_Y] = BSM_MAG_NEUTRAL_Y; - bsm.mag_neutral->ve[AXIS_Z] = BSM_MAG_NEUTRAL_Z; - - bsm.mag_noise_std_dev = v_get(AXIS_NB); - bsm.mag_noise_std_dev->ve[AXIS_X] = BSM_MAG_NOISE_STD_DEV_X; - bsm.mag_noise_std_dev->ve[AXIS_Y] = BSM_MAG_NOISE_STD_DEV_Y; - bsm.mag_noise_std_dev->ve[AXIS_Z] = BSM_MAG_NOISE_STD_DEV_Z; - - bsm.mag_next_update = time; - bsm.mag_available = FALSE; - -} - -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); - 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); - - mag_error->ve[AXIS_X] = mag_error->ve[AXIS_X] * bsm.mag_sensitivity->me[AXIS_X][AXIS_X]; - mag_error->ve[AXIS_Y] = mag_error->ve[AXIS_Y] * bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y]; - mag_error->ve[AXIS_Z] = mag_error->ve[AXIS_Z] * bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z]; - - /* add error */ - v_add(bsm.mag, mag_error, bsm.mag); - - // printf("h body %f %f %f\n", h_body->ve[AXIS_X], h_body->ve[AXIS_Y], h_body->ve[AXIS_Z]); - // printf("mag %f %f %f\n", bsm.mag->ve[AXIS_X], bsm.mag->ve[AXIS_Y], bsm.mag->ve[AXIS_Z]); - /* round signal to account for adc discretisation */ - RoundSensor(bsm.mag); - - bsm.mag_next_update += BSM_MAG_DT; - bsm.mag_available = TRUE; -} - diff --git a/sw/simulator/old_booz/booz_sensors_model_params_booz2.h b/sw/simulator/old_booz/booz_sensors_model_params_booz2.h deleted file mode 100644 index 57dad7195f..0000000000 --- a/sw/simulator/old_booz/booz_sensors_model_params_booz2.h +++ /dev/null @@ -1,145 +0,0 @@ -/* - * 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 (65536) -/* ms-2 */ -#define BSM_ACCEL_SENSITIVITY_XX 1250. -#define BSM_ACCEL_SENSITIVITY_YY 1250. -#define BSM_ACCEL_SENSITIVITY_ZZ 1250. -#define BSM_ACCEL_NEUTRAL_X 32768 -#define BSM_ACCEL_NEUTRAL_Y 32768 -#define BSM_ACCEL_NEUTRAL_Z 32768 -/* 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 5e-1 -#define BSM_ACCEL_NOISE_STD_DEV_Y 5e-1 -#define BSM_ACCEL_NOISE_STD_DEV_Z 5e-1 - -/* ms-2 */ -#define BSM_ACCEL_BIAS_X 0 -#define BSM_ACCEL_BIAS_Y 0 -#define BSM_ACCEL_BIAS_Z 0 -/* s */ -#define BSM_ACCEL_DT (1./1000.) - - - -/* - * Gyrometer - */ -#define BSM_GYRO_RESOLUTION 65536 - -#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 32768 -#define BSM_GYRO_NEUTRAL_Q 32768 -#define BSM_GYRO_NEUTRAL_R 32768 - -#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.5) -#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.) -#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.) - -#define BSM_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0) -#define BSM_GYRO_BIAS_INITIAL_Q RadOfDeg( .0) -#define BSM_GYRO_BIAS_INITIAL_R RadOfDeg( .0) - -#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5) -#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.) -#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.) - -#define BSM_GYRO_DT (1./1000.) - - - -/* - * Magnetometer - */ -#define BSM_MAG_RESOLUTION 4096 - -#define BSM_MAG_SENSITIVITY_XX (680) -#define BSM_MAG_SENSITIVITY_YY (680) -#define BSM_MAG_SENSITIVITY_ZZ (680) - -#define BSM_MAG_NEUTRAL_X 0 -#define BSM_MAG_NEUTRAL_Y 0 -#define BSM_MAG_NEUTRAL_Z 0 - -#define BSM_MAG_NOISE_STD_DEV_X 0 -#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_DT (1./1000.) - - -/* - * 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 900. -#define BSM_BARO_SENSITIVITY 10. -#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/old_booz/booz_sensors_model_params_booz2_a1.h b/sw/simulator/old_booz/booz_sensors_model_params_booz2_a1.h deleted file mode 100644 index 352a6b0d01..0000000000 --- a/sw/simulator/old_booz/booz_sensors_model_params_booz2_a1.h +++ /dev/null @@ -1,184 +0,0 @@ -/* - * 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 - -#include "generated/airframe.h" - - -#define BSM_BODY_TO_IMU_PHI RadOfDeg(4.) -#define BSM_BODY_TO_IMU_THETA RadOfDeg(3.) -//#define BSM_BODY_TO_IMU_PHI RadOfDeg(0.) -//#define BSM_BODY_TO_IMU_THETA RadOfDeg(0.) -#define BSM_BODY_TO_IMU_PSI RadOfDeg(0.) - -/* - * Accelerometer - */ -#define BSM_ACCEL_RESOLUTION (65536) -/* ms-2 */ -/* aka 2^10/ACCEL_X_SENS */ -#define BSM_ACCEL_SENSITIVITY_XX -408.92695 -#define BSM_ACCEL_SENSITIVITY_YY -412.69325 -#define BSM_ACCEL_SENSITIVITY_ZZ -407.32522 -#define BSM_ACCEL_NEUTRAL_X 32081 -#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 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 -#define BSM_ACCEL_BIAS_Y 0 -#define BSM_ACCEL_BIAS_Z 0 -/* s */ -#define BSM_ACCEL_DT (1./512.) - - - -/* - * Gyrometer - */ -#define BSM_GYRO_RESOLUTION 65536 - -/* 2^12/GYRO_X_SENS */ -#define BSM_GYRO_SENSITIVITY_PP ( 4055.) -#define BSM_GYRO_SENSITIVITY_QQ (-4055.) -#define BSM_GYRO_SENSITIVITY_RR (-4055.) - -#define BSM_GYRO_NEUTRAL_P 33924 -#define BSM_GYRO_NEUTRAL_Q 33417 -#define BSM_GYRO_NEUTRAL_R 32809 - -//#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.) -//#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.) -//#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.) - -#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.5) -#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.5) -#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.5) - -#define BSM_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0) -#define BSM_GYRO_BIAS_INITIAL_Q RadOfDeg( .0) -#define BSM_GYRO_BIAS_INITIAL_R RadOfDeg( .0) - -#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.) -#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.) -#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.) - -#define BSM_GYRO_DT (1./512.) - - - -/* - * Magnetometer - */ -//#define BSM_MAG_RESOLUTION 65536 - -#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 -#define BSM_MAG_NEUTRAL_Z 2119 - -//#define BSM_MAG_NOISE_STD_DEV_X 0 -//#define BSM_MAG_NOISE_STD_DEV_Y 0 -//#define BSM_MAG_NOISE_STD_DEV_Z 0 - -#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.) - - -/* - * 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 - */ -/* m */ -/* aka 2^8/INS_BARO_SENS */ -#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 - */ - -#ifdef GPS_PERFECT - -#define BSM_GPS_SPEED_NOISE_STD_DEV 0. -#define BSM_GPS_SPEED_LATENCY 0. -#define BSM_GPS_POS_NOISE_STD_DEV 0.001 -#define BSM_GPS_POS_BIAS_INITIAL_X 0. -#define BSM_GPS_POS_BIAS_INITIAL_Y 0. -#define BSM_GPS_POS_BIAS_INITIAL_Z 0. -#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 0. -#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 0. -#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 0. -#define BSM_GPS_POS_LATENCY 0. - -#else - -#define BSM_GPS_SPEED_NOISE_STD_DEV 1e-1 -#define BSM_GPS_SPEED_LATENCY 0.25 -#define BSM_GPS_POS_NOISE_STD_DEV 1e-1 -#define BSM_GPS_POS_BIAS_INITIAL_X 0e-1 -#define BSM_GPS_POS_BIAS_INITIAL_Y -0e-1 -#define BSM_GPS_POS_BIAS_INITIAL_Z -0e-1 -#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-3 -#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-3 -#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-3 -#define BSM_GPS_POS_LATENCY 0.25 - -#endif /* GPS_PERFECT */ - -#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/old_booz/booz_sensors_model_rangemeter.c b/sw/simulator/old_booz/booz_sensors_model_rangemeter.c deleted file mode 100644 index e1a8ed7d7d..0000000000 --- a/sw/simulator/old_booz/booz_sensors_model_rangemeter.c +++ /dev/null @@ -1,47 +0,0 @@ -#include "booz_sensors_model.h" - -#include BSM_PARAMS -#include "booz_sensors_model_utils.h" -#include "booz_flight_model.h" - -bool_t booz_sensors_model_rangemeter_available() { - if (bsm.rangemeter_available) { - bsm.rangemeter_available = FALSE; - return TRUE; - } - return FALSE; -} - - -void booz_sensors_model_rangemeter_init(double time) { - - bsm.rangemeter = 0.; - - bsm.rangemeter_next_update = time; - bsm.rangemeter_available = FALSE; - -} - - -void booz_sensors_model_rangemeter_run( double time ) { - if (time < bsm.rangemeter_next_update) - return; - - /* compute dist from ground */ - double dz = bfm.pos_ltp->ve[AXIS_Z]; - if (dz > 0.) dz = 0.; - 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 */ - - if (dist > BSM_RANGEMETER_MAX_RANGE) - dist = BSM_RANGEMETER_MAX_RANGE; - dist = rint(dist); - bsm.rangemeter = dist; - - bsm.rangemeter_next_update += BSM_RANGEMETER_DT; - bsm.rangemeter_available = TRUE; - -} diff --git a/sw/simulator/old_booz/booz_sensors_model_utils.c b/sw/simulator/old_booz/booz_sensors_model_utils.c deleted file mode 100644 index 29ba3f2530..0000000000 --- a/sw/simulator/old_booz/booz_sensors_model_utils.c +++ /dev/null @@ -1,94 +0,0 @@ -#include "booz_sensors_model_utils.h" - -#include "6dof.h" -#include - -void UpdateSensorLatency(double time, VEC* cur_reading, GSList **history, - double latency, VEC* sensor_reading) { - /* add new reading */ - struct BoozDatedSensor* cur_read = g_new(struct BoozDatedSensor, 1); - cur_read->time = time; - cur_read->value = v_get(AXIS_NB); - v_copy(cur_reading, cur_read->value); - *history = g_slist_prepend(*history, cur_read); - /* remove old readings */ - GSList* last = g_slist_last(*history); - while (last && - ((struct BoozDatedSensor*)last->data)->time < time - latency) { - *history = g_slist_remove_link(*history, last); - v_free(((struct BoozDatedSensor*)last->data)->value); - g_free((struct BoozDatedSensor*)last->data); - g_slist_free(last); - last = g_slist_last(*history); - } - /* update sensor */ - v_copy(((struct BoozDatedSensor*)last->data)->value, sensor_reading); -} - -#define THAU 5. -VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* out) { - - static VEC *drw = VNULL; - drw = v_resize(drw, AXIS_NB); - v_get_gaussian_noise(std_dev, drw); - - static VEC *tmp = VNULL; - tmp = v_resize(tmp, AXIS_NB); - v_copy(in, tmp); - sv_mlt((-1./THAU), tmp, tmp); - v_add(drw, tmp, drw); - sv_mlt(dt, drw, drw); - v_add(drw, in, out); - return out; -} - -VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out) { - static VEC *tmp = VNULL; - 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; -} - -VEC* v_get_gaussian_noise(VEC* std_dev, VEC* out) { - - unsigned int i; - for (i=0; idim; i++) - out->ve[i] = get_gaussian_noise() * std_dev->ve[i]; - return out; - -} - -/* - http://www.taygeta.com/random/gaussian.html -*/ - -#include "booz_r250.h" - -double get_gaussian_noise(void) { - - double x1; - static int nb_call = 0; - static double 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/old_booz/booz_sensors_model_utils.h b/sw/simulator/old_booz/booz_sensors_model_utils.h deleted file mode 100644 index c4cb8278fd..0000000000 --- a/sw/simulator/old_booz/booz_sensors_model_utils.h +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef BOOZ_SENSORS_MODEL_UTILS_H -#define BOOZ_SENSORS_MODEL_UTILS_H - -#include -#include - -struct BoozDatedSensor { - VEC* value; - double time; -}; - -extern void UpdateSensorLatency(double time, VEC* cur_reading, GSList **history, - double latency, VEC* sensor_reading); -extern VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* out); -extern VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out); -extern VEC* v_get_gaussian_noise(VEC* std_dev, VEC* out); -extern double get_gaussian_noise(); - -#define RoundSensor(_sensor) { \ - _sensor->ve[AXIS_X] = rint(_sensor->ve[AXIS_X]); \ - _sensor->ve[AXIS_Y] = rint(_sensor->ve[AXIS_Y]); \ - _sensor->ve[AXIS_Z] = rint(_sensor->ve[AXIS_Z]); \ - } - -#define BoundSensor(_sensor, _min, _max) { \ - if ( _sensor->ve[AXIS_X] < _min) _sensor->ve[AXIS_X] = _min; \ - if ( _sensor->ve[AXIS_X] > _max) _sensor->ve[AXIS_X] = _max; \ - if ( _sensor->ve[AXIS_Y] < _min) _sensor->ve[AXIS_Y] = _min; \ - if ( _sensor->ve[AXIS_Y] > _max) _sensor->ve[AXIS_Y] = _max; \ - if ( _sensor->ve[AXIS_Z] < _min) _sensor->ve[AXIS_Z] = _min; \ - if ( _sensor->ve[AXIS_Z] > _max) _sensor->ve[AXIS_Z] = _max; \ - } - - - -#endif /* BOOZ_SENSORS_MODEL_UTILS_H */ diff --git a/sw/simulator/old_booz/booz_wind_model.c b/sw/simulator/old_booz/booz_wind_model.c deleted file mode 100644 index 6aaa633e71..0000000000 --- a/sw/simulator/old_booz/booz_wind_model.c +++ /dev/null @@ -1,76 +0,0 @@ -#include "booz_wind_model.h" - -#include "6dof.h" -#include "booz_flight_model_utils.h" - -#if 0 -static void booz_wind_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot); -#endif - -struct BoozWindModel bwm; - -#define BWM_STATE_SIZE 6 - -#define BWM_WM 1.5 -#define BWM_ZETA 5e-5 -#define BWM_STD_DEV 3.3 -#define BWM_MAX_X 3. -#define BWM_MAX_Y 3. -#define BWM_MAX_Z 3. - -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); - -} - - -void booz_wind_model_run( double dt __attribute__ ((unused))) { - - -#if 0 - static VEC *u = VNULL; - u = v_resize(u, AXIS_NB); - u = v_rand(u); - static VEC *one = VNULL; - one = v_resize(one, AXIS_NB); - one = v_ones(one); - u = v_mltadd(one, u, -2., u); - u = sv_mlt((BWM_STD_DEV * BWM_STD_DEV), u, u); - - rk4(booz_wind_model_get_derivatives, bwm.state, u, dt); - - bwm.velocity->ve[AXIS_X] = bwm.state->ve[0] * BWM_WM * BWM_WM * BWM_MAX_X; - bwm.velocity->ve[AXIS_Y] = bwm.state->ve[2] * BWM_WM * BWM_WM * BWM_MAX_Y; - bwm.velocity->ve[AXIS_Z] = bwm.state->ve[4] * BWM_WM * BWM_WM * BWM_MAX_Z; -#endif - - -} - - -#if 0 - -static void booz_wind_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot) { - - Xdot->ve[0] = X->ve[1]; - Xdot->ve[1] = -2. * BWM_ZETA * BWM_WM * X->ve[1] - - BWM_WM * BWM_WM * X->ve[0] + u->ve[AXIS_X]; - Xdot->ve[2] = X->ve[3]; - Xdot->ve[3] = -2. * BWM_ZETA * BWM_WM * X->ve[3] - - BWM_WM * BWM_WM * X->ve[2] + u->ve[AXIS_Y]; - Xdot->ve[4] = X->ve[5]; - Xdot->ve[5] = -2. * BWM_ZETA * BWM_WM * X->ve[5] - - BWM_WM * BWM_WM * X->ve[4] + u->ve[AXIS_Z]; - -} - -#endif diff --git a/sw/simulator/old_booz/booz_wind_model.h b/sw/simulator/old_booz/booz_wind_model.h deleted file mode 100644 index 49e65a65a8..0000000000 --- a/sw/simulator/old_booz/booz_wind_model.h +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef BOOZ_WIND_MODEL_H -#define BOOZ_WIND_MODEL_H - -#include - -struct BoozWindModel { - - /* velocity in earth tp frame */ - VEC* velocity; - - /* internal state */ - VEC* state; - -}; - -extern struct BoozWindModel bwm; - -extern void booz_wind_model_init( void ); -extern void booz_wind_model_run( double dt ); - -#endif /* BOOZ_WIND_MODEL_H */ diff --git a/sw/simulator/old_booz/tests/Makefile b/sw/simulator/old_booz/tests/Makefile deleted file mode 100644 index 4fbd86e779..0000000000 --- a/sw/simulator/old_booz/tests/Makefile +++ /dev/null @@ -1,76 +0,0 @@ -# -# NPS -# -#JSBSIM = /usr/local - -Q=@ - -#CC = g++ -#CFLAGS = -Wall -I$(JSBSIM)/include/JSBSim -I../include -#LDFLAGS = -L$(JSBSIM)/lib -lJSBSim -#CFLAGS += -I/usr/include/meschach -I/usr/local/include/ -#LDFLAGS += -lmeschach -L/usr/lib -#CFLAGS += `pkg-config glib-2.0 --cflags` -#LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lglibivy -lpcre - -#test1: nps_test1.c nps_jsbsim.c -# $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) - -#test2: nps_test2.c -# $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) - -#test_nps_fdm: test_nps_fdm.c nps_fdm.c -# $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) - - -JSBSIM = /home/violato/enac/programs/install_jsbsim - -CFLAGS = -Wall \ - -I.. \ - -I../../../var/BOOZ2_A1 \ - -I../../airborne \ - -I../../include \ - -I$(JSBSIM)/include/JSBSim \ - `pkg-config glib-2.0 --cflags` \ - -LDFLAGS = -lm \ - -lglibivy \ - -L$(JSBSIM)/lib -lJSBSim \ - `pkg-config glib-2.0 --libs` \ - -SIMDIR = .. - -# -# -# - -TEST_FDM_SRCS = nps_test_fdm.c \ - $(SIMDIR)/nps_fdm_jsbsim.c \ - - -nps_test_fdm : $(TEST_FDM_SRCS) - g++ $(CFLAGS) -o $@ $^ $(LDFLAGS) - - -# -# -# - -TEST_SENSORS_SRCS = test_sensors.c \ - $(SIMDIR)/booz_flight_model.c \ - $(SIMDIR)/booz_flight_model_utils.c \ - $(SIMDIR)/booz_sensors_model.c \ - $(SIMDIR)/booz_sensors_model_utils.c \ - $(SIMDIR)/booz_sensors_model_accel.c \ - $(SIMDIR)/booz_sensors_model_gyro.c \ - $(SIMDIR)/booz_sensors_model_mag.c \ - $(SIMDIR)/booz_sensors_model_rangemeter.c \ - $(SIMDIR)/booz_sensors_model_baro.c \ - $(SIMDIR)/booz_sensors_model_gps.c \ - -test_sensors : $(TEST_SENSORS_SRCS) - gcc $(CFLAGS) -o $@ $^ $(LDFLAGS) - -clean: - $(Q)rm -f *.o - diff --git a/sw/simulator/old_booz/tests/test_fdm.c b/sw/simulator/old_booz/tests/test_fdm.c deleted file mode 100644 index bb7cb44156..0000000000 --- a/sw/simulator/old_booz/tests/test_fdm.c +++ /dev/null @@ -1,24 +0,0 @@ -#include "booz_flight_model.h" - - -#define DT_FDM (1./250.) -#define NB_ITER 100 - -int main(int argc, char** argv) { - - double actuators_values[] = {0.6, 0.6, 0.6, 0.6}; - double time = 0.; - - booz_flight_model_init(); - - bfm.on_ground = FALSE; - - int i; - for (i=0; ive[BFMS_Z]); - } - - return 0; -} diff --git a/sw/simulator/old_booz/tests/test_sensors.c b/sw/simulator/old_booz/tests/test_sensors.c deleted file mode 100644 index f712bc9876..0000000000 --- a/sw/simulator/old_booz/tests/test_sensors.c +++ /dev/null @@ -1,32 +0,0 @@ -#include "booz_flight_model.h" -#include "booz_sensors_model.h" - - -#define DT_FDM (1./250.) -#define NB_ITER 100 - -int main(int argc, char** argv) { - - double actuators_values[] = {0.6, 0.6, 0.6, 0.6}; - double time = 0.; - - booz_flight_model_init(); - booz_sensors_model_init(time); - - bfm.on_ground = FALSE; - - int i; - for (i=0; ive[AXIS_Z], bsm.gps_speed->ve[AXIS_Z]); - if (booz_sensors_model_accel_available()) - printf("%f \t %f \t %f \t %f\n", time, bsm.accel->ve[AXIS_X], bsm.accel->ve[AXIS_Y], bsm.accel->ve[AXIS_Z]); - } - - return 0; -} diff --git a/sw/simulator/sim_ac_booz.c b/sw/simulator/sim_ac_booz.c deleted file mode 100644 index acc08ccd2d..0000000000 --- a/sw/simulator/sim_ac_booz.c +++ /dev/null @@ -1,98 +0,0 @@ -/* - * Copyright (C) 2008 Gautier Hattenberger - * - * 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. - * - */ - -#include -#include "sim_ac_jsbsim.h" -#include "firmwares/fixedwing/main_ap.h" -#include "firmwares/fixedwing/main_fbw.h" - -using namespace JSBSim; - -//static void sim_gps_feed_data(void); -//static void sim_ir_feed_data(void); - - -void airborne_run_one_step(void) { - // SEE sim_run_one_step -} - -void sim_autopilot_init(void) { - init_fbw(); - init_ap(); -} - -void autopilot_periodic_task(void) { - periodic_task_ap(); - periodic_task_fbw(); -} - -void autopilot_event_task(void) { - event_task_ap(); - event_task_fbw(); -} - -void copy_inputs_to_jsbsim(FGFDMExec* FDMExec) { - - FGPropertyManager* cur_node; - double cur_value; - char buf[64]; - const char* state[] = {"front_motor", - "back_motor", - "right_motor", - "left_motor"}; - - for (int i=0; i<4; i++) { - sprintf(buf,"fcs/%s",state[i]); - FDMExec->GetPropertyManager()->SetDouble(buf,0.5); - cur_node = FDMExec->GetPropertyManager()->GetNode(buf); - cur_value = cur_node->getDoubleValue(); - cout << state[i] << " " << cur_value << endl; - } -} - -void copy_outputs_from_jsbsim(FGFDMExec* FDMExec) { - - FGPropertyManager* cur_node; - double cur_value, factor=1; - char buf[64]; - const char* state[] = {"sim-time-sec", - "lat-gc-deg","long-gc-deg","h-sl-ft", - "u-fps","v-fps","w-fps", - "theta-deg","phi-deg","psi-true-deg", - "p-rad_sec","q-rad_sec","r-rad_sec"}; - int i=0; - - cur_node = FDMExec->GetPropertyManager()->GetNode("sim-time-sec"); - cur_value = cur_node->getDoubleValue(); - cout << state[i] << cur_value << endl; - - for (i=1; i<12+1; i++) { - sprintf(buf,"ic/%s",state[i]); - if (strstr(state[i],"rad_")!=NULL) factor=RAD2DEG; - if (strstr(state[i],"fps")!=NULL || strstr(state[i],"ft")!=NULL) factor=FT2M; - cur_node = FDMExec->GetPropertyManager()->GetNode(buf); - cur_value = factor*(cur_node->getDoubleValue()); - cout << state[i] << " " << cur_value << endl; - factor = 1; - } - -}