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

added mag BFP <-> FLOAT conversion

Browse Source
This commit is contained in:
Felix Ruess
2009-07-27 17:29:46 +00:00
parent a26829369b
commit 450854b9f8
3 changed files with 28 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c
+2 -79
View File
@@ -189,33 +189,6 @@ void booz_ahrs_propagate(void) {
*
*/
// /* Wxq(pqr) Omega matrix */
// bafl_qom[0][0] = bafl_qom[1][1] = bafl_qom[2][2] = bafl_qom[3][3] = 0;
// bafl_qom[1][0] = bafl_qom[2][3] = bafl_rates.p * 0.5;
// bafl_qom[2][0] = bafl_qom[3][1] = bafl_rates.q * 0.5;
// bafl_qom[3][0] = bafl_qom[1][2] = bafl_rates.r * 0.5;
//
// bafl_qom[0][1] = bafl_qom[3][2] = -bafl_qom[1][0];
// bafl_qom[0][2] = bafl_qom[1][3] = -bafl_qom[2][0];
// bafl_qom[0][3] = bafl_qom[2][1] = -bafl_qom[3][0];
//
// /* quat_dot = Wxq(pqr) * quat */
// bafl_qdot.qi= bafl_qom[0][1]*bafl_quat.qx+bafl_qom[0][2]*bafl_quat.qy+bafl_qom[0][3] * bafl_quat.qz;
// bafl_qdot.qx= bafl_qom[1][0]*bafl_quat.qi +bafl_qom[1][2]*bafl_quat.qy+bafl_qom[1][3] * bafl_quat.qz;
// bafl_qdot.qy= bafl_qom[2][0]*bafl_quat.qi+bafl_qom[2][1]*bafl_quat.qx +bafl_qom[2][3] * bafl_quat.qz;
// bafl_qdot.qz= bafl_qom[3][0]*bafl_quat.qi+bafl_qom[3][1]*bafl_quat.qx+bafl_qom[3][2]*bafl_quat.qy ;
//
// /* propagate quaternion */
// bafl_quat.qi += bafl_qdot.qi * BAFL_DT;
// bafl_quat.qx += bafl_qdot.qx * BAFL_DT;
// bafl_quat.qy += bafl_qdot.qy * BAFL_DT;
// bafl_quat.qz += bafl_qdot.qz * BAFL_DT;
/*
* Makro version
*/
/* compute qdot and normalize it */
FLOAT_QUAT_DERIVATIVE_LAGRANGE(bafl_qdot, bafl_rates, bafl_quat);
@@ -255,17 +228,7 @@ void booz_ahrs_propagate(void) {
bafl_T[i][j+3] = - RMAT_ELMT(bafl_dcm, j, i); /* inverted bafl_dcm */
}
}
/*
bafl_T[0][3] = - RMAT_ELMT(bafl_dcm, 0, 0);
bafl_T[0][4] = - RMAT_ELMT(bafl_dcm, 0, 1);
bafl_T[0][5] = - RMAT_ELMT(bafl_dcm, 0, 2);
bafl_T[1][3] = - RMAT_ELMT(bafl_dcm, 1, 0);
bafl_T[1][4] = - RMAT_ELMT(bafl_dcm, 1, 1);
bafl_T[1][5] = - RMAT_ELMT(bafl_dcm, 1, 2);
bafl_T[2][3] = - RMAT_ELMT(bafl_dcm, 2, 0);
bafl_T[2][4] = - RMAT_ELMT(bafl_dcm, 2, 1);
bafl_T[2][5] = - RMAT_ELMT(bafl_dcm, 2, 2);
*/
/*
* estimate the a priori error covariance matrix P_prio = T * P * T_T + Q
@@ -347,26 +310,6 @@ void booz_ahrs_update_accel(void) {
}
}
}
/*bafl_tempS[0][0] = bafl_H[0][0]*bafl_Pprio[0][0] + bafl_H[0][1]*bafl_Pprio[1][0] + bafl_H[0][2]*bafl_Pprio[2][0];
bafl_tempS[0][1] = bafl_H[0][0]*bafl_Pprio[0][1] + bafl_H[0][1]*bafl_Pprio[1][1] + bafl_H[0][2]*bafl_Pprio[2][1];
bafl_tempS[0][2] = bafl_H[0][0]*bafl_Pprio[0][2] + bafl_H[0][1]*bafl_Pprio[1][2] + bafl_H[0][2]*bafl_Pprio[2][2];
bafl_tempS[0][3] = bafl_H[0][0]*bafl_Pprio[0][3] + bafl_H[0][1]*bafl_Pprio[1][3] + bafl_H[0][2]*bafl_Pprio[2][3];
bafl_tempS[0][4] = bafl_H[0][0]*bafl_Pprio[0][4] + bafl_H[0][1]*bafl_Pprio[1][4] + bafl_H[0][2]*bafl_Pprio[2][4];
bafl_tempS[0][5] = bafl_H[0][0]*bafl_Pprio[0][5] + bafl_H[0][1]*bafl_Pprio[1][5] + bafl_H[0][2]*bafl_Pprio[2][5];
bafl_tempS[1][0] = bafl_H[1][0]*bafl_Pprio[0][0] + bafl_H[1][1]*bafl_Pprio[1][0] + bafl_H[1][2]*bafl_Pprio[2][0];
bafl_tempS[1][1] = bafl_H[1][0]*bafl_Pprio[0][1] + bafl_H[1][1]*bafl_Pprio[1][1] + bafl_H[1][2]*bafl_Pprio[2][1];
bafl_tempS[1][2] = bafl_H[1][0]*bafl_Pprio[0][2] + bafl_H[1][1]*bafl_Pprio[1][2] + bafl_H[1][2]*bafl_Pprio[2][2];
bafl_tempS[1][3] = bafl_H[1][0]*bafl_Pprio[0][3] + bafl_H[1][1]*bafl_Pprio[1][3] + bafl_H[1][2]*bafl_Pprio[2][3];
bafl_tempS[1][4] = bafl_H[1][0]*bafl_Pprio[0][4] + bafl_H[1][1]*bafl_Pprio[1][4] + bafl_H[1][2]*bafl_Pprio[2][4];
bafl_tempS[1][5] = bafl_H[1][0]*bafl_Pprio[0][5] + bafl_H[1][1]*bafl_Pprio[1][5] + bafl_H[1][2]*bafl_Pprio[2][5];
bafl_tempS[2][0] = bafl_H[2][0]*bafl_Pprio[0][0] + bafl_H[2][1]*bafl_Pprio[1][0] + bafl_H[2][2]*bafl_Pprio[2][0];
bafl_tempS[2][1] = bafl_H[2][0]*bafl_Pprio[0][1] + bafl_H[2][1]*bafl_Pprio[1][1] + bafl_H[2][2]*bafl_Pprio[2][1];
bafl_tempS[2][2] = bafl_H[2][0]*bafl_Pprio[0][2] + bafl_H[2][1]*bafl_Pprio[1][2] + bafl_H[2][2]*bafl_Pprio[2][2];
bafl_tempS[2][3] = bafl_H[2][0]*bafl_Pprio[0][3] + bafl_H[2][1]*bafl_Pprio[1][3] + bafl_H[2][2]*bafl_Pprio[2][3];
bafl_tempS[2][4] = bafl_H[2][0]*bafl_Pprio[0][4] + bafl_H[2][1]*bafl_Pprio[1][4] + bafl_H[2][2]*bafl_Pprio[2][4];
bafl_tempS[2][5] = bafl_H[2][0]*bafl_Pprio[0][5] + bafl_H[2][1]*bafl_Pprio[1][5] + bafl_H[2][2]*bafl_Pprio[2][5];*/
/* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
@@ -545,7 +488,7 @@ void booz_ahrs_update_mag(void) {
int i, j, k;
//MAG_FLOAT_OF_BFP(bafl_mag, booz_imu.mag);
MAGS_FLOAT_OF_BFP(bafl_mag, booz_imu.mag);
/* P_prio = P */
for ( i=0; i<BAFL_SSIZE; i++ ) {
@@ -591,26 +534,6 @@ void booz_ahrs_update_mag(void) {
}
}
}
/*bafl_tempS[0][0] = bafl_H[0][0]*bafl_Pprio[0][0] + bafl_H[0][1]*bafl_Pprio[1][0] + bafl_H[0][2]*bafl_Pprio[2][0];
bafl_tempS[0][1] = bafl_H[0][0]*bafl_Pprio[0][1] + bafl_H[0][1]*bafl_Pprio[1][1] + bafl_H[0][2]*bafl_Pprio[2][1];
bafl_tempS[0][2] = bafl_H[0][0]*bafl_Pprio[0][2] + bafl_H[0][1]*bafl_Pprio[1][2] + bafl_H[0][2]*bafl_Pprio[2][2];
bafl_tempS[0][3] = bafl_H[0][0]*bafl_Pprio[0][3] + bafl_H[0][1]*bafl_Pprio[1][3] + bafl_H[0][2]*bafl_Pprio[2][3];
bafl_tempS[0][4] = bafl_H[0][0]*bafl_Pprio[0][4] + bafl_H[0][1]*bafl_Pprio[1][4] + bafl_H[0][2]*bafl_Pprio[2][4];
bafl_tempS[0][5] = bafl_H[0][0]*bafl_Pprio[0][5] + bafl_H[0][1]*bafl_Pprio[1][5] + bafl_H[0][2]*bafl_Pprio[2][5];
bafl_tempS[1][0] = bafl_H[1][0]*bafl_Pprio[0][0] + bafl_H[1][1]*bafl_Pprio[1][0] + bafl_H[1][2]*bafl_Pprio[2][0];
bafl_tempS[1][1] = bafl_H[1][0]*bafl_Pprio[0][1] + bafl_H[1][1]*bafl_Pprio[1][1] + bafl_H[1][2]*bafl_Pprio[2][1];
bafl_tempS[1][2] = bafl_H[1][0]*bafl_Pprio[0][2] + bafl_H[1][1]*bafl_Pprio[1][2] + bafl_H[1][2]*bafl_Pprio[2][2];
bafl_tempS[1][3] = bafl_H[1][0]*bafl_Pprio[0][3] + bafl_H[1][1]*bafl_Pprio[1][3] + bafl_H[1][2]*bafl_Pprio[2][3];
bafl_tempS[1][4] = bafl_H[1][0]*bafl_Pprio[0][4] + bafl_H[1][1]*bafl_Pprio[1][4] + bafl_H[1][2]*bafl_Pprio[2][4];
bafl_tempS[1][5] = bafl_H[1][0]*bafl_Pprio[0][5] + bafl_H[1][1]*bafl_Pprio[1][5] + bafl_H[1][2]*bafl_Pprio[2][5];
bafl_tempS[2][0] = bafl_H[2][0]*bafl_Pprio[0][0] + bafl_H[2][1]*bafl_Pprio[1][0] + bafl_H[2][2]*bafl_Pprio[2][0];
bafl_tempS[2][1] = bafl_H[2][0]*bafl_Pprio[0][1] + bafl_H[2][1]*bafl_Pprio[1][1] + bafl_H[2][2]*bafl_Pprio[2][1];
bafl_tempS[2][2] = bafl_H[2][0]*bafl_Pprio[0][2] + bafl_H[2][1]*bafl_Pprio[1][2] + bafl_H[2][2]*bafl_Pprio[2][2];
bafl_tempS[2][3] = bafl_H[2][0]*bafl_Pprio[0][3] + bafl_H[2][1]*bafl_Pprio[1][3] + bafl_H[2][2]*bafl_Pprio[2][3];
bafl_tempS[2][4] = bafl_H[2][0]*bafl_Pprio[0][4] + bafl_H[2][1]*bafl_Pprio[1][4] + bafl_H[2][2]*bafl_Pprio[2][4];
bafl_tempS[2][5] = bafl_H[2][0]*bafl_Pprio[0][5] + bafl_H[2][1]*bafl_Pprio[1][5] + bafl_H[2][2]*bafl_Pprio[2][5];*/
/* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
sw/airborne/math/pprz_algebra.h
+18
View File
@@ -516,5 +516,23 @@
(_ef).z = ACCEL_FLOAT_OF_BFP((_ei).z); \
}
#define ACCELS_BFP_OF_REAL(_ef, _ei) { \
(_ef).x = ACCEL_BFP_OF_REAL((_ei).x); \
(_ef).y = ACCEL_BFP_OF_REAL((_ei).y); \
(_ef).z = ACCEL_BFP_OF_REAL((_ei).z); \
}
#define MAGS_FLOAT_OF_BFP(_ef, _ei) { \
(_ef).x = ACCEL_FLOAT_OF_BFP((_ei).x); \
(_ef).y = ACCEL_FLOAT_OF_BFP((_ei).y); \
(_ef).z = ACCEL_FLOAT_OF_BFP((_ei).z); \
}
#define MAGS_BFP_OF_REAL(_ef, _ei) { \
(_ef).x = ACCEL_BFP_OF_REAL((_ei).x); \
(_ef).y = ACCEL_BFP_OF_REAL((_ei).y); \
(_ef).z = ACCEL_BFP_OF_REAL((_ei).z); \
}
#endif /* PPRZ_ALGEBRA_H */
sw/airborne/math/pprz_algebra_int.h
+8 -4
View File
@@ -55,6 +55,8 @@ struct Int16Vect3 {
#define INT32_MAG_FRAC 11
#define INT32_ACCEL_FRAC 10
#define INT32_MAG_FRAC 11
struct Int32Vect2 {
int32_t x;
int32_t y;
@@ -138,20 +140,22 @@ struct Int64Vect3 {
#define BFP_OF_REAL(_vr, _frac) ((_vr)*(1<<(_frac)))
#define FLOAT_OF_BFP(_vbfp, _frac) ((float)(_vbfp)/(1<<(_frac)))
#define RATE_BFP_OF_REAL(_af) BFP_OF_REAL(_af, INT32_RATE_FRAC)
#define RATE_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_RATE_FRAC)
#define RATE_BFP_OF_REAL(_af) BFP_OF_REAL(_af, INT32_RATE_FRAC)
#define RATE_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_RATE_FRAC)
#define ANGLE_BFP_OF_REAL(_af) BFP_OF_REAL(_af, INT32_ANGLE_FRAC)
#define ANGLE_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_ANGLE_FRAC)
#define QUAT1_BFP_OF_REAL(_qf) BFP_OF_REAL(_qf, INT32_QUAT_FRAC)
#define QUAT1_FLOAT_OF_BFP(_qi) FLOAT_OF_BFP(_qi, INT32_QUAT_FRAC)
#define TRIG_BFP_OF_REAL(_tf) BFP_OF_REAL(_tf, INT32_TRIG_FRAC)
#define TRIG_FLOAT_OF_BFP(_ti) FLOAT_OF_BFP(_ti,INT32_TRIG_FRAC)
#define POS_BFP_OF_REAL(_af) BFP_OF_REAL(_af, INT32_POS_FRAC)
#define POS_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_POS_FRAC)
#define POS_BFP_OF_REAL(_af) BFP_OF_REAL(_af, INT32_POS_FRAC)
#define POS_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_POS_FRAC)
#define SPEED_BFP_OF_REAL(_af) BFP_OF_REAL(_af, INT32_SPEED_FRAC)
#define SPEED_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_SPEED_FRAC)
#define ACCEL_BFP_OF_REAL(_af) BFP_OF_REAL(_af, INT32_ACCEL_FRAC)
#define ACCEL_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_ACCEL_FRAC)
#define MAG_BFP_OF_REAL(_af) BFP_OF_REAL(_af, INT32_MAG_FRAC)
#define MAG_FLOAT_OF_BFP(_ai) FLOAT_OF_BFP((_ai), INT32_MAG_FRAC)
#define INT_MULT_RSHIFT(_a, _b, _r) (((_a)*(_b))>>(_r))
/*