Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 13:24:03 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-10-30 13:35:25 +00:00
parent ff102470d5
commit 4d9a3852a0
9 changed files with 212 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+37 -38
View File
@@ -417,49 +417,49 @@
<field name="link_imu_nb_err" type="uint32"/>
<field name="imu_status" type="uint8" values="NO_LINK|UNINIT|RUNNING|CRASHED"/>
<field name="rc_status" type="uint8" values="OK|LOST|REALLY_LOST"/>
<field name="ap_mode" type="uint8" values="FAILSAFE|RATE|ATTITUDE|NAV"/>
<field name="ap_mode" type="uint8" values="FAILSAFE|KILL|RATE|ATTITUDE|NAV"/>
<field name="vsupply" type="uint8" unit="decivolt"/>
<field name="cpu_time" type="uint16" unit="s"></field>
<field name="blmc_nb_err" type="uint8"/>
</message>
<message name="BOOZ_DEBUG" ID="232">
<field name="m_phi" type="float"/>
<field name="m_theta" type="float"/>
<field name="m_phi" type="float" unit="rad"/>
<field name="m_theta" type="float" unit="rad"/>
</message>
<message name="BOOZ_RATE_LOOP" ID="233">
<field name="est_p" type="float"/>
<field name="sp_p" type="float"/>
<field name="est_q" type="float"/>
<field name="sp_q" type="float"/>
<field name="est_r" type="float"/>
<field name="sp_r" type="float"/>
<field name="est_p" type="float" unit="rad/s"/>
<field name="sp_p" type="float" unit="rad/s"/>
<field name="est_q" type="float" unit="rad/s"/>
<field name="sp_q" type="float" unit="rad/s"/>
<field name="est_r" type="float" unit="rad/s"/>
<field name="sp_r" type="float" unit="rad/s"/>
</message>
<message name="BOOZ_ATT_LOOP" ID="234">
<field name="est_phi" type="float"/>
<field name="sp_phi" type="float"/>
<field name="est_theta" type="float"/>
<field name="sp_theta" type="float"/>
<field name="est_phi" type="float" unit="rad"/>
<field name="sp_phi" type="float" unit="rad"/>
<field name="est_theta" type="float" unit="rad"/>
<field name="sp_theta" type="float" unit="rad"/>
</message>
<message name="BOOZ_VERT_LOOP" ID="235">
<field name="sp_z" type="float"/>
<field name="est_vz" type="float"/>
<field name="est_z" type="float"/>
<field name="power_command" type="float"/>
<field name="sp_z" type="float" unit="m"/>
<field name="est_vz" type="float" unit="m/s"/>
<field name="est_z" type="float" unit="m"/>
<field name="power_command" type="float" unit="%"/>
</message>
<message name="BOOZ_HOV_LOOP" ID="236">
<field name="sp_x" type="float"/>
<field name="sp_y" type="float"/>
<field name="est_vx" type="float"/>
<field name="est_x" type="float"/>
<field name="est_vy" type="float"/>
<field name="est_y" type="float"/>
<field name="phi_command" type="float"/>
<field name="theta_command" type="float"/>
<field name="sp_x" type="float" unit="m"/>
<field name="sp_y" type="float" unit="m"/>
<field name="est_vx" type="float" unit="m/s"/>
<field name="est_x" type="float" unit="m"/>
<field name="est_vy" type="float" unit="m/s"/>
<field name="est_y" type="float" unit="m"/>
<field name="phi_command" type="float" unit="rad"/>
<field name="theta_command" type="float" unit="rad"/>
</message>
<message name="BOOZ_CMDS" ID="237">
@@ -469,11 +469,10 @@
<field name="right" type="uint8"/>
</message>
<message name="BOOZ_UF_RATES" ID="238">
<field name="p" type="float"/>
<field name="q" type="float"/>
<field name="r" type="float"/>
<field name="p" type="float" unit="rad/s"/>
<field name="q" type="float" unit="rad/s"/>
<field name="r" type="float" unit="rad/s"/>
</message>
<message name="BOOZ_SIM_RPMS" ID="239">
@@ -484,18 +483,18 @@
</message>
<message name="BOOZ_SIM_SPEED_POS" ID="240">
<field name="u" type="float"/>
<field name="v" type="float"/>
<field name="w" type="float"/>
<field name="x" type="float"/>
<field name="y" type="float"/>
<field name="z" type="float"/>
<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"/>
</message>
<message name="BOOZ_SIM_RATE_ATTITUDE" ID="241">
<field name="p" type="float" unit="degres"/>
<field name="q" type="float" unit="degres"/>
<field name="r" type="float" unit="degres"/>
<field name="p" type="float" unit="degres/s"/>
<field name="q" type="float" unit="degres/s"/>
<field name="r" type="float" unit="degres/s"/>
<field name="phi" type="float" unit="degres"/>
<field name="theta" type="float" unit="degres"/>
<field name="psi" type="float" unit="degres"/>
sw/airborne/6dof.h
+4
View File
@@ -10,6 +10,10 @@
#define AXIS_Q 1
#define AXIS_R 2
#define AXIS_U 0
#define AXIS_V 1
#define AXIS_W 2
#define EULER_PHI 0
#define EULER_THETA 1
#define EULER_PSI 2
sw/airborne/booz_estimator.c
+26 -4
View File
@@ -22,6 +22,10 @@ float booz_estimator_x;
float booz_estimator_y;
float booz_estimator_z;
float booz_estimator_vx;
float booz_estimator_vy;
float booz_estimator_vz;
float booz_estimator_u;
float booz_estimator_v;
float booz_estimator_w;
@@ -40,11 +44,16 @@ void booz_estimator_init( void ) {
booz_estimator_phi = 0.;
booz_estimator_theta = 0.;
booz_estimator_psi = 0.;
#ifndef DISABLE_NAV
booz_estimator_x = 0.;
booz_estimator_y = 0.;
booz_estimator_z = 0.;
booz_estimator_vx = 0.;
booz_estimator_vy = 0.;
booz_estimator_vz = 0.;
booz_estimator_u = 0.;
booz_estimator_v = 0.;
booz_estimator_w = 0.;
@@ -91,15 +100,28 @@ void booz_estimator_compute_dcm( void ) {
}
void booz_estimator_set_speed_and_pos(float _u, float _v, float _w, float _x, float _y, float _z) {
/* assume dcm is already computed */
void booz_estimator_set_speed_and_pos(float _vx, float _vy, float _vz, float _x, float _y, float _z) {
booz_estimator_u = _u;
booz_estimator_v = _v;
booz_estimator_w = _w;
booz_estimator_vx = _vx;
booz_estimator_vy = _vy;
booz_estimator_vz = _vz;
booz_estimator_x = _x;
booz_estimator_y = _y;
booz_estimator_z = _z;
booz_estimator_u = booz_estimator_dcm[AXIS_U][AXIS_X] * booz_estimator_vx +
booz_estimator_dcm[AXIS_U][AXIS_Y] * booz_estimator_vy +
booz_estimator_dcm[AXIS_U][AXIS_Z] * booz_estimator_vz ;
booz_estimator_v = booz_estimator_dcm[AXIS_V][AXIS_X] * booz_estimator_vx +
booz_estimator_dcm[AXIS_V][AXIS_Y] * booz_estimator_vy +
booz_estimator_dcm[AXIS_V][AXIS_Z] * booz_estimator_vz ;
booz_estimator_w = booz_estimator_dcm[AXIS_W][AXIS_X] * booz_estimator_vx +
booz_estimator_dcm[AXIS_W][AXIS_Y] * booz_estimator_vy +
booz_estimator_dcm[AXIS_W][AXIS_Z] * booz_estimator_vz ;
}
#endif /* DISABLE_NAV */
sw/airborne/booz_estimator.h
+7 -2
View File
@@ -25,6 +25,11 @@ extern float booz_estimator_x;
extern float booz_estimator_y;
extern float booz_estimator_z;
/* speed in earth frame : not yet available - sim only */
extern float booz_estimator_vx;
extern float booz_estimator_vy;
extern float booz_estimator_vz;
/* speed in body frame : not yet available - sim only */
extern float booz_estimator_u;
extern float booz_estimator_v;
@@ -38,8 +43,8 @@ extern void booz_estimator_read_inter_mcu_state( void );
#ifndef DISABLE_NAV
extern void booz_estimator_compute_dcm( void );
extern void booz_estimator_set_speed_and_pos(float _u, float _v, float _w, float _x, float _y, float _z);
extern void booz_estimator_set_speed_and_pos(float _vx, float _vy, float _vz, float _x, float _y, float _z);
extern void booz_estimator_set_psi( float _psi);
#endif
#endif /* DISABLE_NAV */
#endif /* BOOZ_ESTIMATOR_H */
sw/airborne/booz_nav.c
+1 -1
View File
@@ -123,7 +123,7 @@ static void booz_nav_vertical_loop_run(void) {
booz_nav_power_command = adjusted_hover_power +
/* BOOZ_NAV_VERT_HOVER_COMMAND + */
booz_nav_vertical_pgain * vertical_err +
booz_nav_vertical_dgain * booz_estimator_w;
booz_nav_vertical_dgain * booz_estimator_vz;
Bound(booz_nav_power_command, 0., 1.);
}
sw/simulator/booz_flight_model_params.h
+3 -6
View File
@@ -1,11 +1,11 @@
#ifndef BOOZ_FLIGHT_MODEL_PARAMS_H
#define BOOZ_FLIGHT_MODEL_PARAMS_H
/* drag coefficient of the body */
/* body drag coefficient */
#define C_d_body .005
/* thrust aerodynamic coefficient */
/* propeller thrust aerodynamic coefficient */
#define C_t 0.297
/* moment aerodynamic coefficient */
/* propeller moment aerodynamic coefficient */
#define C_q 0.0276
/* propeller radius in m */
#define PROP_RADIUS 0.125
@@ -25,9 +25,6 @@
#define Iz .0073
/* lenght between centers of vehicle and prop in m */
#define L 0.25
/* height between cg and prop plane in m */
#define H 0.04
/* motors parameters
sw/simulator/booz_sensors_model.c
+118 -51
View File
@@ -2,21 +2,23 @@
#include <math.h>
#include "std.h"
#include "booz_flight_model.h"
#include "booz_flight_model_utils.h"
struct BoozSensorsModel bsm;
static void booz_sensors_model_accel_init(void);
static void booz_sensors_model_accel_run(void);
static void booz_sensors_model_accel_run(MAT* dcm);
static void booz_sensors_model_gyro_init(void);
static void booz_sensors_model_gyro_run(void);
static void booz_sensors_model_gyro_run(double dt);
static void booz_sensors_model_gps_init(void);
static void booz_sensors_model_gps_run(void);
static void booz_sensors_model_gps_run(double dt, MAT* dcm_t);
static VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out);
static VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* out);
void booz_sensors_model_init(void) {
booz_sensors_model_accel_init();
@@ -24,10 +26,26 @@ void booz_sensors_model_init(void) {
booz_sensors_model_gps_init();
}
void booz_sensors_model_run(void) {
booz_sensors_model_accel_run();
booz_sensors_model_gyro_run();
booz_sensors_model_gps_run();
void booz_sensors_model_run(double dt) {
/* 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(dcm);
booz_sensors_model_gyro_run(dt);
booz_sensors_model_gps_run(dt, dcm_t);
}
static void booz_sensors_model_accel_init(void) {
@@ -69,24 +87,34 @@ static void booz_sensors_model_gyro_init(void) {
bsm.gyro_sensitivity = m_get(AXIS_NB, AXIS_NB);
m_zero(bsm.gyro_sensitivity);
bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] = 1./-0.0002202;
bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] = 1./-0.0002150;
bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] = 1./ 0.0002104;
bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] = 32768. / RadOfDeg(-413.41848); /* degres/s - nominal 300 */
bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] = 32768. / RadOfDeg(-403.65564); /* degres/s - nominal 300 */
bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] = 32768. / RadOfDeg( 395.01929); /* degres/s - nominal 300 */
bsm.gyro_neutral = v_get(AXIS_NB);
bsm.gyro_neutral->ve[AXIS_P] = 40885;
bsm.gyro_neutral->ve[AXIS_Q] = 40910;
bsm.gyro_neutral->ve[AXIS_R] = 39552;
bsm.gyro_neutral->ve[AXIS_P] = 65536. * 0.6238556; /* ratio of full scale - nominal 0.5 */
bsm.gyro_neutral->ve[AXIS_Q] = 65536. * 0.6242371; /* ratio of full scale - nominal 0.5 */
bsm.gyro_neutral->ve[AXIS_R] = 65536. * 0.6035156; /* ratio of full scale - nominal 0.5 */
bsm.gyro_noise_std_dev = v_get(AXIS_NB);
bsm.gyro_noise_std_dev->ve[AXIS_P] = 5e-2 * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
bsm.gyro_noise_std_dev->ve[AXIS_Q] = 5e-2 * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
bsm.gyro_noise_std_dev->ve[AXIS_R] = 5e-2 * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
bsm.gyro_noise_std_dev->ve[AXIS_P] = RadOfDeg(1.) * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
bsm.gyro_noise_std_dev->ve[AXIS_Q] = RadOfDeg(1.) * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
bsm.gyro_noise_std_dev->ve[AXIS_R] = RadOfDeg(1.) * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
bsm.gyro_bias = v_get(AXIS_NB);
bsm.gyro_bias->ve[AXIS_P] = 2e-2 * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
bsm.gyro_bias->ve[AXIS_Q] = -2e-2 * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
bsm.gyro_bias->ve[AXIS_R] = -1e-2 * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
bsm.gyro_bias_initial = v_get(AXIS_NB);
bsm.gyro_bias_initial->ve[AXIS_P] = RadOfDeg( 1.0) * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
bsm.gyro_bias_initial->ve[AXIS_Q] = RadOfDeg(-1.0) * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
bsm.gyro_bias_initial->ve[AXIS_R] = RadOfDeg( 0.7) * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
bsm.gyro_bias_random_walk_std_dev = v_get(AXIS_NB);
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_P] = RadOfDeg(5.e-1) * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_Q] = RadOfDeg(5.e-1) * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
bsm.gyro_bias_random_walk_std_dev->ve[AXIS_R] = RadOfDeg(5.e-1) * bsm.gyro_sensitivity->me[AXIS_R][AXIS_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];
}
static void booz_sensors_model_gps_init(void) {
@@ -94,10 +122,29 @@ static void booz_sensors_model_gps_init(void) {
v_zero(bsm.speed_sensor);
bsm.pos_sensor = v_get(AXIS_NB);
v_zero(bsm.pos_sensor);
bsm.pos_noise_std_dev = v_get(AXIS_NB);
bsm.pos_noise_std_dev->ve[AXIS_X] = 1e-1;
bsm.pos_noise_std_dev->ve[AXIS_Y] = 1e-1;
bsm.pos_noise_std_dev->ve[AXIS_Z] = 1e-1;
bsm.pos_bias_initial = v_get(AXIS_NB);
bsm.pos_bias_initial->ve[AXIS_X] = 1e-1;
bsm.pos_bias_initial->ve[AXIS_Y] = 1e-1;
bsm.pos_bias_initial->ve[AXIS_Z] = 1e-1;
bsm.pos_bias_random_walk_std_dev = v_get(AXIS_NB);
bsm.pos_bias_random_walk_std_dev->ve[AXIS_X] = 1e-1;
bsm.pos_bias_random_walk_std_dev->ve[AXIS_Y] = 1e-1;
bsm.pos_bias_random_walk_std_dev->ve[AXIS_Z] = 1e-1;
bsm.pos_bias_random_walk_value = v_get(AXIS_NB);
bsm.pos_bias_random_walk_value->ve[AXIS_X] = bsm.pos_bias_initial->ve[AXIS_X];
bsm.pos_bias_random_walk_value->ve[AXIS_Y] = bsm.pos_bias_initial->ve[AXIS_Y];
bsm.pos_bias_random_walk_value->ve[AXIS_Z] = bsm.pos_bias_initial->ve[AXIS_Z];
}
static void booz_sensors_model_accel_run(void) {
static void booz_sensors_model_accel_run( MAT* dcm ) {
/* */
static VEC* accel_body = VNULL;
@@ -105,21 +152,11 @@ static void booz_sensors_model_accel_run(void) {
accel_body = v_zero(accel_body);
#if 1
/* get g in body frame */
/* 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);
static VEC *g_body = VNULL;
g_body = v_resize(g_body, AXIS_NB);
g_body = mv_mlt(dcm, bfm.g_earth, g_body);
/* non inertial forces */
/* add non inertial forces */
/* extract body speed from state */
static VEC *speed_body = VNULL;
speed_body = v_resize(speed_body, AXIS_NB);
@@ -147,7 +184,6 @@ static void booz_sensors_model_accel_run(void) {
printf(" accel_body ~ %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]);
#endif
// printf("sim accel %f %f %f\n",accel_body->ve[AXIS_X] ,accel_body->ve[AXIS_Y] ,accel_body->ve[AXIS_Z]);
/* 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);
@@ -162,15 +198,15 @@ static void booz_sensors_model_accel_run(void) {
/* add per accel error reading */
bsm.accel = v_add(bsm.accel, accel_error, bsm.accel);
/* round signal to account for adc discretisation */
bsm.accel->ve[AXIS_X] = round( bsm.accel->ve[AXIS_X]);
bsm.accel->ve[AXIS_Y] = round( bsm.accel->ve[AXIS_Y]);
bsm.accel->ve[AXIS_Z] = round( bsm.accel->ve[AXIS_Z]);
bsm.accel->ve[AXIS_X] = rint( bsm.accel->ve[AXIS_X]);
bsm.accel->ve[AXIS_Y] = rint( bsm.accel->ve[AXIS_Y]);
bsm.accel->ve[AXIS_Z] = rint( bsm.accel->ve[AXIS_Z]);
// printf("sim adc %f %f %f\n",bsm.accel->ve[AXIS_X] ,bsm.accel->ve[AXIS_Y] ,bsm.accel->ve[AXIS_Z]);
}
static void booz_sensors_model_gyro_run(void) {
static void booz_sensors_model_gyro_run( double dt) {
/* extract rotational speed from flight model state */
static VEC *rate_body = VNULL;
rate_body = v_resize(rate_body, AXIS_NB);
@@ -188,32 +224,63 @@ static void booz_sensors_model_gyro_run(void) {
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);
/* add a constant bias */
gyro_error = v_add(bsm.gyro_bias, gyro_error, gyro_error);
/* add a random walk */
/* 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, dt,
bsm.gyro_bias_random_walk_value);
gyro_error = v_add(gyro_error, bsm.gyro_bias_random_walk_value, gyro_error);
/* add per gyro error reading */
bsm.gyro = v_add(bsm.gyro, gyro_error, bsm.gyro);
/* round signal to account for adc discretisation */
bsm.gyro->ve[AXIS_P] = round( bsm.gyro->ve[AXIS_P]);
bsm.gyro->ve[AXIS_Q] = round( bsm.gyro->ve[AXIS_Q]);
bsm.gyro->ve[AXIS_R] = round( bsm.gyro->ve[AXIS_R]);
bsm.gyro->ve[AXIS_P] = rint( bsm.gyro->ve[AXIS_P]);
bsm.gyro->ve[AXIS_Q] = rint( bsm.gyro->ve[AXIS_Q]);
bsm.gyro->ve[AXIS_R] = rint( bsm.gyro->ve[AXIS_R]);
}
static void booz_sensors_model_gps_run(void) {
/* very wrong change me */
bsm.speed_sensor->ve[AXIS_X] = bfm.state->ve[BFMS_U];
bsm.speed_sensor->ve[AXIS_Y] = bfm.state->ve[BFMS_V];
bsm.speed_sensor->ve[AXIS_Z] = bfm.state->ve[BFMS_W];
static void booz_sensors_model_gps_run( double dt, MAT* dcm_t ) {
/* extract body speed from state */
static VEC *speed_body = VNULL;
speed_body = v_resize(speed_body, AXIS_NB);
speed_body->ve[AXIS_U] = bfm.state->ve[BFMS_U];
speed_body->ve[AXIS_V] = bfm.state->ve[BFMS_V];
speed_body->ve[AXIS_W] = bfm.state->ve[BFMS_W];
/* convert to earth frame */
bsm.speed_sensor = mv_mlt(dcm_t, speed_body, bsm.speed_sensor);
/* simulate position sensor */
bsm.pos_sensor->ve[AXIS_X] = bfm.state->ve[BFMS_X];
bsm.pos_sensor->ve[AXIS_Y] = bfm.state->ve[BFMS_Y];
bsm.pos_sensor->ve[AXIS_Z] = bfm.state->ve[BFMS_Z];
/* compute gyro error reading */
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.pos_noise_std_dev, pos_error);
/* update random walk bias */
bsm.pos_bias_random_walk_value =
v_update_random_walk(bsm.pos_bias_random_walk_value,
bsm.pos_bias_random_walk_std_dev, dt,
bsm.pos_bias_random_walk_value);
pos_error = v_add(pos_error, bsm.pos_bias_random_walk_value, pos_error);
/* add error reading */
bsm.pos_sensor = v_add(bsm.pos_sensor, pos_error, bsm.pos_sensor);
}
static VEC* v_update_random_walk(VEC* in, VEC* std_dev, double dt, VEC* out) {
static VEC *tmp = VNULL;
tmp = v_resize(tmp, AXIS_NB);
tmp = sv_mlt(dt, std_dev, tmp);
out = v_add_gaussian_noise(in, tmp, out);
return out;
}
static VEC* v_add_gaussian_noise(VEC* in, VEC* std_dev, VEC* out) {
static VEC *tmp = VNULL;
tmp = v_resize(tmp, AXIS_NB);
sw/simulator/booz_sensors_model.h
+8 -2
View File
@@ -5,7 +5,7 @@
#include <matrix.h>
extern void booz_sensors_model_init(void);
extern void booz_sensors_model_run(void);
extern void booz_sensors_model_run( double dt);
struct BoozSensorsModel {
@@ -19,11 +19,17 @@ struct BoozSensorsModel {
MAT* gyro_sensitivity;
VEC* gyro_neutral;
VEC* gyro_noise_std_dev;
VEC* gyro_bias;
VEC* gyro_bias_initial;
VEC* gyro_bias_random_walk_std_dev;
VEC* gyro_bias_random_walk_value;
/* imaginary sensors - gps maybe */
VEC* speed_sensor;
VEC* pos_sensor;
VEC* pos_noise_std_dev;
VEC* pos_bias_initial;
VEC* pos_bias_random_walk_std_dev;
VEC* pos_bias_random_walk_value;
};
sw/simulator/main_booz_sim.c
+8 -8
View File
@@ -14,10 +14,10 @@
//char* fg_host = "127.0.0.1";
//char* fg_host = "10.31.4.107";
char* fg_host = "192.168.1.191";
char* fg_host = "10.31.4.107";
//char* fg_host = "192.168.1.191";
unsigned int fg_port = 5501;
char* joystick_dev = "/dev/input/js1";
char* joystick_dev = "/dev/input/js0";
/* 250Hz <-> 4ms */
#define TIMEOUT_PERIOD 4
@@ -62,7 +62,7 @@ static gboolean booz_sim_periodic(gpointer data) {
/* it sucks, I know */
booz_flight_model_run(DT, booz_sim_actuators_values);
booz_sensors_model_run();
booz_sensors_model_run(DT);
sim_time += DT;
/* call the filter periodic task to read sensors */
@@ -78,6 +78,10 @@ static gboolean booz_sim_periodic(gpointer data) {
/* process the BoozLinkMcuEvent */
/* this will update the controller estimator */
booz_controller_main_event_task();
/* cheat in simulation : psi not available from filter yet */
booz_estimator_set_psi(bfm.state->ve[BFMS_PSI]);
/* in simulation compute dcm as a helper for for nav */
booz_estimator_compute_dcm();
/* in simulation feed speed and pos estimations ( with a pos sensor :( ) */
booz_estimator_set_speed_and_pos(bsm.speed_sensor->ve[AXIS_X],
bsm.speed_sensor->ve[AXIS_Y],
@@ -85,10 +89,6 @@ static gboolean booz_sim_periodic(gpointer data) {
bsm.pos_sensor->ve[AXIS_X],
bsm.pos_sensor->ve[AXIS_Y],
bsm.pos_sensor->ve[AXIS_Z] );
/* cheat in simulation : psi not available from filter yet */
booz_estimator_set_psi(bfm.state->ve[BFMS_PSI]);
/* in simulation compute dcm as a helper for for nav */
booz_estimator_compute_dcm();
/* post a radio control event */