Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[simulator] cleanup: remove old_booz sim

Browse Source
This commit is contained in:
Felix Ruess
2013-10-29 19:26:04 +01:00
parent 69feae7dc6
commit c9fa0b4692
34 changed files with 0 additions and 3274 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/old_booz/booz2_sim_main.c
-449
View File
@@ -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 <glib.h>
#include <getopt.h>
#include <sys/time.h>
#include <Ivy/ivy.h>
#include <Ivy/ivyglibloop.h>
#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: /* $B!G(B?$B!G(B */
printf("?? getopt returned character code 0%o ??\n", c);
fprintf(stderr, usage, argv[0]);
exit(EXIT_FAILURE);
}
}
}
sw/simulator/old_booz/booz_flight_model.c
-362
View File
@@ -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 <math.h>
#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; i<SERVOS_NB; i++)
bfm.mot_voltage->ve[i] = bfm.bat_voltage * commands[i];
// rk4(motor_model_derivative, bfm.mot_omega, bfm.mot_voltage, dt);
rk4(booz_flight_model_get_derivatives, bfm.state, bfm.mot_voltage, dt);
/* wrap euler angles */
WRAP( bfm.state->ve[BFMS_PHI], M_PI);
WRAP( bfm.state->ve[BFMS_THETA], M_PI_2);
WRAP( bfm.state->ve[BFMS_PSI], M_PI);
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
sw/simulator/old_booz/booz_flight_model.h
-65
View File
@@ -1,65 +0,0 @@
#ifndef BOOZ_FLIGHT_MODEL_H
#define BOOZ_FLIGHT_MODEL_H
#include <matrix.h>
#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 */
sw/simulator/old_booz/booz_flight_model_params.h
-44
View File
@@ -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 */
sw/simulator/old_booz/booz_flight_model_utils.c
-100
View File
@@ -1,100 +0,0 @@
#include "booz_flight_model_utils.h"
#include <math.h>
#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;
}
sw/simulator/old_booz/booz_flight_model_utils.h
-18
View File
@@ -1,18 +0,0 @@
#ifndef BOOZ_FM_UTILS_H
#define BOOZ_FM_UTILS_H
#include <matrix.h>
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 */
sw/simulator/old_booz/booz_flightgear.c
-88
View File
@@ -1,88 +0,0 @@
#include "booz_flightgear.h"
#include "flight_gear.h"
#include <sys/socket.h>
#include <netdb.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
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.;
}
sw/simulator/old_booz/booz_flightgear.h
-9
View File
@@ -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 */
sw/simulator/old_booz/booz_joystick.c
-84
View File
@@ -1,84 +0,0 @@
#include "booz_joystick.h"
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <glib.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <linux/joystick.h>
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<JS_NB_AXIS; i++)
booz_joystick_value[i] = 0.;
booz_joystick_value[JS_MODE] = -0.7;
int fd = open(device, O_RDONLY | O_NONBLOCK);
if (fd == -1) {
printf("opening joystick serial device %s : %s\n", device, strerror(errno));
return;
}
GIOChannel* channel = g_io_channel_unix_new(fd);
g_io_channel_set_encoding(channel, NULL, NULL);
g_io_add_watch (channel, G_IO_IN , on_data_received, NULL);
}
static gboolean on_data_received(GIOChannel *source,
GIOCondition condition __attribute__ ((unused)),
gpointer data __attribute__ ((unused))) {
struct js_event js;
gsize len;
GError *err = NULL;
g_io_channel_read_chars(source, (void*)(&js), sizeof(struct js_event), &len, &err);
if (js.type == JS_EVENT_AXIS) {
if (js.number < JS_NB_AXIS) {
// if (js.number == JS_THROTTLE) printf("joystick value %d\n",js.value);
booz_joystick_value[js.number] = (double)(js.value - booz_joystick_neutral[js.number]) /
(booz_joystick_max[js.number] - booz_joystick_min[js.number]);
}
}
return TRUE;
}
#if 0
switch (js.number) {
case JS_THROTTLE:
ppm_pulses[RADIO_THROTTLE] = 1223 + (js.value - 30) * (float)(2050-1223) / (float)(223 - 30);
break;
case JS_PITCH:
ppm_pulses[RADIO_PITCH] = 1498 + (js.value - 113) * (float)(2050-950) / (float)(224 - 1);
break;
case JS_ROLL:
ppm_pulses[RADIO_ROLL] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(1 - 224);
break;
case JS_YAW:
ppm_pulses[RADIO_YAW] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(224 - 1);
break;
case JS_MODE:
ppm_pulses[RADIO_MODE] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(224 - 1);
rc_values_contains_avg_channels = TRUE;
break;
}
#endif
sw/simulator/old_booz/booz_joystick.h
-19
View File
@@ -1,19 +0,0 @@
#ifndef BOOZ_JOYSTICK_H
#define BOOZ_JOYSTICK_H
#define JS_ROLL 0
#define JS_PITCH 1
#define JS_MODE 2
#define JS_YAW 5
#define JS_THROTTLE 6
#define JS_NB_AXIS 7
extern void booz_joystick_init(const char* device);
extern double booz_joystick_value[JS_NB_AXIS];
#endif /* BOOZ_JOYSTICK_H */
sw/simulator/old_booz/booz_joystick_fake.h
-153
View File
@@ -1,153 +0,0 @@
#ifndef BOOZ_JOYSTICK_FAKE_H
#define BOOZ_JOYSTICK_FAKE_H
#define BREAK_MTT() { \
int foo = sim_time/10; \
switch(foo%4) { \
case 0: \
ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950); \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
break; \
case 1: \
ppm_pulses[RADIO_PITCH] = 1500 + 0.0 * (2050-950); \
ppm_pulses[RADIO_YAW] = 1493 + 0.4 * (2050-950); \
break; \
case 2: \
ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950); \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
break; \
case 3: \
ppm_pulses[RADIO_PITCH] = 1500 - 0.0 * (2050-950); \
ppm_pulses[RADIO_YAW] = 1493 + 0.4 * (2050-950); \
break; \
} \
}
#define WALK_OVAL() { \
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
int foo = sim_time/10; \
switch(foo%4) { \
case 0: \
ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
break; \
case 1: \
ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950); \
break; \
case 2: \
ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \
break; \
case 3: \
ppm_pulses[RADIO_YAW] = 1493 + 0.2 * (2050-950); \
break; \
} \
}
#define HOVER() { \
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.6 * (2050-1223); \
ppm_pulses[RADIO_MODE] = 1500 - 0.5 * (2050-950); \
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950); \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
}
#define TOUPIE() { \
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950); \
int foo = sim_time/20; \
switch(foo%4) { \
case 0: \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
break; \
case 1: \
ppm_pulses[RADIO_YAW] = 1493 + 0.2 * (2050-950); \
break; \
case 2: \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
break; \
case 3: \
ppm_pulses[RADIO_YAW] = 1493 + -0.2 * (2050-950); \
break; \
} \
}
#define CIRCLE() { \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
double alpha = 0.01 * sim_time; \
ppm_pulses[RADIO_PITCH] = 1498 + cos(alpha) * 250.; \
ppm_pulses[RADIO_ROLL] = 1500 + sin(alpha) * 250.; \
}
#define ATTITUDE_ROLL_STEPS() { \
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.66 * (2050-1223); \
ppm_pulses[RADIO_MODE] = 1500 + 0. * (2050-950); \
ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950); \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
int foo = sim_time/10; \
switch(foo%2) { \
case 0: \
ppm_pulses[RADIO_ROLL] = 1500 + 0.2 * (2050-950); \
break; \
case 1: \
ppm_pulses[RADIO_ROLL] = 1500 - 0.2 * (2050-950); \
break; \
} \
}
#define ATTITUDE_PITCH_STEPS() { \
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.66 * (2050-1223); \
ppm_pulses[RADIO_MODE] = 1500 + 0. * (2050-950); \
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
int foo = sim_time/10; \
switch(foo%2) { \
case 0: \
ppm_pulses[RADIO_PITCH] = 1498 + 0.2 * (2050-950); \
break; \
case 1: \
ppm_pulses[RADIO_PITCH] = 1498 - 0.2 * (2050-950); \
break; \
} \
}
#define ATTITUDE_YAW_STEPS() { \
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.66 * (2050-1223); \
ppm_pulses[RADIO_MODE] = 1500 + 0. * (2050-950); \
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \
int foo = sim_time/10; \
switch(foo%2) { \
case 0: \
ppm_pulses[RADIO_YAW] = 1493 + 0.2 * (2050-950); \
break; \
case 1: \
ppm_pulses[RADIO_YAW] = 1493 - 0.2 * (2050-950); \
break; \
} \
}
#define TWO_POINTS() { \
ppm_pulses[RADIO_YAW] = 1493 + 0. * (2050-950); \
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
int foo = sim_time/10; \
switch(foo%4) { \
case 0: \
ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950); \
break; \
case 1: \
ppm_pulses[RADIO_PITCH] = 1498 + 0.3 * (2050-950); \
break; \
case 2: \
ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950); \
break; \
case 3: \
ppm_pulses[RADIO_PITCH] = 1498 - 0.3 * (2050-950); \
break; \
} \
}
#endif /* BOOZ_JOYSTICK_FAKE_H */
sw/simulator/old_booz/booz_r250.c
-245
View File
@@ -1,245 +0,0 @@
/* 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 <limits.h>
#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 <stdio.h>
#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
sw/simulator/old_booz/booz_r250.h
-21
View File
@@ -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
sw/simulator/old_booz/booz_randlcg.c
-51
View File
@@ -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 <math.h>
#include <limits.h>
#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;
}
sw/simulator/old_booz/booz_randlcg.h
-19
View File
@@ -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
sw/simulator/old_booz/booz_rc_sim.h
-33
View File
@@ -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 */
sw/simulator/old_booz/booz_sensors_model.c
-64
View File
@@ -1,64 +0,0 @@
#include "booz_sensors_model.h"
#include BSM_PARAMS
#include <math.h>
#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);
}
sw/simulator/old_booz/booz_sensors_model.h
-126
View File
@@ -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 <matrix.h>
#include <glib.h>
#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 */
sw/simulator/old_booz/booz_sensors_model_accel.c
-100
View File
@@ -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;
}
sw/simulator/old_booz/booz_sensors_model_baro.c
-42
View File
@@ -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;
}
sw/simulator/old_booz/booz_sensors_model_gps.c
-151
View File
@@ -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;
}
sw/simulator/old_booz/booz_sensors_model_gyro.c
-102
View File
@@ -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;
}
sw/simulator/old_booz/booz_sensors_model_mag.c
-96
View File
@@ -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;
}
sw/simulator/old_booz/booz_sensors_model_params_booz2.h
-145
View File
@@ -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 */
sw/simulator/old_booz/booz_sensors_model_params_booz2_a1.h
-184
View File
@@ -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 */
sw/simulator/old_booz/booz_sensors_model_rangemeter.c
-47
View File
@@ -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;
}
sw/simulator/old_booz/booz_sensors_model_utils.c
-94
View File
@@ -1,94 +0,0 @@
#include "booz_sensors_model_utils.h"
#include "6dof.h"
#include <math.h>
void UpdateSensorLatency(double time, VEC* cur_reading, GSList **history,
double latency, VEC* sensor_reading) {
/* add new reading */
struct BoozDatedSensor* cur_read = g_new(struct BoozDatedSensor, 1);
cur_read->time = time;
cur_read->value = v_get(AXIS_NB);
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; i<in->dim; 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; i<out->dim; 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;
}
sw/simulator/old_booz/booz_sensors_model_utils.h
-36
View File
@@ -1,36 +0,0 @@
#ifndef BOOZ_SENSORS_MODEL_UTILS_H
#define BOOZ_SENSORS_MODEL_UTILS_H
#include <matrix.h>
#include <glib.h>
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 */
sw/simulator/old_booz/booz_wind_model.c
-76
View File
@@ -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
sw/simulator/old_booz/booz_wind_model.h
-21
View File
@@ -1,21 +0,0 @@
#ifndef BOOZ_WIND_MODEL_H
#define BOOZ_WIND_MODEL_H
#include <matrix.h>
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 */
sw/simulator/old_booz/tests/Makefile
-76
View File
@@ -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
sw/simulator/old_booz/tests/test_fdm.c
-24
View File
@@ -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; i<NB_ITER; i++) {
booz_flight_model_run(DT_FDM, actuators_values);
time += DT_FDM;
printf("%f \t %f\n", time, bfm.state->ve[BFMS_Z]);
}
return 0;
}
sw/simulator/old_booz/tests/test_sensors.c
-32
View File
@@ -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; i<NB_ITER; i++) {
booz_flight_model_run(DT_FDM, actuators_values);
time += DT_FDM;
booz_sensors_model_run(time);
// if (booz_sensors_model_baro_available())
// printf("%f \t %f\n", time, bsm.baro);
// if (booz_sensors_model_gps_available())
// printf("%f \t %f \t %f\n", time, bsm.gps_pos->ve[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;
}
sw/simulator/sim_ac_booz.c
-98
View File
@@ -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 <string.h>
#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;
}
}