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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2008-01-15 02:46:24 +00:00
parent 84eb61ea4f
commit ea68b1d0b8
10 changed files with 92 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/conf_booz.makefile
+5 -1
View File
@@ -169,9 +169,13 @@ sim.srcs += imu_v3.c $(SRC_ARCH)/imu_v3_hw.c
sim.CFLAGS += -DFLOAT_T=float
sim.srcs += booz_ahrs.c
sim.CFLAGS += -DBOOZ_AHRS_TYPE=BOOZ_AHRS_MULTITILT
sim.srcs += multitilt.c
#sim.CFLAGS += -DBOOZ_AHRS_TYPE=BOOZ_AHRS_QUATERNION
#sim.CFLAGS += -DEKF_UPDATE_DISCRETE
#sim.CFLAGS += -DEKF_PREDICT_ONLY
#sim.srcs += ahrs_quat_fast_ekf.c
#sim.srcs += ahrs_quat_fast_ekf.c
sim.srcs += comp_flt.c
sw/airborne/ahrs_quat_fast_ekf.h
-3
View File
@@ -4,9 +4,6 @@
#include <inttypes.h>
#include "6dof.h"
//#define FLOAT_T double
//#define FLOAT_T float
/* ekf state : quaternion and gyro biases */
extern FLOAT_T afe_q0, afe_q1, afe_q2, afe_q3;
//extern FLOAT_T afe_bias_p, afe_bias_q, afe_bias_r;
sw/airborne/booz_filter_main.c
+5 -4
View File
@@ -75,17 +75,18 @@ STATIC_INLINE void booz_filter_main_periodic_task( void ) {
booz_filter_telemetry_periodic_task();
break;
#ifndef SITL
case 1:
// case 1:
/* triger measurements */
ami601_periodic();
// ami601_periodic();
// Z Y X
DOWNLINK_SEND_IMU_MAG_RAW(&ami601_val[0], &ami601_val[4], &ami601_val[2]);
// DOWNLINK_SEND_IMU_MAG_RAW(&ami601_val[0], &ami601_val[4], &ami601_val[2]);
#endif
}
}
static inline void on_imu_event( void ) {
switch (booz_ahrs_status) {
case BOOZ_AHRS_STATUS_UNINIT :
@@ -95,7 +96,7 @@ static inline void on_imu_event( void ) {
booz_ahrs_start(imu_accel, imu_gyro, imu_mag);
}
break;
case BOOZ_AHRS_STATUS_RUNNING :
// t0 = T0TC;
booz_ahrs_run(imu_accel, imu_gyro_prev, imu_mag);
sw/airborne/imu_v3.h
+4 -4
View File
@@ -47,10 +47,10 @@ extern struct adc_buf buf_bat;
#define IMU_ACCEL_X_NEUTRAL (538 * 32)
#define IMU_ACCEL_Y_NEUTRAL (506 * 32)
#define IMU_ACCEL_Z_NEUTRAL (506 * 32)
#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))
// FIXME ??? why 0.5
#define IMU_ACCEL_X_GAIN ( (0.5 * 6. * 9.81) / (1024*32))
#define IMU_ACCEL_Y_GAIN (-(0.5 * 6. * 9.81) / (1024*32))
#define IMU_ACCEL_Z_GAIN (-(0.5 * 6. * 9.81) / (1024*32))
#define ImuUpdateAccels() { \
ImuReadAdcs(); \
sw/airborne/multitilt.c
+4 -4
View File
@@ -15,10 +15,10 @@ float mtt_P_psi[2][2];
/* process covariance noise */
//static const float Q_angle = 0.0005;
static const float Q_angle = 0.00025;
static const float Q_bias = 0.0015;
static const float Q_bias = 0.0005;
/* Measurement covariance */
//static const float R_accel = 0.3;
static const float R_accel = 0.4;
static const float R_accel = 0.5;
static inline void multitilt_update( const float* accel, const int16_t* mag );
static inline void mtt_update_axis(float _err, float _P[2][2], float* angle, float* bias);
@@ -29,7 +29,7 @@ static inline void mtt_predict_axis(float* angle, float angle_dot, float P[2][2]
#define WRAP(x,a) { while (x > a) x -= 2 * a; while (x <= -a) x += 2 * a;}
static inline float phi_of_accel( const float* accel) {
return atan2(accel[AXIS_Y], accel[AXIS_Z]);
return atan2(-accel[AXIS_Y], -accel[AXIS_Z]);
}
static inline float theta_of_accel( const float* accel) {
@@ -37,7 +37,7 @@ static inline float theta_of_accel( const float* accel) {
accel[AXIS_X]*accel[AXIS_X] +
accel[AXIS_Y]*accel[AXIS_Y] +
accel[AXIS_Z]*accel[AXIS_Z];
return -asin( accel[AXIS_X] / sqrt( g2 ) );
return asin( accel[AXIS_X] / sqrt( g2 ) );
}
static inline float psi_of_mag( const int16_t* mag) {
sw/simulator/booz_flight_model.c
+36 -17
View File
@@ -4,7 +4,6 @@
#include "booz_flight_model_params.h"
#include "booz_flight_model_utils.h"
#include "airframe.h"
#include "6dof.h"
@@ -13,11 +12,13 @@ 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 VEC* booz_get_forces_body_frame(VEC* M, MAT* dcm, VEC* omega_square, VEC* speed_body);
static VEC* booz_get_moments_body_frame(VEC* M, VEC* omega_square );
static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot);
void booz_flight_model_init( void ) {
bfm.on_ground = TRUE;
bfm.time = 0.;
bfm.bat_voltage = BAT_VOLTAGE;
bfm.mot_voltage = v_get(SERVOS_NB);
@@ -90,20 +91,33 @@ void booz_flight_model_run( double dt, double* commands ) {
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* F , MAT* dcm, VEC* omega_square, VEC* speed_body) {
// propeller thrust
F->ve[AXIS_X] = 0;
F->ve[AXIS_Y] = 0;
F->ve[AXIS_Z] = -v_sum(omega_square) * bfm.thrust_factor;
// 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, MASS, F);
// body drag
double norm_speed = v_norm2(speed_body);
F = v_mltadd(F, speed_body, - norm_speed * C_d_body, F);
VEC* booz_get_forces_body_frame(VEC* F , MAT* dcm, VEC* omega_square, VEC* speed_body, int exclude_weight) {
// FIXME : nimporte koi !
if (bfm.on_ground) {
F->ve[AXIS_X] = 0;
F->ve[AXIS_Y] = 0;
if (!exclude_weight)
F->ve[AXIS_Z] = 0;
else
F->ve[AXIS_Z] = -9.81*MASS;
}
else {
// propeller thrust
F->ve[AXIS_X] = 0;
F->ve[AXIS_Y] = 0;
F->ve[AXIS_Z] = -v_sum(omega_square) * bfm.thrust_factor;
if (!exclude_weight) {
// 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, MASS, F);
}
// body drag
double norm_speed = v_norm2(speed_body);
F = v_mltadd(F, speed_body, - norm_speed * C_d_body, F);
}
return F;
}
@@ -112,7 +126,12 @@ static VEC* booz_get_forces_body_frame(VEC* F , MAT* dcm, VEC* omega_square, VEC
assumes that omega_square is already precomputed from X
*/
static VEC* booz_get_moments_body_frame(VEC* M, VEC* omega_square ) {
M = mv_mlt(bfm.props_moment_matrix, omega_square, M);
if (bfm.on_ground) {
M = v_zero(M);
}
else {
M = mv_mlt(bfm.props_moment_matrix, omega_square, M);
}
return M;
}
@@ -157,7 +176,7 @@ static void booz_flight_model_get_derivatives(VEC* X, VEC* u, VEC* Xdot) {
/* 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);
f_body = booz_get_forces_body_frame(f_body , dcm, omega_square, speed_body, FALSE);
/* add non inertial forces */
static VEC *fict_f = VNULL;
sw/simulator/booz_flight_model.h
+7
View File
@@ -2,6 +2,8 @@
#define BOOZ_FLIGHT_MODEL_H
#include <matrix.h>
#include "airframe.h"
#define BFMS_X 0
#define BFMS_Y 1
@@ -23,6 +25,9 @@
struct BoozFlightModel {
/* are we flying ? */
int on_ground;
double time;
/* battery voltage in V */
double bat_voltage;
@@ -56,6 +61,8 @@ 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* M, MAT* dcm, VEC* omega_square, VEC* speed_body, int exclude_weight);
#define BoozFlighModelGetPos(_dest) { \
_dest->ve[AXIS_X] = bfm.state->ve[BFMS_X]; \
_dest->ve[AXIS_Y] = bfm.state->ve[BFMS_Y]; \
sw/simulator/booz_sensors_model.c
+23 -26
View File
@@ -238,37 +238,34 @@ static void booz_sensors_model_accel_run( MAT* dcm ) {
accel_body = v_resize(accel_body, AXIS_NB);
accel_body = v_zero(accel_body);
#if 1
/* get g in body frame */
static VEC *g_body = VNULL;
g_body = v_resize(g_body, AXIS_NB);
g_body = mv_mlt(dcm, bfm.g_earth, g_body);
/* compute sum of forces in body frame except gravity */
/* add non inertial forces */
/* 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);
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];
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);
BoozFlighModelGetSpeed(speed_body);
accel_body = booz_get_forces_body_frame(accel_body , dcm, omega_square, speed_body, TRUE);
/* divide by mass */
// accel_body = sv_mlt(1./bfm.mass, accel_body, accel_body);
accel_body = v_add(g_body, fict_f, accel_body);
#else
printf(" accel_body # %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]);
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]);
printf(" accel_body ~ %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]);
accel_body = sv_mlt(1./bfm.mass, accel_body, accel_body);
//#else
//printf(" accel_body # %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]);
static VEC* accel_inert = VNULL;
accel_inert = v_resize(accel_inert, AXIS_NB);
accel_inert->ve[AXIS_X] = 0;
accel_inert->ve[AXIS_Y] = 0;
accel_inert->ve[AXIS_Z] = -9.81;
accel_body = mv_mlt(dcm, accel_inert, accel_body);
//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 */
sw/simulator/booz_sensors_model_params.h
+6 -6
View File
@@ -6,16 +6,16 @@
*/
#define BSM_ACCEL_RESOLUTION (1024 * 32)
/* ms-2 */
#define BSM_ACCEL_SENSITIVITY_XX -(1024. * 32.)/(2 * 6. * 9.81)
#define BSM_ACCEL_SENSITIVITY_YY (1024. * 32.)/(2 * 6. * 9.81)
#define BSM_ACCEL_SENSITIVITY_ZZ (1024. * 32.)/(2 * 6. * 9.81)
#define BSM_ACCEL_SENSITIVITY_XX (1024. * 32.)/(0.5 * 6. * 9.81)
#define BSM_ACCEL_SENSITIVITY_YY -(1024. * 32.)/(0.5 * 6. * 9.81)
#define BSM_ACCEL_SENSITIVITY_ZZ -(1024. * 32.)/(0.5 * 6. * 9.81)
#define BSM_ACCEL_NEUTRAL_X (538. * 32.)
#define BSM_ACCEL_NEUTRAL_Y (506. * 32.)
#define BSM_ACCEL_NEUTRAL_Z (506. * 32.)
/* m2s-4 */
#define BSM_ACCEL_NOISE_STD_DEV_X 2e-1
#define BSM_ACCEL_NOISE_STD_DEV_Y 2e-1
#define BSM_ACCEL_NOISE_STD_DEV_Z 2e-1
#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
sw/simulator/main_booz_sim.c
+2 -1
View File
@@ -61,9 +61,10 @@ static gboolean booz_sim_periodic(gpointer data __attribute__ ((unused))) {
/* run our models */
if (sim_time > 3.)
bfm.on_ground = FALSE;
/* no fdm at start to allow for filter initialisation */
/* it sucks, I know */
booz_flight_model_run(DT, booz_sim_actuators_values);
booz_flight_model_run(DT, booz_sim_actuators_values);
booz_sensors_model_run(DT);