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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2009-02-27 13:19:52 +00:00
parent 98e8c90be6
commit 3dbf9eb210
6 changed files with 92 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz2_a1.xml
+2 -2
View File
@@ -141,7 +141,7 @@
<section name="AUTOPILOT">
<define name="BOOZ2_MODE_MANUAL" value="BOOZ2_AP_MODE_ATTITUDE_DIRECT"/>
<define name="BOOZ2_MODE_AUTO1" value="BOOZ2_AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="BOOZ2_MODE_AUTO1" value="BOOZ2_AP_MODE_ATTITUDE_RC_CLIMB"/>
<define name="BOOZ2_MODE_AUTO2" value="BOOZ2_AP_MODE_HOVER_Z_HOLD"/>
</section>
@@ -152,7 +152,7 @@
<makefile>
# prevents motors from ever starting
ap.CFLAGS += -DKILL_MOTORS
#ap.CFLAGS += -DKILL_MOTORS
include $(PAPARAZZI_SRC)/conf/autopilot/booz2_autopilot.makefile
include $(PAPARAZZI_SRC)/conf/autopilot/booz2_test_progs.makefile
conf/airframes/booz2_a2.xml
+5 -5
View File
@@ -52,9 +52,9 @@
<define name="MAG_Y_SENS" value="5.10207664" integer="16"/>
<define name="MAG_Z_SENS" value="-4.90788848" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="ANGLE_BFP(RadOfDeg(10.66))"/>
<define name="BODY_TO_IMU_THETA" value="ANGLE_BFP(RadOfDeg(-0.7))"/>
<define name="BODY_TO_IMU_PSI" value="ANGLE_BFP(RadOfDeg(0.))"/>
<define name="BODY_TO_IMU_PHI" value="ANGLE_BFP_OF_REAL(RadOfDeg(10.66))"/>
<define name="BODY_TO_IMU_THETA" value="ANGLE_BFP_OF_REAL(RadOfDeg(-0.7))"/>
<define name="BODY_TO_IMU_PSI" value="ANGLE_BFP_OF_REAL(RadOfDeg(0.))"/>
</section>
@@ -106,8 +106,8 @@
<define name="MAX_ERR_Z" value="BOOZ_POS_I_OF_F( 10.)"/>
<define name="MIN_ERR_ZD" value="BOOZ_SPEED_I_OF_F(-10.)"/>
<define name="MAX_ERR_ZD" value="BOOZ_SPEED_I_OF_F( 10.)"/>
<define name="HOVER_KP" value="-300"/>
<define name="HOVER_KD" value="-150"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-250"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
sw/airborne/booz/booz2_filter_attitude_cmpl_euler.c
+1 -1
View File
@@ -165,7 +165,7 @@ void booz_ahrs_propagate(void) {
INT32_RMAT_OF_EULERS(booz_ahrs.ltp_to_imu_rmat, booz_ahrs.ltp_to_imu_euler);
/* Compute LTP to BODY quaternion */
INT32_QUAT_DIV(booz_ahrs.ltp_to_body_quat, booz_imu.body_to_imu_quat, booz_ahrs.ltp_to_imu_quat);
INT32_QUAT_COMP_INV(booz_ahrs.ltp_to_body_quat, booz_imu.body_to_imu_quat, booz_ahrs.ltp_to_imu_quat);
/* Compute LTP to BODY rotation matrix */
INT32_RMAT_COMP_INV(booz_ahrs.ltp_to_body_rmat, booz_ahrs.ltp_to_imu_rmat, booz_imu.body_to_imu_rmat);
/* compute LTP to BODY eulers */
sw/airborne/pprz_algebra_float.h
+11 -15
View File
@@ -168,25 +168,21 @@ struct FloatRates {
}
/* _a2c = _a2b comp _b2c , aka _a2c = _b2c * _a2b */
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c) FLOAT_QUAT_MULT(_a2c, _a2b, _b2c)
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c) { \
(_a2c).qi = (_a2b).qi*(_b2c).qi - (_a2b).qx*(_b2c).qx - (_a2b).qy*(_b2c).qy - (_a2b).qz*(_b2c).qz; \
(_a2c).qx = (_a2b).qi*(_b2c).qx + (_a2b).qx*(_b2c).qi + (_a2b).qy*(_b2c).qz - (_a2b).qz*(_b2c).qy; \
(_a2c).qy = (_a2b).qi*(_b2c).qy - (_a2b).qx*(_b2c).qz + (_a2b).qy*(_b2c).qi + (_a2b).qz*(_b2c).qx; \
(_a2c).qz = (_a2b).qi*(_b2c).qz + (_a2b).qx*(_b2c).qy - (_a2b).qy*(_b2c).qx + (_a2b).qz*(_b2c).qi; \
}
/* _a2b = _a2b comp_inv _b2c , aka _a2b = inv(_b2c) * _a2c */
#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c) FLOAT_QUAT_DIV(_a2b, _a2c, _b2c)
#define FLOAT_QUAT_MULT(_c, _a, _b) { \
(_c).qi = (_a).qi*(_b).qi - (_a).qx*(_b).qx - (_a).qy*(_b).qy - (_a).qz*(_b).qz; \
(_c).qx = (_a).qi*(_b).qx + (_a).qx*(_b).qi + (_a).qy*(_b).qz - (_a).qz*(_b).qy; \
(_c).qy = (_a).qi*(_b).qy - (_a).qx*(_b).qz + (_a).qy*(_b).qi + (_a).qz*(_b).qx; \
(_c).qz = (_a).qi*(_b).qz + (_a).qx*(_b).qy - (_a).qy*(_b).qx + (_a).qz*(_b).qi; \
#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c) { \
(_a2b).qi = (_a2c).qi*(_b2c).qi + (_a2c).qx*(_b2c).qx + (_a2c).qy*(_b2c).qy + (_a2c).qz*(_b2c).qz; \
(_a2b).qx = -(_a2c).qi*(_b2c).qx + (_a2c).qx*(_b2c).qi - (_a2c).qy*(_b2c).qz + (_a2c).qz*(_b2c).qy; \
(_a2b).qy = -(_a2c).qi*(_b2c).qy + (_a2c).qx*(_b2c).qz + (_a2c).qy*(_b2c).qi - (_a2c).qz*(_b2c).qx; \
(_a2b).qz = -(_a2c).qi*(_b2c).qz - (_a2c).qx*(_b2c).qy + (_a2c).qy*(_b2c).qx + (_a2c).qz*(_b2c).qi; \
}
#define FLOAT_QUAT_DIV(b, a, c) { \
b.qi = c.qi*a.qi + c.qx*a.qx + c.qy*a.qy + c.qz*a.qz; \
b.qx = c.qx*a.qi - c.qi*a.qx - c.qz*a.qy + c.qy*a.qz; \
b.qy = c.qy*a.qi + c.qz*a.qx - c.qi*a.qy - c.qx*a.qz; \
b.qz = c.qz*a.qi - c.qy*a.qx + c.qx*a.qy - c.qi*a.qz; \
}
#define FLOAT_QUAT_VMULT(v_out, q, v_in) { \
const float qi2 = q.qi*q.qi; \
sw/airborne/pprz_algebra_int.h
+15 -21
View File
@@ -287,34 +287,28 @@ struct Int64Vect3 {
#define INT32_QUAT_NORMALISE(q) { \
int32_t n; \
INT32_QUAT_NORM(n, q); \
q.qi = q.qi * QUAT1_BFP_OF_REAL(1) / n; \
q.qx = q.qx * QUAT1_BFP_OF_REAL(1) / n; \
q.qy = q.qy * QUAT1_BFP_OF_REAL(1) / n; \
q.qz = q.qz * QUAT1_BFP_OF_REAL(1) / n; \
q.qi = q.qi * QUAT1_BFP_OF_REAL(1) / n; \
q.qx = q.qx * QUAT1_BFP_OF_REAL(1) / n; \
q.qy = q.qy * QUAT1_BFP_OF_REAL(1) / n; \
q.qz = q.qz * QUAT1_BFP_OF_REAL(1) / n; \
}
/* _a2c = _a2b comp _b2c , aka _a2c = _b2c * _a2b */
#define INT32_QUAT_COMP(_a2c, _a2b, _b2c) INT32_QUAT_MULT(_a2c, _a2b, _b2c)
#define INT32_QUAT_COMP(_a2c, _a2b, _b2c) { \
(_a2c).qi = ((_a2b).qi*(_b2c).qi - (_a2b).qx*(_b2c).qx - (_a2b).qy*(_b2c).qy - (_a2b).qz*(_b2c).qz)>>INT32_QUAT_FRAC; \
(_a2c).qx = ((_a2b).qi*(_b2c).qx + (_a2b).qx*(_b2c).qi + (_a2b).qy*(_b2c).qz - (_a2b).qz*(_b2c).qy)>>INT32_QUAT_FRAC; \
(_a2c).qy = ((_a2b).qi*(_b2c).qy - (_a2b).qx*(_b2c).qz + (_a2b).qy*(_b2c).qi + (_a2b).qz*(_b2c).qx)>>INT32_QUAT_FRAC; \
(_a2c).qz = ((_a2b).qi*(_b2c).qz + (_a2b).qx*(_b2c).qy - (_a2b).qy*(_b2c).qx + (_a2b).qz*(_b2c).qi)>>INT32_QUAT_FRAC; \
}
/* _a2b = _a2b comp_inv _b2c , aka _a2b = inv(_b2c) * _a2c */
#define INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c) INT32_QUAT_DIV(_a2b, _a2c, _b2c)
#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)>>INT32_QUAT_FRAC; \
c.qx = (a.qi*b.qx + a.qx*b.qi + a.qy*b.qz - a.qz*b.qy)>>INT32_QUAT_FRAC; \
c.qy = (a.qi*b.qy - a.qx*b.qz + a.qy*b.qi + a.qz*b.qx)>>INT32_QUAT_FRAC; \
c.qz = (a.qi*b.qz + a.qx*b.qy - a.qy*b.qx + a.qz*b.qi)>>INT32_QUAT_FRAC; \
#define INT32_QUAT_COMP_INV(_a2b, _a2c, _b2c) { \
(_a2b).qi = ( (_a2c).qi*(_b2c).qi + (_a2c).qx*(_b2c).qx + (_a2c).qy*(_b2c).qy + (_a2c).qz*(_b2c).qz)>>INT32_QUAT_FRAC; \
(_a2b).qx = (-(_a2c).qi*(_b2c).qx + (_a2c).qx*(_b2c).qi - (_a2c).qy*(_b2c).qz + (_a2c).qz*(_b2c).qy)>>INT32_QUAT_FRAC; \
(_a2b).qy = (-(_a2c).qi*(_b2c).qy + (_a2c).qx*(_b2c).qz + (_a2c).qy*(_b2c).qi - (_a2c).qz*(_b2c).qx)>>INT32_QUAT_FRAC; \
(_a2b).qz = (-(_a2c).qi*(_b2c).qz - (_a2c).qx*(_b2c).qy + (_a2c).qy*(_b2c).qx + (_a2c).qz*(_b2c).qi)>>INT32_QUAT_FRAC; \
}
#define INT32_QUAT_DIV(b, a, c) { \
b.qi = (c.qi*a.qi + c.qx*a.qx + c.qy*a.qy + c.qz*a.qz)>>IQUAT_RES; \
b.qx = (c.qx*a.qi - c.qi*a.qx - c.qz*a.qy + c.qy*a.qz)>>IQUAT_RES; \
b.qy = (c.qy*a.qi + c.qz*a.qx - c.qi*a.qy - c.qx*a.qz)>>IQUAT_RES; \
b.qz = (c.qz*a.qi - c.qy*a.qx + c.qx*a.qy - c.qi*a.qz)>>IQUAT_RES; \
}
#define INT32_QUAT_VMULT(v_out, q, v_in) { \
const int32_t qi2 = (q.qi*q.qi)>>INT32_QUAT_FRAC; \
sw/airborne/test/test_algebra.c
+58 -58
View File
@@ -165,7 +165,7 @@ static void test_3(void) {
/* Compute LTP to BODY quaternion */
struct Int32Quat l2b_q;
INT32_QUAT_DIV(l2b_q, b2i_q, l2i_q);
INT32_QUAT_COMP_INV(l2b_q, b2i_q, l2i_q);
DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2b_q", l2b_q);
/* Compute LTP to BODY rotation matrix */
@@ -344,36 +344,6 @@ static void test_6(void) {
}
static void test_7(void) {
printf("\n");
struct FloatEulers ea2c;
EULERS_ASSIGN(ea2c, RadOfDeg(29.742755), RadOfDeg(-40.966522), RadOfDeg(69.467265));
DISPLAY_FLOAT_EULERS_DEG("ea2c", ea2c);
struct FloatEulers eb2c;
EULERS_ASSIGN(eb2c, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(90.));
DISPLAY_FLOAT_EULERS_DEG("eb2c", eb2c);
struct FloatRMat fa2c;
FLOAT_RMAT_OF_EULERS(fa2c, ea2c);
struct FloatRMat fb2c;
FLOAT_RMAT_OF_EULERS(fb2c, eb2c);
printf("\n");
test_rmat_comp_inv(fa2c, fb2c, 1);
struct FloatQuat qa2c;
FLOAT_QUAT_OF_EULERS(qa2c, ea2c);
struct FloatQuat qb2c;
FLOAT_QUAT_OF_EULERS(qb2c, eb2c);
printf("\n");
test_quat_comp_inv(qa2c, qb2c, 1);
printf("\n");
}
float test_rmat_comp(struct FloatRMat ma2b_f, struct FloatRMat mb2c_f, int display) {
@@ -402,33 +372,6 @@ float test_rmat_comp(struct FloatRMat ma2b_f, struct FloatRMat mb2c_f, int displ
}
float test_rmat_comp_inv(struct FloatRMat ma2c_f, struct FloatRMat mb2c_f, int display) {
struct FloatRMat ma2b_f;
FLOAT_RMAT_COMP_INV(ma2b_f, ma2c_f, mb2c_f);
struct Int32RMat ma2c_i;
RMAT_BFP_OF_REAL(ma2c_i, ma2c_f);
struct Int32RMat mb2c_i;
RMAT_BFP_OF_REAL(mb2c_i, mb2c_f);
struct Int32RMat ma2b_i;
INT32_RMAT_COMP_INV(ma2b_i, ma2c_i, mb2c_i);
struct FloatRMat err;
RMAT_DIFF(err, ma2b_f, ma2b_i);
float norm_err = FLOAT_RMAT_NORM(err);
if (display) {
printf("rmap comp_inv\n");
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("a2cf", ma2b_f);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("a2ci", ma2b_i);
}
return norm_err;
}
float test_quat_comp(struct FloatQuat qa2b_f, struct FloatQuat qb2c_f, int display) {
struct FloatQuat qa2c_f;
@@ -454,6 +397,63 @@ float test_quat_comp(struct FloatQuat qa2b_f, struct FloatQuat qb2c_f, int displ
}
static void test_7(void) {
printf("\n");
struct FloatEulers ea2c;
EULERS_ASSIGN(ea2c, RadOfDeg(29.742755), RadOfDeg(-40.966522), RadOfDeg(69.467265));
DISPLAY_FLOAT_EULERS_DEG("ea2c", ea2c);
struct FloatEulers eb2c;
EULERS_ASSIGN(eb2c, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(90.));
DISPLAY_FLOAT_EULERS_DEG("eb2c", eb2c);
struct FloatRMat fa2c;
FLOAT_RMAT_OF_EULERS(fa2c, ea2c);
struct FloatRMat fb2c;
FLOAT_RMAT_OF_EULERS(fb2c, eb2c);
printf("\n");
test_rmat_comp_inv(fa2c, fb2c, 1);
struct FloatQuat qa2c;
FLOAT_QUAT_OF_EULERS(qa2c, ea2c);
struct FloatQuat qb2c;
FLOAT_QUAT_OF_EULERS(qb2c, eb2c);
printf("\n");
test_quat_comp_inv(qa2c, qb2c, 1);
printf("\n");
}
float test_rmat_comp_inv(struct FloatRMat ma2c_f, struct FloatRMat mb2c_f, int display) {
struct FloatRMat ma2b_f;
FLOAT_RMAT_COMP_INV(ma2b_f, ma2c_f, mb2c_f);
struct Int32RMat ma2c_i;
RMAT_BFP_OF_REAL(ma2c_i, ma2c_f);
struct Int32RMat mb2c_i;
RMAT_BFP_OF_REAL(mb2c_i, mb2c_f);
struct Int32RMat ma2b_i;
INT32_RMAT_COMP_INV(ma2b_i, ma2c_i, mb2c_i);
struct FloatRMat err;
RMAT_DIFF(err, ma2b_f, ma2b_i);
float norm_err = FLOAT_RMAT_NORM(err);
if (display) {
printf("rmap comp_inv\n");
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("a2cf", ma2b_f);
DISPLAY_INT32_RMAT_AS_EULERS_DEG("a2ci", ma2b_i);
}
return norm_err;
}
float test_quat_comp_inv(struct FloatQuat qa2c_f, struct FloatQuat qb2c_f, int display) {
struct FloatQuat qa2b_f;