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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-10-29 11:21:07 +00:00
parent 48692f3c26
commit b516fe1bea
13 changed files with 291 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+28 -11
View File
@@ -424,10 +424,8 @@
</message>
<message name="BOOZ_DEBUG" ID="232">
<field name="sp_p" type="float"/>
<field name="sp_q" type="float"/>
<field name="sp_r" type="float"/>
<field name="sp_t" type="float"/>
<field name="m_phi" type="float"/>
<field name="m_theta" type="float"/>
</message>
<message name="BOOZ_RATE_LOOP" ID="233">
@@ -446,13 +444,6 @@
<field name="sp_theta" type="float"/>
</message>
<message name="BOOZ_RPMS" ID="235">
<field name="front" type="float"/>
<field name="back" type="float"/>
<field name="left" type="float"/>
<field name="right" type="float"/>
</message>
<message name="BOOZ_CMDS" ID="236">
<field name="front" type="uint8"/>
<field name="back" type="uint8"/>
@@ -467,7 +458,33 @@
<field name="r" type="float"/>
</message>
<message name="BOOZ_SIM_RPMS" ID="235">
<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"/>
</message>
<message name="BOOZ_SIM_POS_SPEED" ID="238">
<field name="x" type="float"/>
<field name="y" type="float"/>
<field name="z" type="float"/>
<field name="u" type="float"/>
<field name="v" type="float"/>
<field name="w" type="float"/>
</message>
<message name="BOOZ_SIM_ATTITUDE" ID="239">
<field name="phi" type="float" unit="degres"/>
<field name="theta" type="float" unit="degres"/>
<field name="psi" type="float" unit="degres"/>
</message>
<message name="BOOZ_SIM_BODY_RATE" ID="240">
<field name="p" type="float" unit="degres"/>
<field name="q" type="float" unit="degres"/>
<field name="r" type="float" unit="degres"/>
</message>
<message name="ENOSE_STATUS" ID="250">
<field name="val1" type="uint16"/>
conf/telemetry/booz.xml
+6 -1
View File
@@ -7,7 +7,7 @@
<mode name="default">
<message name="BOOZ_STATUS" period="1."/>
<!-- <message name="BOOZ_FD" period="0.05"/> -->
<message name="BOOZ_DEBUG" period="0.25"/>
<!-- <message name="BOOZ_DEBUG" period="0.25"/> -->
<message name="ACTUATORS" period="0.5"/>
<message name="BOOZ_RATE_LOOP" period="0.05"/>
<message name="BOOZ_ATT_LOOP" period="0.05"/>
@@ -24,9 +24,14 @@
<process name="Filter">
<mode name="default">
<message name="BOOZ_DEBUG" period=".1"/>
<message name="AHRS_STATE" period=".1"/>
<message name="AHRS_COV" period=".1"/>
<message name="IMU_GYRO" period=".017"/>
<message name="IMU_GYRO_LP" period=".017"/>
<message name="IMU_GYRO_RAW" period=".25"/>
<message name="IMU_ACCEL" period=".25"/>
<message name="IMU_ACCEL_RAW" period=".25"/>
</mode>
<mode name="raw_sensors">
<message name="IMU_GYRO_RAW" period=".017"/>
sw/airborne/arm7/imu_v3_hw.h
+7
View File
@@ -1,5 +1,12 @@
#ifndef IMU_V3_HW_H
#define IMU_V3_HW_H
#include "imu_v3.h"
#define ImuReadAdcs() { \
imu_accel_raw[AXIS_X]= buf_ax.sum; \
imu_accel_raw[AXIS_Y]= buf_ay.sum; \
imu_accel_raw[AXIS_Z]= buf_az.sum; \
}
#endif /* IMU_V3_HW_H */
sw/airborne/booz_control.c
+8
View File
@@ -133,11 +133,19 @@ void booz_control_attitude_run(void) {
}
void booz_control_nav_compute_setpoints(void) {
}
void booz_control_nav_run(void) {
#if 0
booz_control_commands[COMMAND_P] = 0;
booz_control_commands[COMMAND_Q] = 0;
booz_control_commands[COMMAND_R] = 0;
booz_control_commands[COMMAND_THROTTLE] = 0;
#else
booz_control_rate_run();
#endif
}
sw/airborne/booz_filter_telemetry.h
+10
View File
@@ -63,6 +63,16 @@
&mtt_P_theta[0][0], &mtt_P_theta[0][1], \
&mtt_P_theta[1][1]);
#define PERIODIC_SEND_BOOZ_DEBUG() { \
float m_phi = atan2(imu_accel[AXIS_Y], imu_accel[AXIS_Z]); \
const float g2 = \
imu_accel[AXIS_X]*imu_accel[AXIS_X] + \
imu_accel[AXIS_Y]*imu_accel[AXIS_Y] + \
imu_accel[AXIS_Z]*imu_accel[AXIS_Z]; \
float m_theta = -asin( imu_accel[AXIS_X] / sqrt( g2 ) ); \
DOWNLINK_SEND_BOOZ_DEBUG(&m_phi, &m_theta); \
}
#define PERIODIC_SEND_DL_VALUE() PeriodicSendDlValue()
extern uint8_t telemetry_mode_Filter;
sw/airborne/booz_telemetry.h
+3 -9
View File
@@ -36,19 +36,13 @@
&booz_estimator_theta, \
&booz_estimator_psi);
#define PERIODIC_SEND_BOOZ_DEBUG() \
DOWNLINK_SEND_BOOZ_DEBUG(&booz_control_p_sp, \
&booz_control_q_sp, \
&booz_control_r_sp, \
&booz_control_power_sp);
#define PERIODIC_SEND_ACTUATORS() \
DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators);
#define PERIODIC_SEND_BOOZ_RATE_LOOP() \
DOWNLINK_SEND_BOOZ_RATE_LOOP(&booz_estimator_p, &booz_control_p_sp, \
&booz_estimator_q, &booz_control_q_sp, \
&booz_estimator_r, &booz_control_r_sp );
DOWNLINK_SEND_BOOZ_RATE_LOOP(&booz_estimator_uf_p, &booz_control_p_sp, \
&booz_estimator_uf_q, &booz_control_q_sp, \
&booz_estimator_uf_r, &booz_control_r_sp );
#define PERIODIC_SEND_BOOZ_ATT_LOOP() \
DOWNLINK_SEND_BOOZ_ATT_LOOP(&booz_estimator_phi, &booz_control_phi_sp, \
sw/airborne/imu_v3.h
+11 -12
View File
@@ -37,6 +37,7 @@ extern float imu_vs_gyro_raw_var[AXIS_NB];
extern int32_t imu_vs_mag_raw_avg[AXIS_NB];
extern float imu_vs_mag_raw_var[AXIS_NB];
extern struct adc_buf buf_ax;
extern struct adc_buf buf_ay;
extern struct adc_buf buf_az;
@@ -46,17 +47,15 @@ extern struct adc_buf buf_bat;
#define IMU_ACCEL_Y_NEUTRAL (506 * 32)
#define IMU_ACCEL_Z_NEUTRAL (506 * 32)
#define IMU_ACCEL_X_GAIN -1.
#define IMU_ACCEL_Y_GAIN 1.
#define IMU_ACCEL_Z_GAIN 1.
#define IMU_ACCEL_X_GAIN (-(6. * 9.81) / (1024*32))
#define IMU_ACCEL_Y_GAIN ( (6. * 9.81) / (1024*32))
#define IMU_ACCEL_Z_GAIN ( (6. * 9.81) / (1024*32))
#define ImuUpdateAccels() { \
imu_accel_raw[AXIS_X]= buf_ax.sum; \
imu_accel_raw[AXIS_Y]= buf_ay.sum; \
imu_accel_raw[AXIS_Z]= buf_az.sum; \
imu_accel[AXIS_X]= IMU_ACCEL_X_GAIN * (buf_ax.sum - IMU_ACCEL_X_NEUTRAL); \
imu_accel[AXIS_Y]= IMU_ACCEL_Y_GAIN * (buf_ay.sum - IMU_ACCEL_Y_NEUTRAL); \
imu_accel[AXIS_Z]= IMU_ACCEL_Y_GAIN * (buf_az.sum - IMU_ACCEL_Z_NEUTRAL); \
#define ImuUpdateAccels() { \
ImuReadAdcs(); \
imu_accel[AXIS_X]= IMU_ACCEL_X_GAIN * (imu_accel_raw[AXIS_X] - IMU_ACCEL_X_NEUTRAL); \
imu_accel[AXIS_Y]= IMU_ACCEL_Y_GAIN * (imu_accel_raw[AXIS_Y] - IMU_ACCEL_Y_NEUTRAL); \
imu_accel[AXIS_Z]= IMU_ACCEL_Y_GAIN * (imu_accel_raw[AXIS_Z] - IMU_ACCEL_Z_NEUTRAL); \
}
#define IMU_GYRO_P_NEUTRAL 40885
@@ -126,8 +125,8 @@ extern struct adc_buf buf_bat;
imu_gyro_prev[AXIS_Q] = imu_gyro[AXIS_Q]; \
imu_gyro_prev[AXIS_R] = imu_gyro[AXIS_R]; \
imu_vs_gyro_initial_bias[AXIS_P] = imu_gyro[AXIS_P]; \
imu_vs_gyro_initial_bias[AXIS_Q] = imu_gyro[AXIS_Q]; \
imu_vs_gyro_initial_bias[AXIS_R] = imu_gyro[AXIS_R]; \
imu_vs_gyro_initial_bias[AXIS_Q] = imu_gyro[AXIS_Q]; \
imu_vs_gyro_initial_bias[AXIS_R] = imu_gyro[AXIS_R]; \
imu_vs_gyro_unbiased[AXIS_P] = 0.; \
imu_vs_gyro_unbiased[AXIS_Q] = 0.; \
imu_vs_gyro_unbiased[AXIS_R] = 0.; \
sw/airborne/sim/imu_v3_hw.h
+9
View File
@@ -1,4 +1,13 @@
#ifndef IMU_V3_HW_H
#define IMU_V3_HW_H
#include "imu_v3.h"
#include "../../simulator/booz_sensors_model.h"
#define ImuReadAdcs() { \
imu_accel_raw[AXIS_X] = bsm.accel->ve[AXIS_X]; \
imu_accel_raw[AXIS_Y] = bsm.accel->ve[AXIS_Y]; \
imu_accel_raw[AXIS_Z] = bsm.accel->ve[AXIS_Z]; \
}
#endif /* IMU_V3_HW_H */
sw/simulator/booz_flight_model.c
+69 -21
View File
@@ -13,7 +13,7 @@ struct BoozFlightModel bfm;
//static void motor_model_derivative(VEC* x, VEC* u, VEC* xdot);
static VEC* booz_get_forces_body_frame( VEC* X, VEC* M, MAT* dcm, VEC* omega_square);
static VEC* booz_get_forces_body_frame( VEC* X, VEC* M, MAT* dcm, VEC* omega_square, VEC* rate_body, VEC* speed_body);
static VEC* booz_get_moments_body_frame( VEC* X, VEC* M, VEC* omega_square );
static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot);
@@ -44,6 +44,8 @@ void booz_flight_model_init( void ) {
bfm.props_moment_matrix->me[AXIS_Z][SERVO_MOTOR_BACK] = -bfm.torque_factor;
bfm.props_moment_matrix->me[AXIS_Z][SERVO_MOTOR_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;
@@ -58,6 +60,9 @@ void booz_flight_model_init( void ) {
}
#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 ) {
@@ -66,16 +71,56 @@ void booz_flight_model_run( double dt, double* commands ) {
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);
bfm.time += dt;
}
VEC* booz_flight_model_get_forces_body_frame(VEC* forces) {
/* square of prop rotational speeds */
static VEC *omega_square = VNULL;
omega_square = v_resize(omega_square,SERVOS_NB);
omega_square->ve[SERVO_MOTOR_BACK] = bfm.state->ve[BFMS_OM_B];
omega_square->ve[SERVO_MOTOR_FRONT] = bfm.state->ve[BFMS_OM_F];
omega_square->ve[SERVO_MOTOR_RIGHT] = bfm.state->ve[BFMS_OM_R];
omega_square->ve[SERVO_MOTOR_LEFT] = bfm.state->ve[BFMS_OM_L];
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);
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);
/* extract body speed from state */
static VEC *speed_body = VNULL;
speed_body = v_resize(speed_body, AXIS_NB);
speed_body->ve[AXIS_X] = bfm.state->ve[BFMS_U];
speed_body->ve[AXIS_Y] = bfm.state->ve[BFMS_V];
speed_body->ve[AXIS_Z] = bfm.state->ve[BFMS_W];
/* extracts body rates from 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];
/* compute forces */
forces = booz_get_forces_body_frame( bfm.state, forces , dcm, omega_square, rate_body, speed_body);
return forces;
}
/*
compute the sum of external forces.
assumes that dcm and omega_square are already precomputed from X
*/
static VEC* booz_get_forces_body_frame( VEC* X, VEC* F , MAT* dcm, VEC* omega_square) {
static VEC* booz_get_forces_body_frame( VEC* X, VEC* F , MAT* dcm, VEC* omega_square, VEC* rate_body, VEC* speed_body) {
// propeller thrust
F->ve[AXIS_X] = 0;
F->ve[AXIS_Y] = 0;
@@ -93,6 +138,12 @@ static VEC* booz_get_forces_body_frame( VEC* X, VEC* F , MAT* dcm, VEC* omega_sq
v_body->ve[AXIS_Z] = X->ve[BFMS_W];
double norm_speed = v_norm2(v_body);
F = v_mltadd(F, v_body, - norm_speed * C_d_body, F);
// 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 = v_add(F, fict_f, F);
return F;
}
@@ -129,11 +180,23 @@ 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);
speed_body->ve[AXIS_X] = X->ve[BFMS_U];
speed_body->ve[AXIS_Y] = X->ve[BFMS_V];
speed_body->ve[AXIS_Z] = X->ve[BFMS_W];
/* extracts body rates from state */
static VEC *rate_body = VNULL;
rate_body = v_resize(rate_body, AXIS_NB);
rate_body->ve[AXIS_P] = X->ve[BFMS_P];
rate_body->ve[AXIS_Q] = X->ve[BFMS_Q];
rate_body->ve[AXIS_R] = X->ve[BFMS_R];
/* compute external forces */
static VEC *f_body = VNULL;
f_body = v_resize(f_body, AXIS_NB);
f_body = booz_get_forces_body_frame( X, f_body , dcm, omega_square);
f_body = booz_get_forces_body_frame( X, f_body , dcm, omega_square, rate_body, speed_body);
/* compute external moments */
static VEC *m_body = VNULL;
@@ -141,11 +204,6 @@ static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot) {
m_body = booz_get_moments_body_frame( X, m_body, omega_square);
/* derivatives of position */
static VEC *speed_body = VNULL;
speed_body = v_resize(speed_body, AXIS_NB);
speed_body->ve[AXIS_X] = X->ve[BFMS_U];
speed_body->ve[AXIS_Y] = X->ve[BFMS_V];
speed_body->ve[AXIS_Z] = X->ve[BFMS_W];
static VEC *speed_earth = VNULL;
speed_earth = v_resize(speed_earth, AXIS_NB);
speed_earth = mv_mlt(dcm_t, speed_body,speed_earth);
@@ -154,19 +212,9 @@ static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot) {
Xdot->ve[BFMS_Z] = speed_earth->ve[AXIS_Z];
/* derivatives of speed */
/* extracts body rates from state */
static VEC *rate_body = VNULL;
rate_body = v_resize(rate_body, AXIS_NB);
rate_body->ve[AXIS_P] = X->ve[BFMS_P];
rate_body->ve[AXIS_Q] = X->ve[BFMS_Q];
rate_body->ve[AXIS_R] = X->ve[BFMS_R];
/* Newton in body frame */
static VEC *fict_f = VNULL;
fict_f = v_resize(fict_f, AXIS_NB);
fict_f = out_prod(speed_body, rate_body, fict_f);
Xdot->ve[BFMS_U] = 1./MASS * f_body->ve[AXIS_X] + fict_f->ve[AXIS_X];
Xdot->ve[BFMS_V] = 1./MASS * f_body->ve[AXIS_Y] + fict_f->ve[AXIS_Y];
Xdot->ve[BFMS_W] = 1./MASS * f_body->ve[AXIS_Z] + fict_f->ve[AXIS_Z];
Xdot->ve[BFMS_U] = 1./MASS * f_body->ve[AXIS_X];
Xdot->ve[BFMS_V] = 1./MASS * f_body->ve[AXIS_Y];
Xdot->ve[BFMS_W] = 1./MASS * f_body->ve[AXIS_Z];
/* derivatives of eulers */
double sinPHI = sin(X->ve[BFMS_PHI]);
sw/simulator/booz_flight_model.h
+3 -1
View File
@@ -41,6 +41,8 @@ struct BoozFlightModel {
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 */
@@ -51,7 +53,7 @@ extern struct BoozFlightModel bfm;
extern void booz_flight_model_init( void );
extern void booz_flight_model_run( double t, double* commands );
extern VEC* booz_flight_model_get_forces_body_frame(VEC* forces);
#endif /* BOOZ_FLIGHT_MODEL_H */
sw/simulator/booz_sensors_model.c
+97 -2
View File
@@ -13,6 +13,26 @@ void booz_sensors_model_init(void) {
bsm.accel->ve[AXIS_Y] = 0.;
bsm.accel->ve[AXIS_Z] = 0.;
bsm.accel_sensitivity = m_get(AXIS_NB, AXIS_NB);
m_zero(bsm.accel_sensitivity);
bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = -(1024 * 32) / (6. * 9.81);
bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = (1024. * 32) / (6. * 9.81);
bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = (1024. * 32) / (6. * 9.81);
bsm.accel_neutral = v_get(AXIS_NB);
bsm.accel_neutral->ve[AXIS_X] = 538 * 32;
bsm.accel_neutral->ve[AXIS_Y] = 506 * 32;
bsm.accel_neutral->ve[AXIS_Z] = 506 * 32;
bsm.accel_noise_std_dev = v_get(AXIS_NB);
bsm.accel_noise_std_dev->ve[AXIS_X] = 1e-1 * bsm.accel_sensitivity->me[AXIS_X][AXIS_X];
bsm.accel_noise_std_dev->ve[AXIS_Y] = 1e-1 * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y];
bsm.accel_noise_std_dev->ve[AXIS_Z] = 1e-1 * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z];
bsm.accel_bias = v_get(AXIS_NB);
bsm.accel_bias->ve[AXIS_P] = 1e-3 * bsm.accel_sensitivity->me[AXIS_X][AXIS_X];
bsm.accel_bias->ve[AXIS_Q] = 1e-3 * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y];
bsm.accel_bias->ve[AXIS_R] = 1e-3 * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z];
bsm.gyro = v_get(AXIS_NB);
bsm.gyro->ve[AXIS_P] = 0.;
@@ -30,11 +50,73 @@ void booz_sensors_model_init(void) {
bsm.gyro_neutral->ve[AXIS_Q] = 40910;
bsm.gyro_neutral->ve[AXIS_R] = 39552;
bsm.gyro_noise_std_dev = v_get(AXIS_NB);
bsm.gyro_noise_std_dev->ve[AXIS_P] = 5e-3 * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
bsm.gyro_noise_std_dev->ve[AXIS_Q] = 5e-3 * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
bsm.gyro_noise_std_dev->ve[AXIS_R] = 5e-3 * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
bsm.gyro_bias = v_get(AXIS_NB);
bsm.gyro_bias->ve[AXIS_P] = 1e-5 * bsm.gyro_sensitivity->me[AXIS_P][AXIS_P];
bsm.gyro_bias->ve[AXIS_Q] = 1e-5 * bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q];
bsm.gyro_bias->ve[AXIS_R] = 1e-5 * bsm.gyro_sensitivity->me[AXIS_R][AXIS_R];
}
void booz_sensors_model_run(void) {
/* compute forces */
static VEC *accel_body = VNULL;
accel_body = v_resize(accel_body, AXIS_NB);
#if 0
accel_body = booz_flight_model_get_forces_body_frame(accel_body);
/* divide by mass */
accel_body = sv_mlt(1./bfm.mass, accel_body, accel_body);
/* 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);
accel_body = v_sub(g_body, accel_body, accel_body);
#else
accel_body->ve[AXIS_X] = -9.81 * sin(bfm.state->ve[BFMS_THETA]);
accel_body->ve[AXIS_Y] = 9.81 * sin(bfm.state->ve[BFMS_PHI]) * cos(bfm.state->ve[BFMS_THETA]);
accel_body->ve[AXIS_Z] = 9.81 * cos(bfm.state->ve[BFMS_PHI]) * cos(bfm.state->ve[BFMS_THETA]);
#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);
/* compute accel error readings */
/* gaussian noise */
static VEC *accel_error = VNULL;
accel_error = v_resize(accel_error, AXIS_NB);
accel_error = v_rand(accel_error);
static VEC *one = VNULL;
one = v_resize(one, AXIS_NB);
one = v_ones(one);
accel_error = v_mltadd(one, accel_error, -2., accel_error);
accel_error = v_star(accel_error, bsm.accel_noise_std_dev, accel_error);
/* constant bias */
accel_error = v_add(accel_error, bsm.accel_bias, accel_error);
/* 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]);
// printf("sim adc %f %f %f\n",bsm.accel->ve[AXIS_X] ,bsm.accel->ve[AXIS_Y] ,bsm.accel->ve[AXIS_Z]);
/* extract rotational speed from flight model state */
static VEC *rate_body = VNULL;
rate_body = v_resize(rate_body, AXIS_NB);
@@ -42,9 +124,22 @@ void booz_sensors_model_run(void) {
rate_body->ve[AXIS_Q] = bfm.state->ve[BFMS_Q];
rate_body->ve[AXIS_R] = bfm.state->ve[BFMS_R];
/* compute sensor readings */
/* compute gyros readings */
bsm.gyro = mv_mlt(bsm.gyro_sensitivity, rate_body, bsm.gyro);
bsm.gyro = v_add(bsm.gyro, bsm.gyro_neutral, bsm.gyro);
/* compute gyro error readinf*/
/* gaussian noise */
static VEC *gyro_error = VNULL;
gyro_error = v_resize(gyro_error, AXIS_NB);
gyro_error = v_rand(gyro_error);
gyro_error = v_mltadd(one, gyro_error, -2., gyro_error);
gyro_error = v_star(gyro_error, bsm.gyro_noise_std_dev, gyro_error);
/* constant bias */
gyro_error = v_add(bsm.gyro_bias, gyro_error, 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]);
sw/simulator/booz_sensors_model.h
+9
View File
@@ -8,10 +8,19 @@ extern void booz_sensors_model_init(void);
extern void booz_sensors_model_run(void);
struct BoozSensorsModel {
VEC* accel;
MAT* accel_sensitivity;
VEC* accel_neutral;
VEC* accel_noise_std_dev;
VEC* accel_bias;
VEC* gyro;
MAT* gyro_sensitivity;
VEC* gyro_neutral;
VEC* gyro_noise_std_dev;
VEC* gyro_bias;
};
extern struct BoozSensorsModel bsm;
sw/simulator/main_booz_sim.c
+31 -7
View File
@@ -56,7 +56,8 @@ static gboolean booz_sim_periodic(gpointer data) {
/* read actuators positions */
booz_sim_read_actuators();
booz_flight_model_run(DT, booz_sim_actuators_values);
if (sim_time > 3.)
booz_flight_model_run(DT, booz_sim_actuators_values);
booz_sensors_model_run();
sim_time += DT;
@@ -64,19 +65,21 @@ static gboolean booz_sim_periodic(gpointer data) {
/* the sim implementation of max1167_read ( called by ImuPeriodic() ) */
/* will post a ImuEvent */
booz_filter_main_periodic_task();
/* process the ImuEvent */
/* it will run the filter and the inter-process communication which */
/* will post a BoozLinkMcuEvent in the Controller process */
booz_filter_main_event_task();
/* process the BoozLinkMcuEvent */
/* this will update the controller stimator */
/* this will update the controller estimator */
booz_controller_main_event_task();
/* post a radio control event */
booz_sim_set_ppm_from_joystick();
ppm_valid = TRUE;
/* and let the controller process it */
booz_controller_main_event_task();
// printf("ppm_pulses %d\n", ppm_pulses[RADIO_THROTTLE]);
// printf("rc_value %d\n", rc_values[RADIO_THROTTLE]);
/* call the controller periodic task to run control loops */
booz_controller_main_periodic_task();
@@ -132,22 +135,43 @@ static inline void booz_sim_display(void) {
if (sim_time >= disp_time) {
disp_time+= DT_DISPLAY;
booz_flightgear_send();
IvySendMsg("148 BOOZ_RPMS %f %f %f %f",
IvySendMsg("148 BOOZ_SIM_RPMS %f %f %f %f",
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]) );
IvySendMsg("148 BOOZ_SIM_BODY_RATE %f %f %f",
DegOfRad(bfm.state->ve[BFMS_P]),
DegOfRad(bfm.state->ve[BFMS_Q]),
DegOfRad(bfm.state->ve[BFMS_R]));
IvySendMsg("148 BOOZ_SIM_ATTITUDE %f %f %f",
DegOfRad(bfm.state->ve[BFMS_PHI]),
DegOfRad(bfm.state->ve[BFMS_THETA]),
DegOfRad(bfm.state->ve[BFMS_PSI]));
}
}
static void booz_sim_set_ppm_from_joystick( void ) {
// printf("joystick_value %f\n", booz_joystick_value[JS_THROTTLE]);
ppm_pulses[RADIO_THROTTLE] = 1223 + booz_joystick_value[JS_THROTTLE] * (2050-1223);
ppm_pulses[RADIO_PITCH] = 1498 + booz_joystick_value[JS_PITCH] * (2050-950);
ppm_pulses[RADIO_ROLL] = 1500 + booz_joystick_value[JS_ROLL] * (2050-950);
ppm_pulses[RADIO_YAW] = 1500 + booz_joystick_value[JS_YAW] * (2050-950);
ppm_pulses[RADIO_YAW] = 1493 + booz_joystick_value[JS_YAW] * (2050-950);
ppm_pulses[RADIO_MODE] = 1500 + booz_joystick_value[JS_MODE] * (2050-950);
int foo = sim_time/5;
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.7 * (2050-1223);
ppm_pulses[RADIO_MODE] = 1500 + 0. * (2050-950);
if (foo%2) {
ppm_pulses[RADIO_ROLL] = 1500 + 0.2 * (2050-950);
ppm_pulses[RADIO_PITCH] = 1500 + 0.2 * (2050-950);
}
else {
ppm_pulses[RADIO_ROLL] = 1500 - 0.2 * (2050-950);
ppm_pulses[RADIO_PITCH] = 1500 - 0.2 * (2050-950);
}
}
@@ -157,12 +181,12 @@ static void booz_sim_read_actuators( void ) {
booz_sim_actuators_values[SERVO_MOTOR_FRONT] = (actuators[SERVO_MOTOR_FRONT] - 1200)/(double)(1850-1200);
booz_sim_actuators_values[SERVO_MOTOR_RIGHT] = (actuators[SERVO_MOTOR_RIGHT] - 1200)/(double)(1850-1200);
booz_sim_actuators_values[SERVO_MOTOR_LEFT] = (actuators[SERVO_MOTOR_LEFT] - 1200)/(double)(1850-1200);
#endif
#else
booz_sim_actuators_values[SERVO_MOTOR_BACK] = (actuators[SERVO_MOTOR_BACK] - 0)/(double)(255);
booz_sim_actuators_values[SERVO_MOTOR_FRONT] = (actuators[SERVO_MOTOR_FRONT] - 0)/(double)(255);
booz_sim_actuators_values[SERVO_MOTOR_RIGHT] = (actuators[SERVO_MOTOR_RIGHT] - 0)/(double)(255);
booz_sim_actuators_values[SERVO_MOTOR_LEFT] = (actuators[SERVO_MOTOR_LEFT] - 0)/(double)(255);
#endif
}