Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

hura ! sim navigates

Browse Source
This commit is contained in:
Antoine Drouin
2009-02-19 05:39:11 +00:00
parent 78c49f5629
commit 862d352257
32 changed files with 862 additions and 506 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz2_a1.xml
+7 -5
View File
@@ -61,12 +61,14 @@
<define name="MAG_Y_NEUTRAL" value="2362"/>
<define name="MAG_Z_NEUTRAL" value="2119"/>
<!-- <define name="MAG_X_SENS" value="-4.94075530" integer="16"/>
<define name="MAG_Y_SENS" value="5.10207664" integer="16"/> * sqrt(2)/2 -->
<define name="MAG_X_SENS" value="-3.4936416" integer="16"/>
<define name="MAG_Y_SENS" value=" 3.607713" integer="16"/>
<!-- <define name="MAG_X_SENS" value="-4.94075530" integer="16"/>-->
<!-- <define name="MAG_Y_SENS" value=" 5.10207664" integer="16"/>-->
<define name="MAG_Z_SENS" value="-4.90788848" integer="16"/>
<define name="MAG_45_HACK" value="1"/>
<define name="MAG_45_HACK" value="1"/>
<!-- <define name="MAG_X_SENS" value="-4.94075530 * sqrt(2)/2" integer="16"/> -->
<!-- <define name="MAG_Y_SENS" value=" 5.10207664 * sqrt(2)/2" integer="16"/> -->
<define name="MAG_X_SENS" value="-3.4936416" integer="16"/>
<define name="MAG_Y_SENS" value=" 3.607713" integer="16"/>
</section>
conf/autopilot/booz2_simulator.makefile
+9 -2
View File
@@ -17,7 +17,12 @@ sim.CFLAGS += -DSITL
sim.CFLAGS += `pkg-config glib-2.0 --cflags` -I /usr/include/meschach
sim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lmeschach -lpcre -lglibivy
sim.CFLAGS += -DBYPASS_AHRS
#sim.CFLAGS += -DBYPASS_AHRS
#sim.CFLAGS += -DBYPASS_INS
sim.CFLAGS += -DINIT_WIND_X=-5.0
sim.CFLAGS += -DINIT_WIND_Y=-0.0
sim.CFLAGS += -DINIT_WIND_Z=-0.0
sim.CFLAGS += -I$(SRC_BOOZ) -I$(SRC_BOOZ_SIM) -I../simulator -DFLOAT_T=float
@@ -26,7 +31,9 @@ sim.srcs = $(SIMDIR)/booz2_sim_main.c \
$(SIMDIR)/booz_flight_model_utils.c \
$(SIMDIR)/booz_sensors_model.c \
$(SIMDIR)/booz_sensors_model_utils.c \
$(SIMDIR)/booz_sensors_model_accel.c \
$(SIMDIR)/booz_r250.c \
$(SIMDIR)/booz_randlcg.c \
$(SIMDIR)/booz_sensors_model_accel.c \
$(SIMDIR)/booz_sensors_model_gyro.c \
$(SIMDIR)/booz_sensors_model_mag.c \
$(SIMDIR)/booz_sensors_model_rangemeter.c \
conf/messages.xml
+13 -13
View File
@@ -458,17 +458,17 @@
<message name="BOOZ_SIM_RPMS" id="239">
<field name="front" type="float" unit="RPM"/>
<field name="back" type="float" unit="RPM"/>
<field name="left" type="float" unit="RPM"/>
<field name="right" type="float" unit="RPM"/>
<field name="left" type="float" unit="RPM"/>
</message>
<message name="BOOZ_SIM_SPEED_POS" id="240">
<field name="u" type="float" unit="m/s"/>
<field name="v" type="float" unit="m/s"/>
<field name="w" type="float" unit="m/s"/>
<field name="x" type="float" unit="m"/>
<field name="y" type="float" unit="m"/>
<field name="z" type="float" unit="m"/>
<field name="xd" type="float" unit="m/s"/>
<field name="yd" type="float" unit="m/s"/>
<field name="zd" type="float" unit="m/s"/>
<field name="x" type="float" unit="m"/>
<field name="y" type="float" unit="m"/>
<field name="z" type="float" unit="m"/>
</message>
<message name="BOOZ_SIM_RATE_ATTITUDE" id="241">
@@ -654,9 +654,9 @@
</message>
<message name="BOOZ2_MAG" id="133">
<field name="mx" type="int32"/>
<field name="my" type="int32"/>
<field name="mz" type="int32"/>
<field name="mx" type="int32" alt_unit="1" alt_unit_coef="0.0004883"/>
<field name="my" type="int32" alt_unit="1" alt_unit_coef="0.0004883"/>
<field name="mz" type="int32" alt_unit="1" alt_unit_coef="0.0004883"/>
</message>
<message name="BOOZ2_FILTER" id="134">
@@ -666,9 +666,9 @@
<field name="measure_phi" type="int32" alt_unit="degres" alt_unit_coef="0.0000273"/>
<field name="measure_theta" type="int32" alt_unit="degres" alt_unit_coef="0.0000273"/>
<field name="measure_psi" type="int32" alt_unit="degres" alt_unit_coef="0.0000273"/>
<field name="corrected_phi" type="int32"/>
<field name="corrected_theta" type="int32"/>
<field name="corrected_psi" type="int32"/>
<field name="corrected_phi" type="int32" alt_unit="degres" alt_unit_coef="0.0001093"/>
<field name="corrected_theta" type="int32" alt_unit="degres" alt_unit_coef="0.0001093"/>
<field name="corrected_psi" type="int32" alt_unit="degres" alt_unit_coef="0.0001093"/>
<field name="correction_phi" type="int32"/>
<field name="correction_theta" type="int32"/>
<field name="correction_psi" type="int32"/>
conf/settings/settings_booz2.xml
+1 -1
View File
@@ -5,7 +5,7 @@
<dl_settings NAME="Misc">
<dl_setting var="telemetry_mode_Main" min="0" step="1" max="11" module="booz2_telemetry" shortname="telemetry" values="Default|PPM|Raw sensors|Scaled sensors|Filter|Rate loop|Att loop|Vert loop|H loop|Aligner|HS_att_roll|Tune_hover"/>
<dl_setting var="booz_fms_on" min="0" step="1" max="1" module="booz2_fms" shortname="fms_on" values="OFF|ON">
<dl_setting var="booz_fms_on" min="0" step="1" max="1" module="booz2_fms" shortname="fms_on" values="OFF|ON" handler="SetOnOff">
<strip_button name="FMS ON" icon="on.png" value="1"/>
<strip_button name="FMS OFF" icon="off.png" value="0"/>
</dl_setting>
conf/settings/settings_booz2_fms_ts.xml
+2 -2
View File
@@ -3,8 +3,8 @@
<dl_settings NAME="FMS_TS">
<dl_setting var="booz_fms_test_signal_period" min="1" step="1" max="100" module="booz2_fms_test_signal" shortname="period" handler="SetPeriod"/>
<dl_setting var="booz_fms_test_signal_amplitude" min="0" step="100" max="10000" module="booz2_fms_test_signal" shortname="amplitude"/>
<dl_setting var="booz_fms_test_signal_mode" min="0" step="1" max="2" module="booz2_fms_test_signal" shortname="ts mode" handler="SetTsMode" values="DISABLED|ATTITUDE|VERTICAL"/>
<dl_setting var="booz_fms_test_signal_amplitude" min="0" step="100" max="183000" module="booz2_fms_test_signal" shortname="amplitude"/>
<dl_setting var="booz_fms_test_signal_mode" min="0" step="1" max="1" module="booz2_fms_test_signal" shortname="ts mode" handler="SetTsMode" values="ATTITUDE|VERTICAL"/>
</dl_settings>
</dl_settings>
sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c
+10 -10
View File
@@ -174,25 +174,25 @@ void booz2_filter_attitude_update(void) {
}
// FIXME
// FIXME : make a real frame change and rotate rates too
static inline void apply_alignment(void) {
#if 0
booz2_filter_attitude_euler_aligned.phi =
booz2_filter_attitude_euler.phi
+ (FILTER_ALIGNMENT_DPSI * booz2_filter_attitude_euler.theta) >> IANGLE_RES
- (FILTER_ALIGNMENT_DTHETA * booz2_filter_attitude_euler.psi) >> IANGLE_RES;
booz2_filter_attitude_euler.phi
+ (((int32_t)FILTER_ALIGNMENT_DPSI * booz2_filter_attitude_euler.theta) >> IANGLE_RES)
- (((int32_t)FILTER_ALIGNMENT_DTHETA * booz2_filter_attitude_euler.psi) >> IANGLE_RES);
booz2_filter_attitude_euler_aligned.theta =
- (FILTER_ALIGNMENT_DPSI * booz2_filter_attitude_euler.phi) >> IANGLE_RES
+ booz2_filter_attitude_euler.theta
+ (FILTER_ALIGNMENT_DPHI * booz2_filter_attitude_euler.psi) >> IANGLE_RES;
- (((int32_t)FILTER_ALIGNMENT_DPSI * booz2_filter_attitude_euler.phi) >> IANGLE_RES)
+ booz2_filter_attitude_euler.theta
+ (((int32_t)FILTER_ALIGNMENT_DPHI * booz2_filter_attitude_euler.psi) >> IANGLE_RES);
booz2_filter_attitude_euler_aligned.psi =
(FILTER_ALIGNMENT_DTHETA * booz2_filter_attitude_euler.phi) >> IANGLE_RES
- (FILTER_ALIGNMENT_DPHI * booz2_filter_attitude_euler.theta) >> IANGLE_RES
+ booz2_filter_attitude_euler.psi;
(((int32_t)FILTER_ALIGNMENT_DTHETA * booz2_filter_attitude_euler.phi) >> IANGLE_RES)
- (((int32_t)FILTER_ALIGNMENT_DPHI * booz2_filter_attitude_euler.theta) >> IANGLE_RES)
+ booz2_filter_attitude_euler.psi;
#else
booz2_filter_attitude_euler_aligned.phi = booz2_filter_attitude_euler.phi - FILTER_ALIGNMENT_DPHI;
sw/airborne/booz/booz2_fms.c
+3
View File
@@ -38,8 +38,11 @@ void booz_fms_periodic(void) {
booz_fms_impl_periodic();
}
void booz_fms_set_on_off(bool_t state) {
}
void booz_fms_update_info(void) {
PPRZ_INT16_OF_INT32_VECT3(booz_fms_info.imu.gyro, booz_imu_state.gyro);
sw/airborne/booz/booz2_fms.h
+6
View File
@@ -80,6 +80,7 @@ extern struct Booz_fms_info booz_fms_info;
extern struct Booz_fms_command booz_fms_input;
extern void booz_fms_init(void);
extern void booz_fms_set_on_off(bool_t state);
extern void booz_fms_periodic(void);
extern void booz_fms_update_info(void);
@@ -109,6 +110,11 @@ extern void booz_fms_update_info(void);
booz_fms_input.h_sp.pos.z = _psi_sp; \
}
#define booz2_fms_SetOnOff(_val) { \
booz_fms_on = _val; \
booz_fms_set_on_off(booz_fms_on); \
}
#endif /* BOOZ2_FMS_H */
sw/airborne/booz/booz2_fms_test_signal.c
+2 -7
View File
@@ -17,24 +17,19 @@ uint32_t booz_fms_test_signal_counter;
uint32_t booz_fms_test_signal_start_z;
void booz_fms_impl_init(void) {
booz_fms_test_signal_mode = BOOZ_FMS_TEST_SIGNAL_MODE_DISABLED;
booz_fms_test_signal_mode = BOOZ_FMS_TEST_SIGNAL_MODE_ATTITUDE;
booz_fms_test_signal_period = BOOZ2_FMS_TEST_SIGNAL_DEFAULT_PERIOD;
booz_fms_test_signal_amplitude = BOOZ2_FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE;
booz_fms_test_signal_axe = BOOZ2_FMS_TEST_SIGNAL_DEFAULT_AXE;
booz_fms_test_signal_counter = 0;
booz_fms_input.h_mode = BOOZ2_GUIDANCE_H_MODE_ATTITUDE;
booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_RC_DIRECT;
booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_HOVER;
}
void booz_fms_impl_periodic(void) {
switch (booz_fms_test_signal_mode) {
case BOOZ_FMS_TEST_SIGNAL_MODE_DISABLED: {
PPRZ_INT32_EULER_ASSIGN(booz_fms_input.h_sp.attitude, 0, 0, 0);
}
break;
case BOOZ_FMS_TEST_SIGNAL_MODE_ATTITUDE: {
if (booz_fms_test_signal_counter < booz_fms_test_signal_period) {
PPRZ_INT32_EULER_ASSIGN(booz_fms_input.h_sp.attitude, booz_fms_test_signal_amplitude, 0, 0);
sw/airborne/booz/booz2_fms_test_signal.h
+3 -8
View File
@@ -3,9 +3,8 @@
#include "std.h"
#define BOOZ_FMS_TEST_SIGNAL_MODE_DISABLED 0
#define BOOZ_FMS_TEST_SIGNAL_MODE_ATTITUDE 1
#define BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL 2
#define BOOZ_FMS_TEST_SIGNAL_MODE_ATTITUDE 0
#define BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL 1
extern uint8_t booz_fms_test_signal_mode;
@@ -24,11 +23,7 @@ extern void booz_fms_impl_periodic(void);
#define booz2_fms_test_signal_SetTsMode(_val) { \
booz_fms_test_signal_mode = _val; \
if (booz_fms_test_signal_mode == BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL) \
booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_HOVER; \
else \
booz_fms_input.v_mode = BOOZ2_GUIDANCE_V_MODE_RC_DIRECT; \
}
}
#endif /* BOOZ_FMS_TEST_SIGNAL_H */
sw/simulator/booz2_sim_main.c
+144 -83
View File
@@ -24,6 +24,7 @@
#include <glib.h>
#include <getopt.h>
#include <sys/time.h>
#include <Ivy/ivy.h>
#include <Ivy/ivyglibloop.h>
@@ -40,9 +41,14 @@ char* fg_host = "10.31.4.107";
unsigned int fg_port = 5501;
char* joystick_dev = "/dev/input/js0";
/* rate of the host mainloop */
#define HOST_TIMEOUT_PERIOD 4
struct timeval host_time_start;
double host_time_elapsed;
double host_time_factor = 1.;
/* 250Hz <-> 4ms */
//#define TIMEOUT_PERIOD 4
#define DT (1./512.)
#define SIM_DT (1./512.)
double sim_time;
#define DT_DISPLAY 0.04
@@ -50,7 +56,13 @@ double disp_time;
double booz_sim_actuators_values[] = {0., 0., 0., 0.};
static void sim_bypass_ahrs(void);
static void sim_run_one_step(void);
#ifdef BYPASS_AHRS
static void sim_overwrite_ahrs(void);
#endif
#ifdef BYPASS_INS
static void sim_overwrite_ins(void);
#endif
static void sim_gps_feed_data(void);
static void sim_mag_feed_data(void);
@@ -68,6 +80,7 @@ static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)),
static void booz2_sim_init(void) {
gettimeofday (&host_time_start, NULL);
sim_time = 0.;
disp_time = 0.;
@@ -87,80 +100,124 @@ static void booz2_sim_init(void) {
#include "booz2_imu.h"
static gboolean booz2_sim_periodic(gpointer data __attribute__ ((unused))) {
/* read actuators positions */
booz_sim_read_actuators();
/* run our models */
if (sim_time > 13.)
bfm.on_ground = FALSE;
booz_wind_model_run(DT);
booz_flight_model_run(DT, booz_sim_actuators_values);
booz_sensors_model_run(sim_time);
struct timeval host_time_now;
gettimeofday (&host_time_now, NULL);
host_time_elapsed = host_time_factor *
((host_time_now.tv_sec - host_time_start.tv_sec) +
(host_time_now.tv_usec - host_time_start.tv_usec)*1e-6);
sim_time += DT;
/* outputs models state */
booz2_sim_display();
/* run the airborne code */
while (sim_time <= host_time_elapsed) {
sim_run_one_step();
sim_time += SIM_DT;
}
// feed a rc frame and signal event
BoozRcSimFeed(sim_time);
// process it
booz2_main_event();
if (booz_sensors_model_baro_available()) {
Booz2BaroISRHandler(bsm.baro);
booz2_main_event();
}
if (booz_sensors_model_gyro_available()) {
booz2_imu_feed_data();
booz2_main_event();
#ifdef BYPASS_AHRS
sim_bypass_ahrs();
#endif /* BYPASS_AHRS */
}
if (booz_sensors_model_gps_available()) {
sim_gps_feed_data();
booz2_main_event();
}
if (booz_sensors_model_mag_available()) {
sim_mag_feed_data();
booz2_main_event();
}
booz2_main_periodic();
return TRUE;
}
#include "booz_geometry_mixed.h"
#include "booz2_filter_attitude.h"
static void sim_bypass_ahrs(void) {
booz2_filter_attitude_euler_aligned.phi = BOOZ_ANGLE_I_OF_F(bfm.state->ve[BFMS_PHI]);
booz2_filter_attitude_euler_aligned.theta = BOOZ_ANGLE_I_OF_F(bfm.state->ve[BFMS_THETA]);
booz2_filter_attitude_euler_aligned.psi = BOOZ_ANGLE_I_OF_F(bfm.state->ve[BFMS_PSI]);
static void sim_run_one_step(void) {
/* read actuators positions */
booz_sim_read_actuators();
/* run our models */
if (sim_time > 13.)
bfm.on_ground = FALSE;
booz_wind_model_run(SIM_DT);
booz_flight_model_run(SIM_DT, booz_sim_actuators_values);
booz_sensors_model_run(sim_time);
/* outputs models state */
booz2_sim_display();
/* run the airborne code */
// feed a rc frame and signal event
BoozRcSimFeed(sim_time);
// process it
booz2_main_event();
if (booz_sensors_model_baro_available()) {
Booz2BaroISRHandler(bsm.baro);
booz2_main_event();
#ifdef BYPASS_INS
sim_overwrite_ins();
#endif /* BYPASS_INS */
}
if (booz_sensors_model_gyro_available()) {
booz2_imu_feed_data();
booz2_main_event();
#ifdef BYPASS_AHRS
sim_overwrite_ahrs();
#endif /* BYPASS_AHRS */
#ifdef BYPASS_INS
sim_overwrite_ins();
#endif /* BYPASS_INS */
}
if (booz_sensors_model_gps_available()) {
sim_gps_feed_data();
booz2_main_event();
}
if (booz_sensors_model_mag_available()) {
sim_mag_feed_data();
booz2_main_event();
#ifdef BYPASS_AHRS
sim_overwrite_ahrs();
#endif /* BYPASS_AHRS */
}
booz2_main_periodic();
booz2_filter_attitude_rate.x = BOOZ_RATE_I_OF_F(bfm.state->ve[BFMS_P]);
booz2_filter_attitude_rate.y = BOOZ_RATE_I_OF_F(bfm.state->ve[BFMS_Q]);
booz2_filter_attitude_rate.z = BOOZ_RATE_I_OF_F(bfm.state->ve[BFMS_R]);
}
#ifdef BYPASS_AHRS
#include "booz_geometry_mixed.h"
#include "booz2_filter_attitude.h"
static void sim_overwrite_ahrs(void) {
booz2_filter_attitude_euler_aligned.phi = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_X]);
booz2_filter_attitude_euler_aligned.theta = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_Y]);
booz2_filter_attitude_euler_aligned.psi = BOOZ_ANGLE_I_OF_F(bfm.eulers->ve[AXIS_Z]);
booz2_filter_attitude_rate.x = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_X]);
booz2_filter_attitude_rate.y = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_Y]);
booz2_filter_attitude_rate.z = BOOZ_RATE_I_OF_F(bfm.ang_rate_body->ve[AXIS_Z]);
}
#endif /* BYPASS_AHRS */
#ifdef BYPASS_INS
#include "booz2_ins.h"
static void sim_overwrite_ins(void) {
booz_ins_position.z = BOOZ_POS_I_OF_F(bfm.pos_ltp->ve[AXIS_Z]);
booz_ins_speed_earth.z = BOOZ_SPEED_I_OF_F(bfm.speed_ltp->ve[AXIS_Z]);
booz_ins_accel_earth.z = BOOZ_ACCEL_I_OF_F(bfm.accel_ltp->ve[AXIS_Z]);
}
#endif /* BYPASS_INS */
#include "booz2_gps.h"
static void sim_gps_feed_data(void) {
booz2_gps_lat = bsm.gps_pos_lla.lat;
booz2_gps_lon = bsm.gps_pos_lla.lon;
// speed?
booz2_gps_vel_n = rint(bsm.gps_speed->ve[AXIS_Y] * 100.);
booz2_gps_vel_e = rint(bsm.gps_speed->ve[AXIS_X] * 100.);
booz2_gps_vel_n = rint(bsm.gps_speed->ve[AXIS_X] * 100.);
booz2_gps_vel_e = rint(bsm.gps_speed->ve[AXIS_Y] * 100.);
booz_gps_state.fix = BOOZ2_GPS_FIX_3D;
}
@@ -169,16 +226,20 @@ static void sim_mag_feed_data(void) {
#ifdef IMU_MAG_45_HACK
// booz2_imu_mag.x = msx - msy;
// booz2_imu_mag.y = msx + msy;
int32_t foo_x = bsm.mag->ve[AXIS_X] * cos(M_PI_4);
int32_t foo_y = bsm.mag->ve[AXIS_Y] * cos(M_PI_4);
ami601_val[IMU_MAG_X_CHAN] = foo_x + foo_y;
ami601_val[IMU_MAG_Y_CHAN] = -foo_x + foo_y;
// double cos_pi_4 = cos(M_PI_4);
// int32_t foo_x = bsm.mag->ve[AXIS_X];// / cos(M_PI_4);
// int32_t foo_y = bsm.mag->ve[AXIS_Y];// / cos(M_PI_4);
ami601_val[IMU_MAG_X_CHAN] = bsm.mag->ve[AXIS_X];
ami601_val[IMU_MAG_Y_CHAN] = bsm.mag->ve[AXIS_Y];
// ami601_val[IMU_MAG_X_CHAN] = foo_x + foo_y;
// ami601_val[IMU_MAG_Y_CHAN] = -foo_x + foo_y;
ami601_val[IMU_MAG_Z_CHAN] = bsm.mag->ve[AXIS_Z];
#else
ami601_val[IMU_MAG_X_CHAN] = bsm.mag->ve[AXIS_X];
ami601_val[IMU_MAG_Y_CHAN] = bsm.mag->ve[AXIS_Y];
ami601_val[IMU_MAG_Z_CHAN] = bsm.mag->ve[AXIS_Z];
#endif
ami601_status = AMI601_DATA_AVAILABLE;
}
@@ -190,26 +251,26 @@ static void booz2_sim_display(void) {
// booz_flightgear_send();
IvySendMsg("%d BOOZ_SIM_RPMS %f %f %f %f",
AC_ID,
RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_F]),
RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_B]),
RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_L]),
RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_R]) );
RPM_OF_RAD_S(bfm.omega->ve[SERVO_FRONT]),
RPM_OF_RAD_S(bfm.omega->ve[SERVO_BACK]),
RPM_OF_RAD_S(bfm.omega->ve[SERVO_RIGHT]),
RPM_OF_RAD_S(bfm.omega->ve[SERVO_LEFT]) );
IvySendMsg("%d BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f",
AC_ID,
DegOfRad(bfm.state->ve[BFMS_P]),
DegOfRad(bfm.state->ve[BFMS_Q]),
DegOfRad(bfm.state->ve[BFMS_R]),
DegOfRad(bfm.state->ve[BFMS_PHI]),
DegOfRad(bfm.state->ve[BFMS_THETA]),
DegOfRad(bfm.state->ve[BFMS_PSI]));
DegOfRad(bfm.ang_rate_body->ve[AXIS_X]),
DegOfRad(bfm.ang_rate_body->ve[AXIS_Y]),
DegOfRad(bfm.ang_rate_body->ve[AXIS_Z]),
DegOfRad(bfm.eulers->ve[AXIS_X]),
DegOfRad(bfm.eulers->ve[AXIS_Y]),
DegOfRad(bfm.eulers->ve[AXIS_Z]));
IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f",
AC_ID,
(bfm.state->ve[BFMS_U]),
(bfm.state->ve[BFMS_V]),
(bfm.state->ve[BFMS_W]),
(bfm.state->ve[BFMS_X]),
(bfm.state->ve[BFMS_Y]),
(bfm.state->ve[BFMS_Z]));
(bfm.speed_ltp->ve[AXIS_X]),
(bfm.speed_ltp->ve[AXIS_Y]),
(bfm.speed_ltp->ve[AXIS_Z]),
(bfm.pos_ltp->ve[AXIS_X]),
(bfm.pos_ltp->ve[AXIS_Y]),
(bfm.pos_ltp->ve[AXIS_Z]));
IvySendMsg("%d BOOZ_SIM_WIND %f %f %f",
AC_ID,
bwm.velocity->ve[AXIS_X],
@@ -226,7 +287,7 @@ int main ( int argc, char** argv) {
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
g_timeout_add(4, booz2_sim_periodic, NULL);
g_timeout_add(HOST_TIMEOUT_PERIOD, booz2_sim_periodic, NULL);
g_main_loop_run(ml);
sw/simulator/booz_flight_model.c
+154 -53
View File
@@ -1,9 +1,60 @@
#include "booz_flight_model.h"
#define BFMS_X 0
#define BFMS_Y 1
#define BFMS_Z 2
#define BFMS_XD 3
#define BFMS_YD 4
#define BFMS_ZD 5
#define BFMS_PHI 6
#define BFMS_THETA 7
#define BFMS_PSI 8
#define BFMS_P 9
#define BFMS_Q 10
#define BFMS_R 11
#define BFMS_OM_B 12
#define BFMS_OM_F 13
#define BFMS_OM_R 14
#define BFMS_OM_L 15
#define BFMS_SIZE 16
#define BoozFlighModelGetPos(_dest) { \
_dest->ve[AXIS_X] = bfm.state->ve[BFMS_X]; \
_dest->ve[AXIS_Y] = bfm.state->ve[BFMS_Y]; \
_dest->ve[AXIS_Z] = bfm.state->ve[BFMS_Z]; \
}
#define BoozFlighModelGetSpeedLtp(_dest) { \
_dest->ve[AXIS_X] = bfm.state->ve[BFMS_XD]; \
_dest->ve[AXIS_Y] = bfm.state->ve[BFMS_YD]; \
_dest->ve[AXIS_Z] = bfm.state->ve[BFMS_ZD]; \
}
#define BoozFlighModelGetAngles(_dest) { \
_dest->ve[EULER_PHI] = bfm.state->ve[BFMS_PHI]; \
_dest->ve[EULER_THETA] = bfm.state->ve[BFMS_THETA]; \
_dest->ve[EULER_PSI] = bfm.state->ve[BFMS_PSI]; \
}
#define BoozFlighModelGetRate(_dest) { \
_dest->ve[AXIS_P] = bfm.state->ve[BFMS_P]; \
_dest->ve[AXIS_Q] = bfm.state->ve[BFMS_Q]; \
_dest->ve[AXIS_R] = bfm.state->ve[BFMS_R]; \
}
#define BoozFlighModelGetRPMS(_dest) { \
_dest->ve[SERVO_BACK] = bfm.state->ve[BFMS_OM_B]; \
_dest->ve[SERVO_FRONT] = bfm.state->ve[BFMS_OM_F]; \
_dest->ve[SERVO_RIGHT] = bfm.state->ve[BFMS_OM_R]; \
_dest->ve[SERVO_LEFT] = bfm.state->ve[BFMS_OM_L]; \
}
#include <math.h>
#include "booz_flight_model_params.h"
#include "booz_flight_model_utils.h"
#include "booz_wind_model.h"
#include "6dof.h"
@@ -12,7 +63,8 @@ struct BoozFlightModel bfm;
//static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot);
//static VEC* booz_get_forces_body_frame(VEC* M, MAT* dcm, VEC* omega_square, VEC* speed_body);
static void booz_flight_model_update_byproducts(void);
static VEC* booz_get_forces_ltp(VEC* F , VEC* speed_ltp, MAT* dcm_t, VEC* omega_square);
static VEC* booz_get_moments_body_frame(VEC* M, VEC* omega_square );
static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot);
@@ -24,18 +76,37 @@ void booz_flight_model_init( void ) {
bfm.mot_voltage = v_get(SERVOS_NB);
bfm.state = v_get(BFMS_SIZE);
bfm.pos_ltp = v_get(AXIS_NB);
bfm.speed_ltp = v_get(AXIS_NB);
bfm.accel_ltp = v_get(AXIS_NB);
bfm.speed_body = v_get(AXIS_NB);
bfm.accel_body = v_get(AXIS_NB);
bfm.eulers = v_get(AXIS_NB);
bfm.ang_rate_body = v_get(AXIS_NB);
bfm.ang_accel_body = v_get(AXIS_NB);
bfm.dcm = m_get(AXIS_NB, AXIS_NB);
bfm.dcm_t = m_get(AXIS_NB, AXIS_NB);
bfm.quat = v_get(AXIS_NB);
bfm.omega = v_get(SERVOS_NB);
bfm.omega_square = v_get(SERVOS_NB);
/* constants */
bfm.g_earth = v_get(AXIS_NB);
bfm.g_earth->ve[AXIS_X] = 0.;
bfm.g_earth->ve[AXIS_Y] = 0.;
bfm.g_earth->ve[AXIS_Z] = G;
bfm.g_ltp = v_get(AXIS_NB);
bfm.g_ltp->ve[AXIS_X] = 0.;
bfm.g_ltp->ve[AXIS_Y] = 0.;
bfm.g_ltp->ve[AXIS_Z] = G;
/* FIXME */
bfm.h_earth = v_get(AXIS_NB);
bfm.h_earth->ve[AXIS_X] = 1.;
bfm.h_earth->ve[AXIS_Y] = 0.;
bfm.h_earth->ve[AXIS_Z] = 1.;
bfm.h_ltp = v_get(AXIS_NB);
bfm.h_ltp->ve[AXIS_X] = 1.;
bfm.h_ltp->ve[AXIS_Y] = 0.;
bfm.h_ltp->ve[AXIS_Z] = 1.;
bfm.thrust_factor = 0.5 * RHO * PROP_AREA * C_t * PROP_RADIUS * PROP_RADIUS;
bfm.torque_factor = 0.5 * RHO * PROP_AREA * C_q * PROP_RADIUS * PROP_RADIUS;
@@ -82,36 +153,79 @@ void booz_flight_model_run( double dt, double* commands ) {
WRAP( bfm.state->ve[BFMS_PHI], M_PI);
WRAP( bfm.state->ve[BFMS_THETA], M_PI_2);
WRAP( bfm.state->ve[BFMS_PSI], M_PI);
booz_flight_model_update_byproducts();
bfm.time += dt;
}
static void booz_flight_model_update_byproducts(void) {
/* extract eulers angles from state */
BoozFlighModelGetAngles( bfm.eulers);
/* extract body rotational rates from state */
BoozFlighModelGetRate( bfm.ang_rate_body);
/* direct cosine matrix ( inertial to body )*/
dcm_of_eulers(bfm.eulers, bfm.dcm);
/* transpose of dcm ( body to inertial ) */
m_transp(bfm.dcm, bfm.dcm_t);
BoozFlighModelGetPos(bfm.pos_ltp);
/* extract speed in ltp frame from state */
BoozFlighModelGetSpeedLtp(bfm.speed_ltp);
/* extract prop rotational speeds from state */
BoozFlighModelGetRPMS(bfm.omega);
/* compute square */
v_star(bfm.omega, bfm.omega, bfm.omega_square);
/* compute ltp accelerations */
static VEC *f_ltp = VNULL;
f_ltp = v_resize(f_ltp, AXIS_NB);
f_ltp = booz_get_forces_ltp(f_ltp , bfm.speed_ltp, bfm.dcm_t, bfm.omega_square);
sv_mlt( 1./bfm.mass, f_ltp, bfm.accel_ltp);
/* rotate speed and accel to body frame */
mv_mlt(bfm.dcm, bfm.speed_ltp, bfm.speed_body);
mv_mlt(bfm.dcm, bfm.accel_ltp, bfm.accel_body);
/* rotational accelerations */
/* quaternion */
}
/*
compute the sum of external forces.
assumes that dcm and omega_square are already precomputed from X
*/
VEC* booz_get_forces_body_frame(VEC* F , MAT* dcm, VEC* omega_square, VEC* speed_body) {
static VEC* booz_get_forces_ltp(VEC* F , VEC* speed_ltp, MAT* dcm_t, VEC* omega_square) {
// FIXME : nimporte koi !
if (bfm.on_ground) {
F->ve[AXIS_X] = 0;
F->ve[AXIS_Y] = 0;
F->ve[AXIS_Z] = 0;
}
else {
F = v_zero(F);
if (!bfm.on_ground) {
// propeller thrust
F->ve[AXIS_X] = 0;
F->ve[AXIS_Y] = 0;
F->ve[AXIS_Z] = -v_sum(omega_square) * bfm.thrust_factor;
static VEC *prop_thrust_body = VNULL;
prop_thrust_body = v_resize(prop_thrust_body, AXIS_NB);
prop_thrust_body->ve[AXIS_X] = 0;
prop_thrust_body->ve[AXIS_Y] = 0;
prop_thrust_body->ve[AXIS_Z] = -v_sum(omega_square) * bfm.thrust_factor;
static VEC *prop_thrust_ltp = VNULL;
prop_thrust_ltp = v_resize(prop_thrust_ltp, AXIS_NB);
prop_thrust_ltp = mv_mlt(dcm_t, prop_thrust_body, prop_thrust_ltp);
F = v_add(F, prop_thrust_ltp, F);
// gravity
static VEC *g_body = VNULL;
g_body = v_resize(g_body, AXIS_NB);
g_body = mv_mlt(dcm, bfm.g_earth, g_body);
F = v_mltadd(F, g_body, bfm.mass, F);
// body drag
double norm_speed = v_norm2(speed_body);
F = v_mltadd(F, speed_body, -norm_speed * C_d_body, F);
F = v_mltadd(F, bfm.g_ltp, bfm.mass, F);
// drag
static VEC *airspeed_ltp = VNULL;
airspeed_ltp = v_resize(airspeed_ltp, AXIS_NB);
airspeed_ltp = v_sub(speed_ltp, bwm.velocity, airspeed_ltp);
double norm_speed = v_norm2(airspeed_ltp);
F = v_mltadd(F, airspeed_ltp, -norm_speed * C_d_body, F);
}
return F;
}
@@ -149,40 +263,27 @@ static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot) {
static MAT *dcm_t = MNULL;
dcm_t = m_resize(dcm_t,AXIS_NB, AXIS_NB);
dcm_t = m_transp(dcm, dcm_t);
/* extract body_speeds_from state */
static VEC *speed_body = VNULL;
speed_body = v_resize(speed_body, AXIS_NB);
BoozFlighModelGetSpeed(speed_body);
/* extract ltp_speeds_from state */
static VEC *speed_ltp = VNULL;
speed_ltp = v_resize(speed_ltp, AXIS_NB);
BoozFlighModelGetSpeedLtp(speed_ltp);
/* extracts body rates from state */
static VEC *rate_body = VNULL;
rate_body = v_resize(rate_body, AXIS_NB);
BoozFlighModelGetRate(rate_body);
/* derivatives of position */
static VEC *speed_earth = VNULL;
speed_earth = v_resize(speed_earth, AXIS_NB);
speed_earth = mv_mlt(dcm_t, speed_body,speed_earth);
Xdot->ve[BFMS_X] = speed_earth->ve[AXIS_X];
Xdot->ve[BFMS_Y] = speed_earth->ve[AXIS_Y];
Xdot->ve[BFMS_Z] = speed_earth->ve[AXIS_Z];
Xdot->ve[BFMS_X] = speed_ltp->ve[AXIS_X];
Xdot->ve[BFMS_Y] = speed_ltp->ve[AXIS_Y];
Xdot->ve[BFMS_Z] = speed_ltp->ve[AXIS_Z];
/* derivatives of speed */
/* compute external forces */
static VEC *f_body = VNULL;
f_body = v_resize(f_body, AXIS_NB);
f_body = booz_get_forces_body_frame(f_body , dcm, omega_square, speed_body);
/* add non inertial forces */
static VEC *fict_f = VNULL;
fict_f = v_resize(fict_f, AXIS_NB);
fict_f = out_prod(speed_body, rate_body, fict_f);
fict_f = sv_mlt(bfm.mass, fict_f, fict_f);
f_body = v_add(f_body, fict_f, f_body);
Xdot->ve[BFMS_U] = 1./bfm.mass * f_body->ve[AXIS_X];
Xdot->ve[BFMS_V] = 1./bfm.mass * f_body->ve[AXIS_Y];
Xdot->ve[BFMS_W] = 1./bfm.mass * f_body->ve[AXIS_Z];
static VEC *f_ltp = VNULL;
f_ltp = v_resize(f_ltp, AXIS_NB);
f_ltp = booz_get_forces_ltp(f_ltp , speed_ltp, dcm_t, omega_square);
Xdot->ve[BFMS_XD] = 1./bfm.mass * f_ltp->ve[AXIS_X];
Xdot->ve[BFMS_YD] = 1./bfm.mass * f_ltp->ve[AXIS_Y];
Xdot->ve[BFMS_ZD] = 1./bfm.mass * f_ltp->ve[AXIS_Z];
/* derivatives of eulers */
double sinPHI = sin(eulers->ve[EULER_PHI]);
sw/simulator/booz_flight_model.h
+20 -53
View File
@@ -5,24 +5,6 @@
#include "airframe.h"
#define BFMS_X 0
#define BFMS_Y 1
#define BFMS_Z 2
#define BFMS_U 3
#define BFMS_V 4
#define BFMS_W 5
#define BFMS_PHI 6
#define BFMS_THETA 7
#define BFMS_PSI 8
#define BFMS_P 9
#define BFMS_Q 10
#define BFMS_R 11
#define BFMS_OM_B 12
#define BFMS_OM_F 13
#define BFMS_OM_R 14
#define BFMS_OM_L 15
#define BFMS_SIZE 16
struct BoozFlightModel {
/* are we flying ? */
@@ -34,14 +16,31 @@ struct BoozFlightModel {
/* motors supply voltage in V */
VEC* mot_voltage;
/* state : see define above for fields */
/* private state : see defines in .c for fields */
VEC* state;
/* user products */
VEC* pos_ltp;
VEC* speed_ltp;
VEC* accel_ltp;
VEC* speed_body;
VEC* accel_body;
VEC* eulers;
VEC* ang_rate_body;
VEC* ang_accel_body;
MAT* dcm;
MAT* dcm_t;
VEC* quat;
VEC* omega;
VEC* omega_square;
/* constants used in derivative computation */
/* magnetic field in earth frame */
VEC* h_earth;
VEC* h_ltp;
/* gravitation in earth frame */
VEC* g_earth;
VEC* g_ltp;
/* propeller thrust factor */
double thrust_factor;
/* propeller torque factor */
@@ -61,37 +60,5 @@ extern struct BoozFlightModel bfm;
extern void booz_flight_model_init( void );
extern void booz_flight_model_run( double t, double* commands );
extern VEC* booz_get_forces_body_frame(VEC* F, MAT* dcm, VEC* omega_square, VEC* speed_body);
#define BoozFlighModelGetPos(_dest) { \
_dest->ve[AXIS_X] = bfm.state->ve[BFMS_X]; \
_dest->ve[AXIS_Y] = bfm.state->ve[BFMS_Y]; \
_dest->ve[AXIS_Z] = bfm.state->ve[BFMS_Z]; \
}
#define BoozFlighModelGetSpeed(_dest) { \
_dest->ve[AXIS_X] = bfm.state->ve[BFMS_U]; \
_dest->ve[AXIS_Y] = bfm.state->ve[BFMS_V]; \
_dest->ve[AXIS_Z] = bfm.state->ve[BFMS_W]; \
}
#define BoozFlighModelGetAngles(_dest) { \
_dest->ve[EULER_PHI] = bfm.state->ve[BFMS_PHI]; \
_dest->ve[EULER_THETA] = bfm.state->ve[BFMS_THETA]; \
_dest->ve[EULER_PSI] = bfm.state->ve[BFMS_PSI]; \
}
#define BoozFlighModelGetRate(_dest) { \
_dest->ve[AXIS_P] = bfm.state->ve[BFMS_P]; \
_dest->ve[AXIS_Q] = bfm.state->ve[BFMS_Q]; \
_dest->ve[AXIS_R] = bfm.state->ve[BFMS_R]; \
}
#define BoozFlighModelGetRPMS(_dest) { \
_dest->ve[SERVO_BACK] = bfm.state->ve[BFMS_OM_B]; \
_dest->ve[SERVO_FRONT] = bfm.state->ve[BFMS_OM_F]; \
_dest->ve[SERVO_RIGHT] = bfm.state->ve[BFMS_OM_R]; \
_dest->ve[SERVO_LEFT] = bfm.state->ve[BFMS_OM_L]; \
}
#endif /* BOOZ_FLIGHT_MODEL_H */
sw/simulator/booz_flight_model_params.h
+1 -1
View File
@@ -2,7 +2,7 @@
#define BOOZ_FLIGHT_MODEL_PARAMS_H
/* body drag coefficient */
#define C_d_body .0005
#define C_d_body .0075
/* propeller thrust aerodynamic coefficient */
#define C_t 0.297
/* propeller moment aerodynamic coefficient */
sw/simulator/booz_r250.c
+245
View File
@@ -0,0 +1,245 @@
/* r250.c the r250 uniform random number algorithm
Kirkpatrick, S., and E. Stoll, 1981; "A Very Fast
Shift-Register Sequence Random Number Generator",
Journal of Computational Physics, V.40
also:
see W.L. Maier, DDJ May 1991
*/
#include <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/booz_r250.h
+21
View File
@@ -0,0 +1,21 @@
/* r250.h prototypes for r250 random number generator,
Kirkpatrick, S., and E. Stoll, 1981; "A Very Fast
Shift-Register Sequence Random Number Generator",
Journal of Computational Physics, V.40
also:
see W.L. Maier, DDJ May 1991
*/
#ifndef _R250_H_
#define _R250_H_ 1.2
void r250_init(int seed);
unsigned int r250( void );
double dr250( void );
#endif
sw/simulator/booz_randlcg.c
+51
View File
@@ -0,0 +1,51 @@
/* rndlcg Linear Congruential Method, the "minimal standard generator"
Park & Miller, 1988, Comm of the ACM, 31(10), pp. 1192-1201
*/
#include <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/booz_randlcg.h
+19
View File
@@ -0,0 +1,19 @@
/* randlcg.h prototypes for the minimal standard random number generator,
Linear Congruential Method, the "minimal standard generator"
Park & Miller, 1988, Comm of the ACM, 31(10), pp. 1192-1201
rcsid: @(#)randlcg.h 1.1 15:48:09 11/21/94 EFC
*/
#ifndef _RANDLCG_H_
#define _RANDLCG_H_ 1.1
long set_seed(long);
long get_seed(long);
unsigned long int randlcg();
#endif
sw/simulator/booz_rc_sim.h
+1 -1
View File
@@ -22,7 +22,7 @@
else { \
ppm_pulses[RADIO_YAW] = 1500 + 0. * (2050-950); \
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.99 * (2050-1223); \
ppm_pulses[RADIO_MODE] = MODE_SWITCH_AUTO1; \
ppm_pulses[RADIO_MODE] = MODE_SWITCH_AUTO2; \
} \
ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
sw/simulator/booz_sensors_model.c
+17 -22
View File
@@ -1,5 +1,5 @@
#include "booz_sensors_model.h"
#include "booz_sensors_model_params.h"
#include BSM_PARAMS
#include <math.h>
@@ -10,13 +10,13 @@
struct BoozSensorsModel bsm;
extern void booz_sensors_model_accel_init(double time);
extern void booz_sensors_model_accel_run(double time, MAT* dcm);
extern void booz_sensors_model_accel_run(double time);
extern void booz_sensors_model_gyro_init(double time);
extern void booz_sensors_model_gyro_run(double time);
extern void booz_sensors_model_mag_init(double time);
extern void booz_sensors_model_mag_run(double time, MAT* dcm);
extern void booz_sensors_model_mag_run(double time);
extern void booz_sensors_model_rangemeter_init(double time);
extern void booz_sensors_model_rangemeter_run(double time);
@@ -25,41 +25,36 @@ extern void booz_sensors_model_baro_init(double time);
extern void booz_sensors_model_baro_run(double time);
extern void booz_sensors_model_gps_init(double time);
extern void booz_sensors_model_gps_run(double time, MAT* dcm_t);
extern void booz_sensors_model_gps_run(double time);
void booz_sensors_model_init(double time) {
VEC* tmp_eulers = v_get(AXIS_NB);
tmp_eulers->ve[EULER_PHI] = BSM_BODY_TO_IMU_PHI;
tmp_eulers->ve[EULER_THETA] = BSM_BODY_TO_IMU_THETA;
tmp_eulers->ve[EULER_PSI] = BSM_BODY_TO_IMU_PSI;
bsm.body_to_imu = m_get(AXIS_NB, AXIS_NB);
dcm_of_eulers (tmp_eulers, bsm.body_to_imu );
booz_sensors_model_accel_init(time);
booz_sensors_model_gyro_init(time);
booz_sensors_model_mag_init(time);
booz_sensors_model_rangemeter_init(time);
booz_sensors_model_baro_init(time);
booz_sensors_model_gps_init(time);
}
void booz_sensors_model_run(double time) {
/* extract eulers angles from state */
static VEC *eulers = VNULL;
eulers = v_resize(eulers, AXIS_NB);
eulers->ve[EULER_PHI] = bfm.state->ve[BFMS_PHI];
eulers->ve[EULER_THETA] = bfm.state->ve[BFMS_THETA];
eulers->ve[EULER_PSI] = bfm.state->ve[BFMS_PSI];
/* direct cosine matrix ( inertial to body )*/
static MAT *dcm = MNULL;
dcm = m_resize(dcm,AXIS_NB, AXIS_NB);
dcm = dcm_of_eulers(eulers, dcm);
/* transpose of dcm ( body to inertial ) */
static MAT *dcm_t = MNULL;
dcm_t = m_resize(dcm_t,AXIS_NB, AXIS_NB);
dcm_t = m_transp(dcm, dcm_t);
booz_sensors_model_accel_run(time, dcm);
booz_sensors_model_accel_run(time);
booz_sensors_model_gyro_run(time);
booz_sensors_model_mag_run(time, dcm);
booz_sensors_model_mag_run(time);
booz_sensors_model_rangemeter_run(time);
booz_sensors_model_baro_run(time);
booz_sensors_model_gps_run(time, dcm_t);
booz_sensors_model_gps_run(time);
}
sw/simulator/booz_sensors_model.h
+4 -1
View File
@@ -43,6 +43,9 @@ extern bool_t booz_sensors_model_gps_available();
struct BoozSensorsModel {
/* our imu alignement */
MAT* body_to_imu;
/* Accelerometer */
VEC* accel;
unsigned int accel_resolution;
@@ -69,7 +72,7 @@ struct BoozSensorsModel {
/* Magnetometer */
VEC* mag;
unsigned int mag_resolution;
MAT* mag_imu_to_sensor;
MAT* mag_sensitivity;
VEC* mag_neutral;
VEC* mag_noise_std_dev;
sw/simulator/booz_sensors_model_accel.c
+13 -43
View File
@@ -49,60 +49,30 @@ void booz_sensors_model_accel_init(double time) {
}
void booz_sensors_model_accel_run( double time, MAT* dcm ) {
void booz_sensors_model_accel_run( double time ) {
if (time < bsm.accel_next_update)
return;
/* */
static VEC* accel_ltp = VNULL;
accel_ltp = v_resize(accel_ltp, AXIS_NB);
/* substract gravity to acceleration in ltp frame */
accel_ltp = v_sub(bfm.accel_ltp, bfm.g_ltp, accel_ltp);
/* convert to body frame */
static VEC* accel_body = VNULL;
accel_body = v_resize(accel_body, AXIS_NB);
accel_body = v_zero(accel_body);
#if 1
/* compute sum of forces in body frame */
/* square of prop rotational speeds */
static VEC *omega_square = VNULL;
omega_square = v_resize(omega_square,SERVOS_NB);
BoozFlighModelGetRPMS(omega_square);
omega_square = v_star(omega_square, omega_square, omega_square);
/* extract body speed from state */
static VEC *speed_body = VNULL;
speed_body = v_resize(speed_body, AXIS_NB);
BoozFlighModelGetSpeed(speed_body);
accel_body = booz_get_forces_body_frame(accel_body , dcm, omega_square, speed_body);
/* divide by mass */
accel_body = sv_mlt(1./bfm.mass, accel_body, accel_body);
mv_mlt(bfm.dcm, accel_ltp, accel_body);
/* convert to imu frame */
static VEC* accel_imu = VNULL;
accel_imu = v_resize(accel_imu, AXIS_NB);
mv_mlt(bsm.body_to_imu, accel_body, accel_imu);
static VEC* g_inert = VNULL;
g_inert = v_resize(g_inert, AXIS_NB);
g_inert->ve[AXIS_X] = 0;
g_inert->ve[AXIS_Y] = 0;
g_inert->ve[AXIS_Z] = 9.81;
static VEC* g_body = VNULL;
g_body = v_resize(g_body, AXIS_NB);
g_body = mv_mlt(dcm, g_inert, g_body);
accel_body = v_sub(accel_body, g_body, accel_body);
#if 0
IvySendMsg("148 BOOZ_SIM_WIND %f %f %f",
accel_body->ve[AXIS_X],
accel_body->ve[AXIS_Y],
accel_body->ve[AXIS_Z]);
accel_body = mv_mlt(dcm, g_inert, accel_body);
accel_body = sv_mlt(-1., accel_body, accel_body);
#endif
// printf(" accel_body ~ %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]);
#endif
/* compute accel reading */
bsm.accel = mv_mlt(bsm.accel_sensitivity, accel_body, bsm.accel);
bsm.accel = v_add(bsm.accel, bsm.accel_neutral, bsm.accel);
mv_mlt(bsm.accel_sensitivity, accel_imu, bsm.accel);
v_add(bsm.accel, bsm.accel_neutral, bsm.accel);
/* compute accel error readings */
static VEC *accel_error = VNULL;
sw/simulator/booz_sensors_model_baro.c
+1 -1
View File
@@ -28,7 +28,7 @@ void booz_sensors_model_baro_run( double time ) {
if (time < 12.5)
bsm.baro = 840;
else {
double z = bfm.state->ve[BFMS_Z];
double z = bfm.pos_ltp->ve[AXIS_Z] + get_gaussian_noise()*BSM_BARO_NOISE_STD_DEV;
// double p = ( z / 0.084 ) + BSM_BARO_QNH;
// double baro_reading = p * BSM_BARO_SENSITIVITY;
double baro_reading = BSM_BARO_QNH + z * BSM_BARO_SENSITIVITY;
sw/simulator/booz_sensors_model_gps.c
+11 -17
View File
@@ -58,21 +58,16 @@ void booz_sensors_model_gps_init( double time ) {
}
/* We store our positions like the fdm and convert to UTM and funcky course, gspeed, climb */
void booz_sensors_model_gps_run( double time, MAT* dcm_t ) {
void booz_sensors_model_gps_run( double time) {
if (time < bsm.gps_next_update)
return;
/*
* simulate speed sensor
*/
/* extract body speed from state */
static VEC *speed_body = VNULL;
speed_body = v_resize(speed_body, AXIS_NB);
BoozFlighModelGetSpeed(speed_body);
static VEC *cur_speed_reading = VNULL;
cur_speed_reading = v_resize(cur_speed_reading, AXIS_NB);
/* convert to earth frame */
cur_speed_reading = mv_mlt(dcm_t, speed_body, cur_speed_reading);
v_copy(bfm.speed_ltp, cur_speed_reading);;
/* add a gaussian noise */
cur_speed_reading = v_add_gaussian_noise(cur_speed_reading, bsm.gps_speed_noise_std_dev,
cur_speed_reading);
@@ -90,12 +85,8 @@ void booz_sensors_model_gps_run( double time, MAT* dcm_t ) {
/*
* simulate position sensor
*/
static VEC *cur_pos_reading = VNULL;
cur_pos_reading = v_resize(cur_pos_reading, AXIS_NB);
/* extract pos from state */
BoozFlighModelGetPos(cur_pos_reading);
/* compute position error reading */
/* compute position error */
static VEC *pos_error = VNULL;
pos_error = v_resize(pos_error, AXIS_NB);
pos_error = v_zero(pos_error);
@@ -106,10 +97,13 @@ void booz_sensors_model_gps_run( double time, MAT* dcm_t ) {
v_update_random_walk(bsm.gps_pos_bias_random_walk_value,
bsm.gps_pos_bias_random_walk_std_dev, BSM_GPS_DT,
bsm.gps_pos_bias_random_walk_value);
/* add it */
pos_error = v_add(pos_error, bsm.gps_pos_bias_random_walk_value, pos_error);
/* add error reading */
cur_pos_reading = v_add(cur_pos_reading, pos_error, cur_pos_reading);
/* sum true pos and error reading */
static VEC *cur_pos_reading = VNULL;
cur_pos_reading = v_resize(cur_pos_reading, AXIS_NB);
v_add(bfm.pos_ltp, pos_error, cur_pos_reading);
/* store that for later and retrieve a previously stored data */
UpdateSensorLatency(time, cur_pos_reading, bsm.gps_pos_history, BSM_GPS_POS_LATENCY, bsm.gps_pos);
/* UTM conversion */
@@ -126,9 +120,9 @@ void booz_sensors_model_gps_run( double time, MAT* dcm_t ) {
#define LON0 1.
#define GROUND_ALT 180.
bsm.gps_pos_lla.lat = (bsm.gps_pos->ve[AXIS_Y] * 9e-6 + LAT0) * 1e7;
bsm.gps_pos_lla.lat = (bsm.gps_pos->ve[AXIS_X] * 9e-6 + LAT0) * 1e7;
bsm.gps_pos_lla.lat = rint(bsm.gps_pos_lla.lat);
bsm.gps_pos_lla.lon = (bsm.gps_pos->ve[AXIS_X] * 9e-6 + LON0) * 1e7;
bsm.gps_pos_lla.lon = (bsm.gps_pos->ve[AXIS_Y] * 9e-6 + LON0) * 1e7;
bsm.gps_pos_lla.lon = rint(bsm.gps_pos_lla.lon);
bsm.gps_pos_lla.alt = (bsm.gps_pos->ve[AXIS_Z] + GROUND_ALT)* 100.;
bsm.gps_pos_lla.alt = rint(bsm.gps_pos_lla.alt);
sw/simulator/booz_sensors_model_gyro.c
+6 -8
View File
@@ -60,15 +60,13 @@ void booz_sensors_model_gyro_run( double time ) {
if (time < bsm.gyro_next_update)
return;
/* extract rotational speed from flight model state */
static VEC *rate_body = VNULL;
rate_body = v_resize(rate_body, AXIS_NB);
rate_body->ve[AXIS_P] = bfm.state->ve[BFMS_P];
rate_body->ve[AXIS_Q] = bfm.state->ve[BFMS_Q];
rate_body->ve[AXIS_R] = bfm.state->ve[BFMS_R];
/* rotate to IMU frame */
/* convert to imu frame */
static VEC* rate_imu = VNULL;
rate_imu = v_resize(rate_imu, AXIS_NB);
mv_mlt(bsm.body_to_imu, bfm.ang_rate_body, rate_imu);
/* compute gyros readings */
bsm.gyro = mv_mlt(bsm.gyro_sensitivity, rate_body, bsm.gyro);
bsm.gyro = mv_mlt(bsm.gyro_sensitivity, rate_imu, bsm.gyro);
bsm.gyro = v_add(bsm.gyro, bsm.gyro_neutral, bsm.gyro);
/* compute gyro error readings */
sw/simulator/booz_sensors_model_mag.c
+24 -7
View File
@@ -3,6 +3,7 @@
#include BSM_PARAMS
#include "booz_sensors_model_utils.h"
#include "booz_flight_model.h"
#include "booz_flight_model_utils.h"
bool_t booz_sensors_model_mag_available() {
if (bsm.mag_available) {
@@ -19,7 +20,14 @@ void booz_sensors_model_mag_init( double time ) {
bsm.mag->ve[AXIS_X] = 0.;
bsm.mag->ve[AXIS_Y] = 0.;
bsm.mag->ve[AXIS_Z] = 0.;
bsm.mag_resolution = BSM_MAG_RESOLUTION;
// bsm.mag_resolution = BSM_MAG_RESOLUTION;
bsm.mag_imu_to_sensor = m_get(AXIS_NB, AXIS_NB);
VEC* tmp_eulers = v_get(AXIS_NB);
tmp_eulers->ve[EULER_PHI] = BSM_MAG_IMU_TO_SENSOR_PHI;
tmp_eulers->ve[EULER_THETA] = BSM_MAG_IMU_TO_SENSOR_THETA;
tmp_eulers->ve[EULER_PSI] = BSM_MAG_IMU_TO_SENSOR_PSI;
dcm_of_eulers (tmp_eulers, bsm.mag_imu_to_sensor );
bsm.mag_sensitivity = m_get(AXIS_NB, AXIS_NB);
m_zero(bsm.mag_sensitivity);
@@ -42,21 +50,30 @@ void booz_sensors_model_mag_init( double time ) {
}
void booz_sensors_model_mag_run( double time, MAT* dcm ) {
void booz_sensors_model_mag_run( double time ) {
if (time < bsm.mag_next_update)
return;
/* rotate h to body frame */
static VEC *h_body = VNULL;
h_body = v_resize(h_body, AXIS_NB);
h_body = mv_mlt(dcm, bfm.h_earth, h_body);
bsm.mag = mv_mlt(bsm.mag_sensitivity, h_body, bsm.mag);
bsm.mag = v_add(bsm.mag, bsm.mag_neutral, bsm.mag);
mv_mlt(bfm.dcm, bfm.h_ltp, h_body);
/* rotate to imu frame */
static VEC *h_imu = VNULL;
h_imu = v_resize(h_imu, AXIS_NB);
mv_mlt(bsm.body_to_imu, h_body, h_imu);
/* rotate to sensor frame */
static VEC *h_sensor = VNULL;
h_sensor = v_resize(h_sensor, AXIS_NB);
mv_mlt(bsm.mag_imu_to_sensor, h_imu, h_sensor);
mv_mlt(bsm.mag_sensitivity, h_sensor, bsm.mag);
v_add(bsm.mag, bsm.mag_neutral, bsm.mag);
/* compute mag error readings */
static VEC *mag_error = VNULL;
mag_error = v_resize(mag_error, AXIS_NB);
/* add hard iron now ? */
mag_error = v_zero(mag_error);
/* add a gaussian noise */
mag_error = v_add_gaussian_noise(mag_error, bsm.mag_noise_std_dev, mag_error);
@@ -66,7 +83,7 @@ void booz_sensors_model_mag_run( double time, MAT* dcm ) {
mag_error->ve[AXIS_Z] = mag_error->ve[AXIS_Z] * bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z];
/* add error */
bsm.mag = v_add(bsm.mag, mag_error, bsm.mag);
v_add(bsm.mag, mag_error, bsm.mag);
// printf("h body %f %f %f\n", h_body->ve[AXIS_X], h_body->ve[AXIS_Y], h_body->ve[AXIS_Z]);
// printf("mag %f %f %f\n", bsm.mag->ve[AXIS_X], bsm.mag->ve[AXIS_Y], bsm.mag->ve[AXIS_Z]);
sw/simulator/booz_sensors_model_params.h
-133
View File
@@ -1,133 +0,0 @@
/*
* $Id$
*
* Copyright (C) 2008 Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#ifndef BOOZ_SENSORS_MODEL_PARAMS_H
#define BOOZ_SENSORS_MODEL_PARAMS_H
/*
* Accelerometer
*/
#define BSM_ACCEL_RESOLUTION (1024 * 32)
/* ms-2 */
#define BSM_ACCEL_SENSITIVITY_XX 827.294562
#define BSM_ACCEL_SENSITIVITY_YY -823.507909
#define BSM_ACCEL_SENSITIVITY_ZZ -824.835155
#define BSM_ACCEL_NEUTRAL_X 17595
#define BSM_ACCEL_NEUTRAL_Y 16427
#define BSM_ACCEL_NEUTRAL_Z 16458
/* m2s-4 */
#define BSM_ACCEL_NOISE_STD_DEV_X 1e-1
#define BSM_ACCEL_NOISE_STD_DEV_Y 1e-1
#define BSM_ACCEL_NOISE_STD_DEV_Z 1e-1
/* ms-2 */
#define BSM_ACCEL_BIAS_X 1e-3
#define BSM_ACCEL_BIAS_Y 1e-3
#define BSM_ACCEL_BIAS_Z 1e-3
/* s */
#define BSM_ACCEL_DT (1./250.)
/*
* Gyrometer
*/
#define BSM_GYRO_RESOLUTION 65536
/* degres/s - nominal 300 */
//#define BSM_GYRO_SENSITIVITY_PP 65536. / (2.*RadOfDeg(-413.41848));
//#define BSM_GYRO_SENSITIVITY_QQ 65536. / (2.*RadOfDeg(-403.65564));
//#define BSM_GYRO_SENSITIVITY_RR 65536. / (2.*RadOfDeg( 395.01929));
#define BSM_GYRO_SENSITIVITY_PP (-4541.3261)
#define BSM_GYRO_SENSITIVITY_QQ (-4651.1628)
#define BSM_GYRO_SENSITIVITY_RR ( 4752.8517)
//#define BSM_GYRO_NEUTRAL_P 65536. * 0.6238556;
//#define BSM_GYRO_NEUTRAL_Q 65536. * 0.6242371;
//#define BSM_GYRO_NEUTRAL_R 65536. * 0.6035156;
#define BSM_GYRO_NEUTRAL_P 40852
#define BSM_GYRO_NEUTRAL_Q 40916
#define BSM_GYRO_NEUTRAL_R 39619
#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(.5)
#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(.5)
#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(.5)
#define BSM_GYRO_BIAS_INITIAL_P RadOfDeg( .0)
#define BSM_GYRO_BIAS_INITIAL_Q RadOfDeg(-0.5)
#define BSM_GYRO_BIAS_INITIAL_R RadOfDeg( .25)
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(5.e-1)
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(5.e-1)
#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(5.e-1)
#define BSM_GYRO_DT (1./250.)
/*
* Magnetometer
*/
#define BSM_MAG_DT (1./20.)
/*
* Range meter
*/
#define BSM_RANGEMETER_RESOLUTION (1024)
#define BSM_RANGEMETER_SENSITIVITY (1024. / 12.)
#define BSM_RANGEMETER_MAX_RANGE (6. * BSM_RANGEMETER_SENSITIVITY)
#define BSM_RANGEMETER_DT (1./20.)
/*
* Barometer
*/
#define BSM_BARO_QNH 101300
#define BSM_BARO_SENSITIVITY 4.
#define BSM_BARO_DT (1./10.)
/*
* GPS
*/
#define BSM_GPS_SPEED_NOISE_STD_DEV 1e-1
#define BSM_GPS_SPEED_LATENCY 0.25
#define BSM_GPS_POS_NOISE_STD_DEV 3e-1
#define BSM_GPS_POS_BIAS_INITIAL_X 1e-1
#define BSM_GPS_POS_BIAS_INITIAL_Y -1e-1
#define BSM_GPS_POS_BIAS_INITIAL_Z -5e-1
#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-1
#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-1
#define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-1
#define BSM_GPS_POS_LATENCY 0.25
#define BSM_GPS_POS_INITIAL_UTM_EAST 37728000
#define BSM_GPS_POS_INITIAL_UTM_NORTH 482464300
#define BSM_GPS_POS_INITIAL_UTM_ALT 15200
#define BSM_GPS_DT (1./4.)
#endif /* BOOZ_SENSORS_MODEL_PARAMS_H */
sw/simulator/booz_sensors_model_params_booz2_a1.h
+26 -15
View File
@@ -25,6 +25,13 @@
#ifndef BOOZ_SENSORS_MODEL_PARAMS_H
#define BOOZ_SENSORS_MODEL_PARAMS_H
#include "airframe.h"
#define BSM_BODY_TO_IMU_PHI RadOfDeg(4.)
#define BSM_BODY_TO_IMU_THETA RadOfDeg(3.)
#define BSM_BODY_TO_IMU_PSI RadOfDeg(0.)
/*
* Accelerometer
*/
@@ -38,13 +45,13 @@
#define BSM_ACCEL_NEUTRAL_Y 33738
#define BSM_ACCEL_NEUTRAL_Z 32441
/* m2s-4 */
#define BSM_ACCEL_NOISE_STD_DEV_X 0
#define BSM_ACCEL_NOISE_STD_DEV_Y 0
#define BSM_ACCEL_NOISE_STD_DEV_Z 0
//#define BSM_ACCEL_NOISE_STD_DEV_X 0
//#define BSM_ACCEL_NOISE_STD_DEV_Y 0
//#define BSM_ACCEL_NOISE_STD_DEV_Z 0
//#define BSM_ACCEL_NOISE_STD_DEV_X 1e-1
//#define BSM_ACCEL_NOISE_STD_DEV_Y 1e-1
//#define BSM_ACCEL_NOISE_STD_DEV_Z 1e-1
#define BSM_ACCEL_NOISE_STD_DEV_X 1.e-1
#define BSM_ACCEL_NOISE_STD_DEV_Y 1.e-1
#define BSM_ACCEL_NOISE_STD_DEV_Z 1.1e-1
/* ms-2 */
#define BSM_ACCEL_BIAS_X 0
@@ -88,11 +95,15 @@
/*
* Magnetometer
*/
#define BSM_MAG_RESOLUTION 65536
//#define BSM_MAG_RESOLUTION 65536
#define BSM_MAG_SENSITIVITY_XX (1./-3.4936416)
#define BSM_MAG_SENSITIVITY_YY (1./ 3.607713)
#define BSM_MAG_SENSITIVITY_ZZ (1./-4.90788848)
#define BSM_MAG_IMU_TO_SENSOR_PHI 0.
#define BSM_MAG_IMU_TO_SENSOR_THETA 0.
#define BSM_MAG_IMU_TO_SENSOR_PSI RadOfDeg(45.)
#define BSM_MAG_SENSITIVITY_XX (1.*(1<<11)/-4.94075530)
#define BSM_MAG_SENSITIVITY_YY (1.*(1<<11)/ 5.10207664)
#define BSM_MAG_SENSITIVITY_ZZ (1.*(1<<11)/-4.90788848)
#define BSM_MAG_NEUTRAL_X 2358
#define BSM_MAG_NEUTRAL_Y 2362
@@ -102,9 +113,9 @@
//#define BSM_MAG_NOISE_STD_DEV_Y 0
//#define BSM_MAG_NOISE_STD_DEV_Z 0
#define BSM_MAG_NOISE_STD_DEV_X 2e-2
#define BSM_MAG_NOISE_STD_DEV_Y 2e-2
#define BSM_MAG_NOISE_STD_DEV_Z 2e-2
#define BSM_MAG_NOISE_STD_DEV_X 2e-3
#define BSM_MAG_NOISE_STD_DEV_Y 2e-3
#define BSM_MAG_NOISE_STD_DEV_Z 2e-3
#define BSM_MAG_DT (1./100.)
@@ -126,7 +137,7 @@
#define BSM_BARO_QNH 900.
#define BSM_BARO_SENSITIVITY 17.066667
#define BSM_BARO_DT (1./100.)
#define BSM_BARO_NOISE_STD_DEV 5.e-2
/*
* GPS
@@ -134,7 +145,7 @@
#define BSM_GPS_SPEED_NOISE_STD_DEV 1e-1
#define BSM_GPS_SPEED_LATENCY 0.25
#define BSM_GPS_POS_NOISE_STD_DEV 3e-1
#define BSM_GPS_POS_NOISE_STD_DEV 3e-2
#define BSM_GPS_POS_BIAS_INITIAL_X 1e-1
#define BSM_GPS_POS_BIAS_INITIAL_Y -1e-1
#define BSM_GPS_POS_BIAS_INITIAL_Z -5e-1
sw/simulator/booz_sensors_model_rangemeter.c
+3 -3
View File
@@ -28,10 +28,10 @@ void booz_sensors_model_rangemeter_run( double time ) {
return;
/* compute dist from ground */
double dz = bfm.state->ve[BFMS_Z];
double dz = bfm.pos_ltp->ve[AXIS_Z];
if (dz > 0.) dz = 0.;
double dx = dz * tan(bfm.state->ve[BFMS_THETA]);
double dy = dz * tan(bfm.state->ve[BFMS_PHI]);
double dx = dz * tan(bfm.eulers->ve[EULER_THETA]);
double dy = dz * tan(bfm.eulers->ve[EULER_PHI]);
double dist = sqrt( dx*dx + dy*dy + dz*dz);
dist *= BSM_RANGEMETER_SENSITIVITY;
/* add gaussian noise */
sw/simulator/booz_sensors_model_utils.c
+40 -10
View File
@@ -1,6 +1,7 @@
#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) {
@@ -8,7 +9,7 @@ void UpdateSensorLatency(double time, VEC* cur_reading, GSList* history,
struct BoozDatedSensor* cur_read = g_new(struct BoozDatedSensor, 1);
cur_read->time = time;
cur_read->value = v_get(AXIS_NB);
CopyVect(cur_read->value, cur_reading);
v_copy(cur_reading, cur_read->value);
history = g_slist_prepend(history, cur_read);
/* remove old readings */
GSList* last = g_slist_last(history);
@@ -21,7 +22,7 @@ void UpdateSensorLatency(double time, VEC* cur_reading, GSList* history,
last = g_slist_last(history);
}
/* update sensor */
CopyVect(sensor_reading, ((struct BoozDatedSensor*)last->data)->value);
v_copy(((struct BoozDatedSensor*)last->data)->value, sensor_reading);
}
@@ -35,13 +36,42 @@ VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* out) {
VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out) {
static VEC *tmp = VNULL;
tmp = v_resize(tmp, AXIS_NB);
tmp = v_rand(tmp);
static VEC *one = VNULL;
one = v_resize(one, AXIS_NB);
one = v_ones(one);
tmp = v_mltadd(one, tmp, -2., tmp);
tmp = v_star(tmp, std_dev, tmp);
out = v_add(in, tmp, out);
tmp = v_resize(tmp, in->dim);
unsigned int i;
for (i=0; i<in->dim; i++)
tmp->ve[i] = get_gaussian_noise() * std_dev->ve[i];
v_add(in, tmp, out);
return out;
}
/*
http://www.taygeta.com/random/gaussian.html
*/
#include "booz_r250.h"
double get_gaussian_noise(void) {
static int nb_call = 0;
static double x1, x2, w;
if (nb_call==0) r250_init(0);
nb_call++;
if (nb_call%2) {
do {
x1 = 2.0 * dr250() - 1.0;
x2 = 2.0 * dr250() - 1.0;
w = x1 * x1 + x2 * x2;
} while ( w >= 1.0 );
w = sqrt( (-2.0 * log( w ) ) / w );
return x1 * w;
}
else
return x2 * w;
}
sw/simulator/booz_sensors_model_utils.h
+1 -7
View File
@@ -13,7 +13,7 @@ extern void UpdateSensorLatency(double time, VEC* cur_reading, GSList* history,
double latency, VEC* sensor_reading);
extern VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* out);
extern VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out);
extern double get_gaussian_noise();
#define RoundSensor(_sensor) { \
_sensor->ve[AXIS_X] = rint(_sensor->ve[AXIS_X]); \
@@ -30,12 +30,6 @@ extern VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out);
if ( _sensor->ve[AXIS_Z] > _max) _sensor->ve[AXIS_Z] = _max; \
}
#define CopyVect(_dest, _src) { \
_dest->ve[AXIS_X] = _src->ve[AXIS_X]; \
_dest->ve[AXIS_Y] = _src->ve[AXIS_Y]; \
_dest->ve[AXIS_Z] = _src->ve[AXIS_Z]; \
}
#endif /* BOOZ_SENSORS_MODEL_UTILS_H */
sw/simulator/booz_wind_model.c
+4
View File
@@ -22,6 +22,10 @@ void booz_wind_model_init( void ) {
bwm.velocity = v_get(AXIS_NB);
v_zero(bwm.velocity);
bwm.velocity->ve[AXIS_X] = INIT_WIND_X;
bwm.velocity->ve[AXIS_Y] = INIT_WIND_Y;
bwm.velocity->ve[AXIS_Z] = INIT_WIND_Z;
bwm.state = v_get(BWM_STATE_SIZE);
v_zero(bwm.state);