[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5041] added new define for SIGN to allow using posi
From: |
antoine drouin |
Subject: |
[paparazzi-commits] [5041] added new define for SIGN to allow using positive scale factors only |
Date: |
Mon, 12 Jul 2010 13:43:16 +0000 |
Revision: 5041
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5041
Author: poine
Date: 2010-07-12 13:43:16 +0000 (Mon, 12 Jul 2010)
Log Message:
-----------
added new define for SIGN to allow using positive scale factors only
Modified Paths:
--------------
paparazzi3/trunk/sw/airborne/booz/booz_imu.h
paparazzi3/trunk/sw/airborne/booz/imu/booz_imu_b2.h
Modified: paparazzi3/trunk/sw/airborne/booz/booz_imu.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_imu.h 2010-07-12 13:41:12 UTC
(rev 5040)
+++ paparazzi3/trunk/sw/airborne/booz/booz_imu.h 2010-07-12 13:43:16 UTC
(rev 5041)
@@ -69,32 +69,32 @@
#define BoozImuScaleGyro() { \
RATES_COPY(booz_imu.gyro_prev, booz_imu.gyro); \
- booz_imu.gyro.p = ((booz_imu.gyro_unscaled.p -
booz_imu.gyro_neutral.p)*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN; \
- booz_imu.gyro.q = ((booz_imu.gyro_unscaled.q -
booz_imu.gyro_neutral.q)*IMU_GYRO_Q_SENS_NUM)/IMU_GYRO_Q_SENS_DEN; \
- booz_imu.gyro.r = ((booz_imu.gyro_unscaled.r -
booz_imu.gyro_neutral.r)*IMU_GYRO_R_SENS_NUM)/IMU_GYRO_R_SENS_DEN; \
+ booz_imu.gyro.p = ((booz_imu.gyro_unscaled.p -
booz_imu.gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN;
\
+ booz_imu.gyro.q = ((booz_imu.gyro_unscaled.q -
booz_imu.gyro_neutral.q)*IMU_GYRO_Q_SIGN*IMU_GYRO_Q_SENS_NUM)/IMU_GYRO_Q_SENS_DEN;
\
+ booz_imu.gyro.r = ((booz_imu.gyro_unscaled.r -
booz_imu.gyro_neutral.r)*IMU_GYRO_R_SIGN*IMU_GYRO_R_SENS_NUM)/IMU_GYRO_R_SENS_DEN;
\
}
#define BoozImuScaleAccel() { \
VECT3_COPY(booz_imu.accel_prev, booz_imu.accel); \
- booz_imu.accel.x = ((booz_imu.accel_unscaled.x -
booz_imu.accel_neutral.x)*IMU_ACCEL_X_SENS_NUM)/IMU_ACCEL_X_SENS_DEN; \
- booz_imu.accel.y = ((booz_imu.accel_unscaled.y -
booz_imu.accel_neutral.y)*IMU_ACCEL_Y_SENS_NUM)/IMU_ACCEL_Y_SENS_DEN; \
- booz_imu.accel.z = ((booz_imu.accel_unscaled.z -
booz_imu.accel_neutral.z)*IMU_ACCEL_Z_SENS_NUM)/IMU_ACCEL_Z_SENS_DEN; \
+ booz_imu.accel.x = ((booz_imu.accel_unscaled.x -
booz_imu.accel_neutral.x)*IMU_ACCEL_X_SIGN*IMU_ACCEL_X_SENS_NUM)/IMU_ACCEL_X_SENS_DEN;
\
+ booz_imu.accel.y = ((booz_imu.accel_unscaled.y -
booz_imu.accel_neutral.y)*IMU_ACCEL_Y_SIGN*IMU_ACCEL_Y_SENS_NUM)/IMU_ACCEL_Y_SENS_DEN;
\
+ booz_imu.accel.z = ((booz_imu.accel_unscaled.z -
booz_imu.accel_neutral.z)*IMU_ACCEL_Z_SIGN*IMU_ACCEL_Z_SENS_NUM)/IMU_ACCEL_Z_SENS_DEN;
\
}
#if defined IMU_MAG_45_HACK
#define BoozImuScaleMag() { \
- int32_t msx = ((booz_imu.mag_unscaled.x - booz_imu.mag_neutral.x) *
IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
- int32_t msy = ((booz_imu.mag_unscaled.y - booz_imu.mag_neutral.y) *
IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
+ int32_t msx = ((booz_imu.mag_unscaled.x - booz_imu.mag_neutral.x) *
IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
+ int32_t msy = ((booz_imu.mag_unscaled.y - booz_imu.mag_neutral.y) *
IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
booz_imu.mag.x = msx - msy; \
booz_imu.mag.y = msx + msy; \
- booz_imu.mag.z = ((booz_imu.mag_unscaled.z - booz_imu.mag_neutral.z) *
IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
+ booz_imu.mag.z = ((booz_imu.mag_unscaled.z - booz_imu.mag_neutral.z) *
IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
}
#else
#define BoozImuScaleMag() { \
- booz_imu.mag.x = ((booz_imu.mag_unscaled.x - booz_imu.mag_neutral.x) *
IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
- booz_imu.mag.y = ((booz_imu.mag_unscaled.y - booz_imu.mag_neutral.y) *
IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
- booz_imu.mag.z = ((booz_imu.mag_unscaled.z - booz_imu.mag_neutral.z) *
IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
+ booz_imu.mag.x = ((booz_imu.mag_unscaled.x - booz_imu.mag_neutral.x) *
IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
+ booz_imu.mag.y = ((booz_imu.mag_unscaled.y - booz_imu.mag_neutral.y) *
IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
+ booz_imu.mag.z = ((booz_imu.mag_unscaled.z - booz_imu.mag_neutral.z) *
IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
}
#endif
Modified: paparazzi3/trunk/sw/airborne/booz/imu/booz_imu_b2.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/imu/booz_imu_b2.h 2010-07-12 13:41:12 UTC
(rev 5040)
+++ paparazzi3/trunk/sw/airborne/booz/imu/booz_imu_b2.h 2010-07-12 13:43:16 UTC
(rev 5041)
@@ -28,10 +28,28 @@
#include "peripherals/booz_max1168.h"
+/* type of magnetometer */
#define IMU_B2_MAG_NONE 0
#define IMU_B2_MAG_MS2001 1
#define IMU_B2_MAG_AMI601 2
+#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined
IMU_GYRO_R_SIGN
+#define IMU_GYRO_P_SIGN 1
+#define IMU_GYRO_Q_SIGN -1
+#define IMU_GYRO_R_SIGN -1
+#endif
+#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined
IMU_ACCEL_Z_SIGN
+#define IMU_ACCEL_X_SIGN -1
+#define IMU_ACCEL_Y_SIGN -1
+#define IMU_ACCEL_Z_SIGN -1
+#endif
+#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
+#define IMU_MAG_X_SIGN 1
+#define IMU_MAG_Y_SIGN -1
+#define IMU_MAG_Z_SIGN -1
+#endif
+
+
#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
#include "peripherals/booz_ms2001.h"
#define BoozImuMagEvent(_mag_handler) {
\
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5041] added new define for SIGN to allow using positive scale factors only,
antoine drouin <=