[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4675] changed units for IMU alignement in airframe
|
From: |
antoine drouin |
|
Subject: |
[paparazzi-commits] [4675] changed units for IMU alignement in airframe file |
|
Date: |
Sat, 13 Mar 2010 22:59:46 +0000 |
Revision: 4675
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4675
Author: poine
Date: 2010-03-13 22:59:46 +0000 (Sat, 13 Mar 2010)
Log Message:
-----------
changed units for IMU alignement in airframe file
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.)"/>
Modified Paths:
--------------
paparazzi3/trunk/conf/airframes/booz2_a1.xml
paparazzi3/trunk/conf/airframes/booz2_a2.xml
paparazzi3/trunk/conf/airframes/booz2_a3.xml
paparazzi3/trunk/conf/airframes/booz2_a4.xml
paparazzi3/trunk/conf/airframes/booz2_a5.xml
paparazzi3/trunk/conf/simulator/nps/nps_sensors_params_booz2_a1.h
paparazzi3/trunk/sw/airborne/booz/ahrs/booz2_filter_attitude_cmpl_euler.c
paparazzi3/trunk/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c
paparazzi3/trunk/sw/airborne/booz/booz_ahrs.h
paparazzi3/trunk/sw/airborne/booz/booz_imu.c
paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_test_signal.c
Added Paths:
-----------
paparazzi3/trunk/conf/autopilot/subsystems/booz_ahrs_mlkf.makefile
Modified: paparazzi3/trunk/conf/airframes/booz2_a1.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_a1.xml 2010-03-13 00:51:28 UTC
(rev 4674)
+++ paparazzi3/trunk/conf/airframes/booz2_a1.xml 2010-03-13 22:59:46 UTC
(rev 4675)
@@ -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>
Modified: paparazzi3/trunk/conf/airframes/booz2_a2.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_a2.xml 2010-03-13 00:51:28 UTC
(rev 4674)
+++ paparazzi3/trunk/conf/airframes/booz2_a2.xml 2010-03-13 22:59:46 UTC
(rev 4675)
@@ -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>
Modified: paparazzi3/trunk/conf/airframes/booz2_a3.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_a3.xml 2010-03-13 00:51:28 UTC
(rev 4674)
+++ paparazzi3/trunk/conf/airframes/booz2_a3.xml 2010-03-13 22:59:46 UTC
(rev 4675)
@@ -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>
Modified: paparazzi3/trunk/conf/airframes/booz2_a4.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_a4.xml 2010-03-13 00:51:28 UTC
(rev 4674)
+++ paparazzi3/trunk/conf/airframes/booz2_a4.xml 2010-03-13 22:59:46 UTC
(rev 4675)
@@ -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>
Modified: paparazzi3/trunk/conf/airframes/booz2_a5.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/booz2_a5.xml 2010-03-13 00:51:28 UTC
(rev 4674)
+++ paparazzi3/trunk/conf/airframes/booz2_a5.xml 2010-03-13 22:59:46 UTC
(rev 4675)
@@ -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>
Added: paparazzi3/trunk/conf/autopilot/subsystems/booz_ahrs_mlkf.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/booz_ahrs_mlkf.makefile
(rev 0)
+++ paparazzi3/trunk/conf/autopilot/subsystems/booz_ahrs_mlkf.makefile
2010-03-13 22:59:46 UTC (rev 4675)
@@ -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
Modified: paparazzi3/trunk/conf/simulator/nps/nps_sensors_params_booz2_a1.h
===================================================================
--- paparazzi3/trunk/conf/simulator/nps/nps_sensors_params_booz2_a1.h
2010-03-13 00:51:28 UTC (rev 4674)
+++ paparazzi3/trunk/conf/simulator/nps/nps_sensors_params_booz2_a1.h
2010-03-13 22:59:46 UTC (rev 4675)
@@ -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.)
Modified:
paparazzi3/trunk/sw/airborne/booz/ahrs/booz2_filter_attitude_cmpl_euler.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ahrs/booz2_filter_attitude_cmpl_euler.c
2010-03-13 00:51:28 UTC (rev 4674)
+++ paparazzi3/trunk/sw/airborne/booz/ahrs/booz2_filter_attitude_cmpl_euler.c
2010-03-13 22:59:46 UTC (rev 4675)
@@ -196,9 +196,13 @@
}
-void booz_ahrs_update(void) {
+void booz_ahrs_update_accel(void) {
}
+void booz_ahrs_update_mag(void) {
+
+}
+
Modified: paparazzi3/trunk/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c
2010-03-13 00:51:28 UTC (rev 4674)
+++ paparazzi3/trunk/sw/airborne/booz/ahrs/booz_ahrs_float_lkf.c
2010-03-13 22:59:46 UTC (rev 4675)
@@ -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 @@
#endif
}
+void booz_ahrs_update_accel(void) {
+ RunOnceEvery(50, booz_ahrs_do_update_accel());
+}
-void booz_ahrs_update_accel(void) {
+static void booz_ahrs_do_update_accel(void) {
int i, j, k;
#ifdef BAFL_DEBUG2
@@ -632,6 +637,10 @@
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
Modified: paparazzi3/trunk/sw/airborne/booz/booz_ahrs.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_ahrs.h 2010-03-13 00:51:28 UTC
(rev 4674)
+++ paparazzi3/trunk/sw/airborne/booz/booz_ahrs.h 2010-03-13 22:59:46 UTC
(rev 4675)
@@ -33,43 +33,53 @@
#define BOOZ_AHRS_RUNNING 1
struct BoozAhrs {
+
+ struct Int32Quat ltp_to_imu_quat;
+ struct Int32Eulers ltp_to_imu_euler;
+ struct Int32RMat ltp_to_imu_rmat;
+ struct Int32Rates imu_rate;
+
+ struct Int32Quat ltp_to_body_quat;
struct Int32Eulers ltp_to_body_euler;
- struct Int32Quat ltp_to_body_quat;
- struct Int32RMat ltp_to_body_rmat;
- 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_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);
Modified: paparazzi3/trunk/sw/airborne/booz/booz_imu.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_imu.c 2010-03-13 00:51:28 UTC
(rev 4674)
+++ paparazzi3/trunk/sw/airborne/booz/booz_imu.c 2010-03-13 22:59:46 UTC
(rev 4675)
@@ -38,7 +38,10 @@
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);
Modified: paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_test_signal.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_test_signal.c
2010-03-13 00:51:28 UTC (rev 4674)
+++ paparazzi3/trunk/sw/airborne/booz/fms/booz_fms_test_signal.c
2010-03-13 22:59:46 UTC (rev 4675)
@@ -63,6 +63,9 @@
}
}
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)
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4675] changed units for IMU alignement in airframe file,
antoine drouin <=