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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2009-02-19 15:10:55 +00:00
parent f7677499e0
commit 9922e16062
17 changed files with 269 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/booz2_simulator.makefile
+2 -1
View File
@@ -19,7 +19,7 @@ sim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lmeschach -lpcre -lglibivy
#sim.CFLAGS += -DBYPASS_AHRS
#sim.CFLAGS += -DBYPASS_INS
sim.CFLAGS += -DINIT_WIND_X=-5.0
sim.CFLAGS += -DINIT_WIND_X=-0.0
sim.CFLAGS += -DINIT_WIND_Y=-0.0
sim.CFLAGS += -DINIT_WIND_Z=-0.0
@@ -106,6 +106,7 @@ sim.srcs += $(SRC_BOOZ)/booz2_ins.c
# vertical filter float version
sim.srcs += $(SRC_BOOZ)/booz2_vf_float.c
sim.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)" -DFLOAT_T=float
sim.srcs += $(SRC_BOOZ)/booz2_hf_float.c
conf/messages.xml
+21 -1
View File
@@ -510,6 +510,12 @@
<field name="errno" type="uint8"/>
</message>
<message name="BOOZ_SIM_ACCEL_LTP" id="248">
<field name="xdd" type="float" unit="m/s2"/>
<field name="ydd" type="float" unit="m/s2"/>
<field name="zdd" type="float" unit="m/s2"/>
</message>
<message name="VF_UPDATE" id="76">
<field name="baro" type="float" unit="m"/>
<field name="range_meter" type="float" unit="m"/>
@@ -870,8 +876,22 @@
<field name="ins_zdd" type="int32" alt_unit="m/s2" alt_unit_coef="0.0009766"/>
</message>
<message name="BOOZ2_INS2" id="152">
<field name="xdd" type="int32" alt_unit="m/s2" alt_unit_coef="0.0009766"/>
<field name="ydd" type="int32" alt_unit="m/s2" alt_unit_coef="0.0009766"/>
<field name="zdd" type="int32" alt_unit="m/s2" alt_unit_coef="0.0009766"/>
<field name="xd" type="int32" alt_unit="m/s" alt_unit_coef="0.0000019073"/>
<field name="yd" type="int32" alt_unit="m/s" alt_unit_coef="0.0000019073"/>
<field name="zd" type="int32" alt_unit="m/s" alt_unit_coef="0.0000019073"/>
<field name="x" type="int32" alt_unit="m" alt_unit_coef="0.0039"/>
<field name="y" type="int32" alt_unit="m" alt_unit_coef="0.0039"/>
<field name="z" type="int32" alt_unit="m" alt_unit_coef="0.0039"/>
</message>
<message name="WT" id="152">
<message name="WT" id="153">
<field name="rpm" type="float"/>
</message>
sw/airborne/6dof.h
+9
View File
@@ -19,4 +19,13 @@
#define EULER_PSI 2
#define EULER_NB 3
#define QUAT_QI 0
#define QUAT_QX 1
#define QUAT_QY 2
#define QUAT_QZ 3
#define QUAT_NB 4
#endif /* _6DOF_H */
sw/airborne/booz/booz2_filter_attitude.h
+2
View File
@@ -16,6 +16,8 @@ struct Booz_ahrs_state {
extern struct Booz_ahrs_state booz_ahrs_state;
extern struct booz_ieuler booz2_filter_attitude_euler_aligned;
extern struct booz_iquat booz2_filter_attitude_quat_aligned;
extern struct booz_ieuler booz2_filter_attitude_euler;
extern struct booz_ivect booz2_filter_attitude_rate;
sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c
+3 -3
View File
@@ -14,7 +14,7 @@ struct booz_ieuler booz2_filter_attitude_euler;
struct booz_ivect booz2_filter_attitude_rate;
struct booz_ieuler booz2_filter_attitude_euler_aligned;
struct booz_iquat booz2_filter_attitude_quat_aligned;
struct booz_ivect booz2_face_gyro_bias;
struct booz_ieuler booz2_face_measure;
@@ -161,10 +161,10 @@ void booz2_filter_attitude_propagate(void) {
/* scale our result */
BOOZ_IEULER_SDIV(booz2_filter_attitude_euler, booz2_face_corrected, F_UPDATE);
/* convert to quaternion */
// BOOZ_IQUAT_OF_EULER(booz2_filter_attitude_quat, booz2_filter_attitude_euler);
apply_alignment();
/* convert to quaternion */
BOOZ_IQUAT_OF_EULER(booz2_filter_attitude_quat_aligned, booz2_filter_attitude_euler_aligned);
}
sw/airborne/booz/booz2_hf_float.c
+85 -10
View File
@@ -1,22 +1,97 @@
#include "booz2_hf_float.h"
#include "booz2_imu.h"
#include "booz2_filter_attitude.h"
#include "booz_geometry_mixed.h"
struct FloatVect3 bhff_accel_bias;
struct FloatVect3 bhff_pos;
struct FloatVect3 bhff_speed_ltp;
struct FloatVect3 bhff_accel_ltp;
struct Int32Vect3 b2ins_accel_bias;
struct Int32Vect3 b2ins_accel_ltp;
struct Int32Vect3 b2ins_speed_ltp;
struct Int64Vect3 b2ins_pos_ltp;
struct Int32Eulers b2ins_body_to_imu_eulers;
struct Int32Quat b2ins_body_to_imu_quat;
struct Int32Quat b2ins_imu_to_body_quat;
struct Int32Vect2 b2ins_ltp_meas_gps;
#include "downlink.h"
void b2ins_init(void) {
INT32_VECT3_ZERO(b2ins_accel_bias);
b2ins_body_to_imu_eulers.phi = FILTER_ALIGNMENT_DPHI;
b2ins_body_to_imu_eulers.theta = FILTER_ALIGNMENT_DTHETA;
b2ins_body_to_imu_eulers.psi = FILTER_ALIGNMENT_DPSI;
BOOZ_IQUAT_OF_EULER(b2ins_body_to_imu_quat, b2ins_body_to_imu_eulers);
INT32_QUAT_INVERT(b2ins_imu_to_body_quat, b2ins_body_to_imu_quat);
}
#include "booz_flight_model.h"
#include "6dof.h"
void b2ins_propagate(void) {
#ifdef BYPASS_AHRS
booz2_filter_attitude_quat_aligned.qi = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QI], IQUAT_RES);
booz2_filter_attitude_quat_aligned.qx = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QX], IQUAT_RES);
booz2_filter_attitude_quat_aligned.qy = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QY], IQUAT_RES);
booz2_filter_attitude_quat_aligned.qz = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QZ], IQUAT_RES);
#endif /* BYPASS_AHRS */
void bhff_update(void) {
struct FloatVect3 accel_body;
/* unbias accels */
FLOAT_VECT3_SUB(accel_body, booz2_imu_accel, bhff_accel_bias);
struct Int32Vect3 accel_imu;
/* unbias accelerometers */
INT32_VECT3_DIFF(accel_imu, booz2_imu_accel, b2ins_accel_bias);
/* convert to LTP */
struct Int32Quat body_to_ltp;
BOOZ_IQUAT_INVERT(body_to_ltp, booz2_filter_attitude_quat_aligned);
struct Int32Quat imu_to_ltp;
INT32_QUAT_MULT(imu_to_ltp, body_to_ltp, b2ins_imu_to_body_quat);
BOOZ_IQUAT_VMULT(b2ins_accel_ltp, imu_to_ltp, accel_imu);
/* correct for gravity */
b2ins_accel_ltp.z += BOOZ_ACCEL_I_OF_F(9.81);
/* propagate speed */
b2ins_speed_ltp.x += b2ins_accel_ltp.x;
b2ins_speed_ltp.y += b2ins_accel_ltp.y;
b2ins_speed_ltp.z += b2ins_accel_ltp.z;
/* propagate position */
b2ins_pos_ltp.x += b2ins_speed_ltp.x;
b2ins_pos_ltp.y += b2ins_speed_ltp.y;
b2ins_pos_ltp.z += b2ins_speed_ltp.z;
RunOnceEvery(10, {
struct Int32Vect3 pos_low_res; \
pos_low_res.x = (int32_t)(b2ins_pos_ltp.x>> 20); \
pos_low_res.y = (int32_t)(b2ins_pos_ltp.y>> 20); \
pos_low_res.z = (int32_t)(b2ins_pos_ltp.z>> 20); \
DOWNLINK_SEND_BOOZ2_INS2(&b2ins_accel_ltp.x, \
&b2ins_accel_ltp.y, \
&b2ins_accel_ltp.z, \
&b2ins_speed_ltp.x, \
&b2ins_speed_ltp.y, \
&b2ins_speed_ltp.z, \
&pos_low_res.x, \
&pos_low_res.y, \
&pos_low_res.z \
); \
});
}
void b2ins_update_gps(void) {
b2ins_ltp_meas_gps.x = ;
b2ins_ltp_meas_gps.y = ;
}
sw/airborne/booz/booz2_hf_float.h
+5 -1
View File
@@ -1,7 +1,11 @@
#ifnedf BOOZ2_HF_FLOAT_H
#ifndef BOOZ2_HF_FLOAT_H
#define BOOZ2_HF_FLOAT_H
#include "pprz_algebra_float.h"
#include "pprz_algebra_int.h"
void b2ins_init(void);
extern void b2ins_propagate(void);
extern void b2ins_update_gps(void);
#endif /* BOOZ2_HF_FLOAT_H */
sw/airborne/booz/booz2_ins.c
+14
View File
@@ -10,6 +10,10 @@
#ifdef USE_VFF
#include "booz2_vf_float.h"
#endif
#ifdef SITL
#include "booz2_filter_attitude.h"
#include "booz2_hf_float.h"
#endif
struct Pprz_int32_lla booz_ins_position_init_lla; // LLA
@@ -38,6 +42,9 @@ void booz_ins_init() {
booz_ins_baro_initialised = FALSE;
b2_vff_init(0., 0., 0.);
#endif
#ifdef SITL
b2ins_init();
#endif
}
void booz_ins_propagate() {
@@ -57,6 +64,10 @@ void booz_ins_propagate() {
booz_ins_position.z = BOOZ_POS_I_OF_F(b2_vff_z);
}
#endif
#ifdef SITL
if (booz2_filter_attitude_status == BOOZ2_FILTER_ATTITUDE_RUNNING)
b2ins_propagate();
#endif
}
void booz_ins_update_baro() {
@@ -90,6 +101,9 @@ void booz_ins_update_gps(void) {
// copy to pos NED
PPRZ_INT32_VECT2_OF_LL(booz_ins_position, booz_ins_position_lla);
}
}
sw/airborne/booz/booz2_main.c
+1
View File
@@ -193,6 +193,7 @@ static inline void on_imu_event( void ) {
// LED_ON(7);
booz2_filter_attitude_propagate();
booz2_filter_attitude_update();
// LED_OFF(7);
booz_ins_propagate();
}
sw/airborne/booz/booz2_vf_float.c
+2 -2
View File
@@ -5,8 +5,8 @@
X = [ z zdot bias ]
temps :
predict 86us
update 46us
propagate 86us
update 46us
*/
/* initial covariance diagonal */
sw/airborne/pprz_algebra_int.h
+64 -2
View File
@@ -15,6 +15,27 @@ struct Int32Vect3 {
int32_t z;
};
struct Int64Vect3 {
int64_t x;
int64_t y;
int64_t z;
};
struct Int32Quat {
int32_t qi;
int32_t qx;
int32_t qy;
int32_t qz;
};
struct Int32Eulers {
int32_t phi;
int32_t theta;
int32_t psi;
};
#if 0
struct Int32Mat33 {
@@ -23,16 +44,57 @@ struct Int32Mat33 {
#endif
typedef int32_t Int32Mat33[3*3];
#define PPRZ_INT32_VECT3_COPY(_o, _i) { \
#define INT32_VECT3_ZERO(_o) { \
_o.x = 0; \
_o.y = 0; \
_o.z = 0; \
}
#define INT32_VECT3_COPY(_o, _i) { \
_o.x = _i.x; \
_o.y = _i.y; \
_o.z = _i.z; \
}
#define PPRZ_INT32_MAT33_VECT3_MUL(_o, _m, _v) { \
#define INT32_VECT3_DIFF(_c, _a, _b) { \
_c.x = _a.x - _b.x; \
_c.y = _a.y - _b.y; \
_c.z = _a.z - _b.z; \
}
#define INT32_MAT33_VECT3_MUL(_o, _m, _v) { \
_o.x = _m[0][0]*_v.x + _m[0][1]*_v.y + _m[0][2]*_v.z; \
_o.y = _m[1][0]*_v.x + _m[1][1]*_v.y + _m[1][2]*_v.z; \
_o.z = _m[2][0]*_v.x + _m[2][1]*_v.y + _m[2][2]*_v.z; \
}
#define INT32_QUAT_ZERO(_q) { \
_q.qi = (1 << IQUAT_RES); \
_q.qx = 0; \
_q.qy = 0; \
_q.qz = 0; \
}
#define INT32_QUAT_MULT(c, a, b) { \
c.qi = (a.qi*b.qi - a.qx*b.qx - a.qy*b.qy - a.qz*b.qz)>>IQUAT_RES; \
c.qx = (a.qi*b.qx + a.qx*b.qi + a.qy*b.qz - a.qz*b.qy)>>IQUAT_RES; \
c.qy = (a.qi*b.qy - a.qx*b.qz + a.qy*b.qi + a.qz*b.qx)>>IQUAT_RES; \
c.qz = (a.qi*b.qz + a.qx*b.qy - a.qy*b.qx + a.qz*b.qi)>>IQUAT_RES; \
}
#define INT32_QUAT_INVERT(_qo, _qi) { \
_qo.qi = _qi.qi; \
_qo.qx = -_qi.qx; \
_qo.qy = -_qi.qy; \
_qo.qz = -_qi.qz; \
}
#endif /* PPRZ_ALGEBRA_INT_H */
sw/simulator/booz2_sim_main.c
+17
View File
@@ -118,6 +118,8 @@ static gboolean booz2_sim_periodic(gpointer data __attribute__ ((unused))) {
}
#include "booz2_filter_attitude.h"
static void sim_run_one_step(void) {
/* read actuators positions */
@@ -191,6 +193,13 @@ static void sim_overwrite_ahrs(void) {
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_quat_aligned.qi = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QI], IQUAT_RES);
booz2_filter_attitude_quat_aligned.qx = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QX], IQUAT_RES);
booz2_filter_attitude_quat_aligned.qy = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QY], IQUAT_RES);
booz2_filter_attitude_quat_aligned.qz = BOOZ_INT_OF_FLOAT(bfm.quat->ve[QUAT_QZ], IQUAT_RES);
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]);
@@ -271,11 +280,19 @@ static void booz2_sim_display(void) {
(bfm.pos_ltp->ve[AXIS_X]),
(bfm.pos_ltp->ve[AXIS_Y]),
(bfm.pos_ltp->ve[AXIS_Z]));
#if 0
IvySendMsg("%d BOOZ_SIM_WIND %f %f %f",
AC_ID,
bwm.velocity->ve[AXIS_X],
bwm.velocity->ve[AXIS_Y],
bwm.velocity->ve[AXIS_Z]);
#endif
IvySendMsg("%d BOOZ_SIM_ACCEL_LTP %f %f %f",
AC_ID,
bfm.accel_ltp->ve[AXIS_X],
bfm.accel_ltp->ve[AXIS_Y],
bfm.accel_ltp->ve[AXIS_Z]);
}
}
sw/simulator/booz_flight_model.c
+4 -3
View File
@@ -169,7 +169,9 @@ static void booz_flight_model_update_byproducts(void) {
dcm_of_eulers(bfm.eulers, bfm.dcm);
/* transpose of dcm ( body to inertial ) */
m_transp(bfm.dcm, bfm.dcm_t);
/* quaternion */
quat_of_eulers(bfm.quat, bfm.eulers);
BoozFlighModelGetPos(bfm.pos_ltp);
/* extract speed in ltp frame from state */
BoozFlighModelGetSpeedLtp(bfm.speed_ltp);
@@ -188,8 +190,7 @@ static void booz_flight_model_update_byproducts(void) {
/* rotational accelerations */
/* quaternion */
}
sw/simulator/booz_flight_model_utils.c
+25
View File
@@ -39,6 +39,7 @@ void rk4(ode_fun f, VEC* x, VEC* u, double dt) {
}
MAT* dcm_of_eulers (VEC* eulers, MAT* dcm ) {
dcm = m_resize(dcm, 3,3);
@@ -63,6 +64,30 @@ MAT* dcm_of_eulers (VEC* eulers, MAT* dcm ) {
return dcm;
}
VEC* quat_of_eulers(VEC* quat, VEC* eulers) {
double phi2 = eulers->ve[EULER_PHI] / 2.0;
double theta2 = eulers->ve[EULER_THETA] / 2.0;
double psi2 = eulers->ve[EULER_PSI] / 2.0;
double sinphi2 = sin( phi2 );
double cosphi2 = cos( phi2 );
double sintheta2 = sin( theta2 );
double costheta2 = cos( theta2 );
double sinpsi2 = sin( psi2 );
double cospsi2 = cos( psi2 );
quat->ve[QUAT_QI] = cosphi2 * costheta2 * cospsi2 + sinphi2 * sintheta2 * sinpsi2;
quat->ve[QUAT_QX] = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2;
quat->ve[QUAT_QY] = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2;
quat->ve[QUAT_QZ] = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2;
return quat;
}
VEC* out_prod( VEC* a, VEC* b, VEC* out) {
if ( a->dim != 3 || b->dim != 3 )
error(E_SIZES,"out_prod");
sw/simulator/booz_flight_model_utils.h
+1
View File
@@ -8,6 +8,7 @@ typedef void (*ode_fun)(VEC* x, VEC* u, VEC* xdot);
extern void rk4(ode_fun f, VEC* x, VEC* u, double dt);
extern MAT* dcm_of_eulers (VEC* eulers, MAT* dcm );
extern VEC* quat_of_eulers(VEC* quat, VEC* eulers);
extern VEC* out_prod( VEC* a, VEC* b, VEC* out);
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_AUTO2; \
ppm_pulses[RADIO_MODE] = MODE_SWITCH_AUTO1; \
} \
ppm_pulses[RADIO_PITCH] = 1500 + 0. * (2050-950); \
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950); \
sw/simulator/booz_sensors_model_params_booz2_a1.h
+13 -9
View File
@@ -45,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 1.e-1
#define BSM_ACCEL_NOISE_STD_DEV_Y 1.e-1
#define BSM_ACCEL_NOISE_STD_DEV_Z 1.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
@@ -76,9 +76,13 @@
#define BSM_GYRO_NEUTRAL_Q 33417
#define BSM_GYRO_NEUTRAL_R 32809
#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.5)
#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.5)
#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.5)
#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.)
#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.)
#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.)
//#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(0.5)
//#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.5)
//#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(0.5)
#define BSM_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0)
#define BSM_GYRO_BIAS_INITIAL_Q RadOfDeg( .0)