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 28 Packages Projects Releases Wiki Activity

changed units for IMU alignement in airframe file

Browse Source 
was
 <define name="BODY_TO_IMU_PHI"   value="ANGLE_BFP_OF_REAL(RadOfDeg(0.))"/>
is now
 <define name="BODY_TO_IMU_PHI"   value="RadOfDeg(0.)"/>
This commit is contained in:
Antoine Drouin
2010-03-13 22:59:46 +00:00
parent 8ea32deec6
commit 2580e321cd
12 changed files with 84 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz2_a1.xml
+8 -8
View File
@@ -76,13 +76,13 @@
<define name="MAG_X_SENS" value="-3.4936416" integer="16"/>
<define name="MAG_Y_SENS" value=" 3.607713" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="ANGLE_BFP_OF_REAL(RadOfDeg(4.))"/>
<define name="BODY_TO_IMU_THETA" value="ANGLE_BFP_OF_REAL(RadOfDeg(3.))"/>
<define name="BODY_TO_IMU_PSI" value="ANGLE_BFP_OF_REAL(RadOfDeg(0.))"/>
<define name="BODY_TO_IMU_PHI" value="RadOfDeg(4.)"/>
<define name="BODY_TO_IMU_THETA" value="RadOfDeg(3.)"/>
<define name="BODY_TO_IMU_PSI" value="RadOfDeg(0.)"/>
<!--
<define name="BODY_TO_IMU_PHI" value="ANGLE_BFP_OF_REAL(RadOfDeg(0.))"/>
<define name="BODY_TO_IMU_THETA" value="ANGLE_BFP_OF_REAL(RadOfDeg(0.))"/>
<define name="BODY_TO_IMU_PSI" value="ANGLE_BFP_OF_REAL(RadOfDeg(0.))"/>
<define name="BODY_TO_IMU_PHI" value="RadOfDeg(0.)"/>
<define name="BODY_TO_IMU_THETA" value="RadOfDeg(0.)"/>
<define name="BODY_TO_IMU_PSI" value="RadOfDeg(0.)"/>
-->
</section>
@@ -161,7 +161,7 @@
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
<define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
<define name="INV_M" value ="0.2"/>
<!-- <define name="INV_M" value ="0.2"/> -->
</section>
@@ -179,7 +179,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_DIRECT"/>
<define name="BOOZ2_MODE_AUTO1" value="BOOZ2_AP_MODE_ATTITUDE_RC_CLIMB"/>
<define name="BOOZ2_MODE_AUTO2" value="BOOZ2_AP_MODE_ATTITUDE_Z_HOLD"/>
</section>
conf/airframes/booz2_a2.xml
+3 -3
View File
@@ -52,9 +52,9 @@
<define name="MAG_Y_SENS" value="-4.2959321" integer="16"/>
<define name="MAG_Z_SENS" value="-4.0988408" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="ANGLE_BFP_OF_REAL(RadOfDeg( 1.5))"/>
<define name="BODY_TO_IMU_THETA" value="ANGLE_BFP_OF_REAL(RadOfDeg(-1.5))"/>
<define name="BODY_TO_IMU_PSI" value="ANGLE_BFP_OF_REAL(RadOfDeg(0.))"/>
<define name="BODY_TO_IMU_PHI" value="RadOfDeg( 1.5)"/>
<define name="BODY_TO_IMU_THETA" value="RadOfDeg(-1.5)"/>
<define name="BODY_TO_IMU_PSI" value="RadOfDeg( 0. )"/>
</section>
conf/airframes/booz2_a3.xml
+3 -3
View File
@@ -52,9 +52,9 @@
<define name="MAG_Y_SENS" value="20." integer="16"/>
<define name="MAG_Z_SENS" value="15." integer="16"/>
<define name="BODY_TO_IMU_PHI" value="ANGLE_BFP_OF_REAL(RadOfDeg( 14.5))"/>
<define name="BODY_TO_IMU_THETA" value="ANGLE_BFP_OF_REAL(RadOfDeg(-8.5))"/>
<define name="BODY_TO_IMU_PSI" value="ANGLE_BFP_OF_REAL(RadOfDeg(-45.))"/>
<define name="BODY_TO_IMU_PHI" value="RadOfDeg( 14.5)"/>
<define name="BODY_TO_IMU_THETA" value="RadOfDeg( -8.5)"/>
<define name="BODY_TO_IMU_PSI" value="RadOfDeg(-45. )"/>
</section>
conf/airframes/booz2_a4.xml
+3 -3
View File
@@ -54,9 +54,9 @@
<define name="MAG_X_SENS" value="-3.03386034" integer="16"/>
<define name="MAG_Y_SENS" value=" 4.99085017" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="ANGLE_BFP_OF_REAL(RadOfDeg( 0.))"/>
<define name="BODY_TO_IMU_THETA" value="ANGLE_BFP_OF_REAL(RadOfDeg( 0.))"/>
<define name="BODY_TO_IMU_PSI" value="ANGLE_BFP_OF_REAL(RadOfDeg( 0.))"/>
<define name="BODY_TO_IMU_PHI" value="RadOfDeg( 0.)"/>
<define name="BODY_TO_IMU_THETA" value="RadOfDeg( 0.)"/>
<define name="BODY_TO_IMU_PSI" value="RadOfDeg( 0.)"/>
</section>
conf/airframes/booz2_a5.xml
+3 -3
View File
@@ -75,9 +75,9 @@
<define name="MAG_Y_NEUTRAL" value="24"/>
<define name="MAG_Z_NEUTRAL" value="28"/>
<define name="BODY_TO_IMU_PHI" value="ANGLE_BFP_OF_REAL(RadOfDeg(0.))"/>
<define name="BODY_TO_IMU_THETA" value="ANGLE_BFP_OF_REAL(RadOfDeg(0.))"/>
<define name="BODY_TO_IMU_PSI" value="ANGLE_BFP_OF_REAL(RadOfDeg(-45.))"/>
<define name="BODY_TO_IMU_PHI" value="RadOfDeg( 0.)"/>
<define name="BODY_TO_IMU_THETA" value="RadOfDeg( 0.)"/>
<define name="BODY_TO_IMU_PSI" value="RadOfDeg(-45.)"/>
</section>
conf/autopilot/subsystems/booz_ahrs_mlkf.makefile
+14
View File
@@ -0,0 +1,14 @@
#
#
#
ap.CFLAGS += -DAHRS_ALIGNER_LED=3
ap.srcs += $(SRC_BOOZ)/booz_ahrs.c
ap.srcs += $(SRC_BOOZ)/ahrs/booz_ahrs_aligner.c
ap.srcs += $(SRC_BOOZ_PRIV)/ahrs/booz_ahrs_mlkf.c
sim.CFLAGS += -I$(SRC_BOOZ_PRIV)
sim.CFLAGS += -DAHRS_ALIGNER_LED=3
sim.srcs += $(SRC_BOOZ)/booz_ahrs.c
sim.srcs += $(SRC_BOOZ)/ahrs/booz_ahrs_aligner.c
sim.srcs += $(SRC_BOOZ_PRIV)/ahrs/booz_ahrs_mlkf.c
conf/simulator/nps/nps_sensors_params_booz2_a1.h
+4 -4
View File
@@ -27,10 +27,10 @@
#include "airframe.h"
#if 0
#define NPS_BODY_TO_IMU_PHI RadOfDeg(4.)
#define NPS_BODY_TO_IMU_THETA RadOfDeg(3.)
#define NPS_BODY_TO_IMU_PSI RadOfDeg(0.)
#if 1
#define NPS_BODY_TO_IMU_PHI IMU_BODY_TO_IMU_PHI
#define NPS_BODY_TO_IMU_THETA IMU_BODY_TO_IMU_THETA
#define NPS_BODY_TO_IMU_PSI IMU_BODY_TO_IMU_PSI
#else
#define NPS_BODY_TO_IMU_PHI RadOfDeg(0.)
#define NPS_BODY_TO_IMU_THETA RadOfDeg(0.)
sw/airborne/booz/ahrs/booz2_filter_attitude_cmpl_euler.c
+5 -1
View File
@@ -196,9 +196,13 @@ void booz_ahrs_propagate(void) {
}
void booz_ahrs_update(void) {
void booz_ahrs_update_accel(void) {
}
void booz_ahrs_update_mag(void) {
}
sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c
+10 -1
View File
@@ -35,6 +35,8 @@
#define BAFL_DEBUG
static void booz_ahrs_do_update_accel(void);
static void booz_ahrs_do_update_mag(void);
/* our estimated attitude (ltp <-> imu) */
@@ -403,8 +405,11 @@ void booz_ahrs_propagate(void) {
#endif
}
void booz_ahrs_update_accel(void) {
RunOnceEvery(50, booz_ahrs_do_update_accel());
}
static void booz_ahrs_do_update_accel(void) {
int i, j, k;
#ifdef BAFL_DEBUG2
@@ -632,6 +637,10 @@ void booz_ahrs_update_accel(void) {
void booz_ahrs_update_mag(void) {
RunOnceEvery(10, booz_ahrs_do_update_mag());
}
static void booz_ahrs_update_mag(void) {
int i, j, k;
#ifdef BAFL_DEBUG2
sw/airborne/booz/booz_ahrs.h
+24 -14
View File
@@ -33,43 +33,53 @@
#define BOOZ_AHRS_RUNNING 1
struct BoozAhrs {
struct Int32Eulers ltp_to_body_euler;
struct Int32Quat ltp_to_body_quat;
struct Int32RMat ltp_to_body_rmat;
struct Int32Quat ltp_to_imu_quat;
struct Int32Eulers ltp_to_imu_euler;
struct Int32Quat ltp_to_imu_quat;
struct Int32RMat ltp_to_imu_rmat;
struct Int32Rates imu_rate;
struct Int32Rates body_rate;
struct Int32RMat ltp_to_imu_rmat;
struct Int32Rates imu_rate;
struct Int32Quat ltp_to_body_quat;
struct Int32Eulers ltp_to_body_euler;
struct Int32RMat ltp_to_body_rmat;
struct Int32Rates body_rate;
uint8_t status;
};
struct BoozAhrsFloat {
struct FloatQuat ltp_to_imu_quat;
struct FloatEulers ltp_to_imu_euler;
struct FloatRMat ltp_to_imu_rmat;
struct FloatRates imu_rate;
struct FloatQuat ltp_to_body_quat;
struct FloatEulers ltp_to_body_euler;
struct FloatRMat ltp_to_body_rmat;
struct FloatRates body_rate;
uint8_t status;
};
extern struct BoozAhrs booz_ahrs;
extern struct BoozAhrsFloat booz_ahrs_float;
#define BOOZ_AHRS_FLOAT_OF_INT32() { \
QUAT_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_quat, booz_ahrs.ltp_to_body_quat); \
RATES_FLOAT_OF_BFP(booz_ahrs_float.body_rate, booz_ahrs.body_rate); \
booz_ahrs_float.ltp_to_body_euler.phi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.phi); \
booz_ahrs_float.ltp_to_body_euler.theta = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.theta); \
booz_ahrs_float.ltp_to_body_euler.psi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.psi); \
#define BOOZ_AHRS_FLOAT_OF_INT32() { \
QUAT_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_quat, booz_ahrs.ltp_to_body_quat); \
EULERS_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_euler, booz_ahrs.ltp_to_body_euler); \
RATES_FLOAT_OF_BFP(booz_ahrs_float.body_rate, booz_ahrs.body_rate); \
}
#define BOOZ_AHRS_INT_OF_FLOAT() { \
QUAT_BFP_OF_REAL(booz_ahrs.ltp_to_body_quat, booz_ahrs_float.ltp_to_body_quat); \
EULERS_BFP_OF_REAL(booz_ahrs.ltp_to_body_euler, booz_ahrs_float.ltp_to_body_euler); \
RMAT_BFP_OF_REAL(booz_ahrs.ltp_to_body_rmat, booz_ahrs_float.ltp_to_body_rmat); \
RATES_BFP_OF_REAL(booz_ahrs.body_rate, booz_ahrs_float.body_rate); \
}
extern void booz_ahrs_init(void);
extern void booz_ahrs_align(void);
extern void booz_ahrs_propagate(void);
extern void booz_ahrs_update(void);
extern void booz_ahrs_update_accel(void);
extern void booz_ahrs_update_mag(void);
sw/airborne/booz/booz_imu.c
+4 -1
View File
@@ -38,7 +38,10 @@ void booz_imu_init(void) {
Compute quaternion and rotation matrix
for conversions between body and imu frame
*/
struct Int32Eulers body_to_imu_eulers = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
struct Int32Eulers body_to_imu_eulers =
{ ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI),
ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA),
ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) };
INT32_QUAT_OF_EULERS(booz_imu.body_to_imu_quat, body_to_imu_eulers);
INT32_QUAT_NORMALISE(booz_imu.body_to_imu_quat);
INT32_RMAT_OF_EULERS(booz_imu.body_to_imu_rmat, body_to_imu_eulers);
sw/airborne/booz/fms/booz_fms_test_signal.c
+3
View File
@@ -63,6 +63,9 @@ void booz_fms_impl_periodic(void) {
}
}
break;
case STEP_PITCH:
case STEP_VERT:
break;
#if 0
case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: {
if (booz2_guidance_v_mode < BOOZ2_GUIDANCE_V_MODE_HOVER)