[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5970] move booz imu to firmwares/rotorcraft and ren
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [5970] move booz imu to firmwares/rotorcraft and rename |
|
Date: |
Mon, 27 Sep 2010 22:55:53 +0000 |
Revision: 5970
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5970
Author: flixr
Date: 2010-09-27 22:55:53 +0000 (Mon, 27 Sep 2010)
Log Message:
-----------
move booz imu to firmwares/rotorcraft and rename
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.h
Removed Paths:
-------------
paparazzi3/trunk/sw/airborne/booz/booz_imu.c
paparazzi3/trunk/sw/airborne/booz/booz_imu.h
Deleted: paparazzi3/trunk/sw/airborne/booz/booz_imu.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_imu.c 2010-09-27 22:55:46 UTC
(rev 5969)
+++ paparazzi3/trunk/sw/airborne/booz/booz_imu.c 2010-09-27 22:55:53 UTC
(rev 5970)
@@ -1,53 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-#include "booz_imu.h"
-
-#include "airframe.h"
-
-struct BoozImu booz_imu;
-
-void booz_imu_init(void) {
-
- /* initialises neutrals */
- RATES_ASSIGN(booz_imu.gyro_neutral, IMU_GYRO_P_NEUTRAL,
IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL);
- VECT3_ASSIGN(booz_imu.accel_neutral, IMU_ACCEL_X_NEUTRAL,
IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);
- VECT3_ASSIGN(booz_imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL,
IMU_MAG_Z_NEUTRAL);
-
- /*
- Compute quaternion and rotation matrix
- for conversions between body and imu frame
- */
-#ifdef IMU_BODY_TO_IMU_PHI
- 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);
-#endif
-
- booz_imu_impl_init();
-}
-
Deleted: paparazzi3/trunk/sw/airborne/booz/booz_imu.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz_imu.h 2010-09-27 22:55:46 UTC
(rev 5969)
+++ paparazzi3/trunk/sw/airborne/booz/booz_imu.h 2010-09-27 22:55:53 UTC
(rev 5970)
@@ -1,103 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-#ifndef BOOZ_IMU_H
-#define BOOZ_IMU_H
-
-#include "math/pprz_algebra_int.h"
-#include "math/pprz_algebra_float.h"
-
-/* must be defined by underlying hardware */
-extern void booz_imu_impl_init(void);
-extern void booz_imu_periodic(void);
-
-struct BoozImu {
- struct Int32Rates gyro;
- struct Int32Vect3 accel;
- struct Int32Vect3 mag;
- struct Int32Rates gyro_prev;
- struct Int32Vect3 accel_prev;
- struct Int32Rates gyro_neutral;
- struct Int32Vect3 accel_neutral;
- struct Int32Vect3 mag_neutral;
- struct Int32Rates gyro_unscaled;
- struct Int32Vect3 accel_unscaled;
- struct Int32Vect3 mag_unscaled;
- struct Int32Quat body_to_imu_quat;
- struct Int32RMat body_to_imu_rmat;
-};
-
-/* abstract IMU interface providing floating point interface */
-struct BoozImuFloat {
- struct FloatRates gyro;
- struct FloatVect3 accel;
- struct FloatVect3 mag;
- struct FloatRates gyro_prev;
- struct FloatEulers body_to_imu_eulers;
- struct FloatQuat body_to_imu_quat;
- struct FloatRMat body_to_imu_rmat;
- uint32_t sample_count;
-};
-
-/* underlying hardware */
-#ifdef BOOZ_IMU_TYPE_H
-#include BOOZ_IMU_TYPE_H
-#endif
-
-extern struct BoozImu booz_imu;
-
-extern void booz_imu_init(void);
-
-#define BoozImuScaleGyro(_imu) { \
- RATES_COPY(_imu.gyro_prev, _imu.gyro); \
- _imu.gyro.p = ((_imu.gyro_unscaled.p -
_imu.gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN; \
- _imu.gyro.q = ((_imu.gyro_unscaled.q -
_imu.gyro_neutral.q)*IMU_GYRO_Q_SIGN*IMU_GYRO_Q_SENS_NUM)/IMU_GYRO_Q_SENS_DEN; \
- _imu.gyro.r = ((_imu.gyro_unscaled.r -
_imu.gyro_neutral.r)*IMU_GYRO_R_SIGN*IMU_GYRO_R_SENS_NUM)/IMU_GYRO_R_SENS_DEN; \
- }
-
-
-#define BoozImuScaleAccel(_imu) { \
- VECT3_COPY(_imu.accel_prev, _imu.accel); \
- _imu.accel.x = ((_imu.accel_unscaled.x -
_imu.accel_neutral.x)*IMU_ACCEL_X_SIGN*IMU_ACCEL_X_SENS_NUM)/IMU_ACCEL_X_SENS_DEN;
\
- _imu.accel.y = ((_imu.accel_unscaled.y -
_imu.accel_neutral.y)*IMU_ACCEL_Y_SIGN*IMU_ACCEL_Y_SENS_NUM)/IMU_ACCEL_Y_SENS_DEN;
\
- _imu.accel.z = ((_imu.accel_unscaled.z -
_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(_imu) {
\
- int32_t msx = ((_imu.mag_unscaled.x - _imu.mag_neutral.x) * IMU_MAG_X_SIGN
* IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
- int32_t msy = ((_imu.mag_unscaled.y - _imu.mag_neutral.y) * IMU_MAG_Y_SIGN
* IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
- _imu.mag.x = msx - msy; \
- _imu.mag.y = msx + msy; \
- _imu.mag.z = ((_imu.mag_unscaled.z - _imu.mag_neutral.z) * IMU_MAG_Z_SIGN
* IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
- }
-#else
-#define BoozImuScaleMag(_imu) {
\
- _imu.mag.x = ((_imu.mag_unscaled.x - _imu.mag_neutral.x) * IMU_MAG_X_SIGN
* IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
- _imu.mag.y = ((_imu.mag_unscaled.y - _imu.mag_neutral.y) * IMU_MAG_Y_SIGN
* IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
- _imu.mag.z = ((_imu.mag_unscaled.z - _imu.mag_neutral.z) * IMU_MAG_Z_SIGN
* IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
- }
-#endif
-
-
-#endif /* BOOZ2_IMU_H */
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.c (from rev 5969,
paparazzi3/trunk/sw/airborne/booz/booz_imu.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.c 2010-09-27
22:55:53 UTC (rev 5970)
@@ -0,0 +1,52 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#include "booz_imu.h"
+
+#include "airframe.h"
+
+struct BoozImu booz_imu;
+
+void booz_imu_init(void) {
+
+ /* initialises neutrals */
+ RATES_ASSIGN(booz_imu.gyro_neutral, IMU_GYRO_P_NEUTRAL,
IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL);
+ VECT3_ASSIGN(booz_imu.accel_neutral, IMU_ACCEL_X_NEUTRAL,
IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);
+ VECT3_ASSIGN(booz_imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL,
IMU_MAG_Z_NEUTRAL);
+
+ /*
+ Compute quaternion and rotation matrix
+ for conversions between body and imu frame
+ */
+#ifdef IMU_BODY_TO_IMU_PHI
+ 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);
+#endif
+
+ booz_imu_impl_init();
+}
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.h (from rev 5969,
paparazzi3/trunk/sw/airborne/booz/booz_imu.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.h 2010-09-27
22:55:53 UTC (rev 5970)
@@ -0,0 +1,103 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#ifndef BOOZ_IMU_H
+#define BOOZ_IMU_H
+
+#include "math/pprz_algebra_int.h"
+#include "math/pprz_algebra_float.h"
+
+/* must be defined by underlying hardware */
+extern void booz_imu_impl_init(void);
+extern void booz_imu_periodic(void);
+
+struct BoozImu {
+ struct Int32Rates gyro;
+ struct Int32Vect3 accel;
+ struct Int32Vect3 mag;
+ struct Int32Rates gyro_prev;
+ struct Int32Vect3 accel_prev;
+ struct Int32Rates gyro_neutral;
+ struct Int32Vect3 accel_neutral;
+ struct Int32Vect3 mag_neutral;
+ struct Int32Rates gyro_unscaled;
+ struct Int32Vect3 accel_unscaled;
+ struct Int32Vect3 mag_unscaled;
+ struct Int32Quat body_to_imu_quat;
+ struct Int32RMat body_to_imu_rmat;
+};
+
+/* abstract IMU interface providing floating point interface */
+struct BoozImuFloat {
+ struct FloatRates gyro;
+ struct FloatVect3 accel;
+ struct FloatVect3 mag;
+ struct FloatRates gyro_prev;
+ struct FloatEulers body_to_imu_eulers;
+ struct FloatQuat body_to_imu_quat;
+ struct FloatRMat body_to_imu_rmat;
+ uint32_t sample_count;
+};
+
+/* underlying hardware */
+#ifdef BOOZ_IMU_TYPE_H
+#include BOOZ_IMU_TYPE_H
+#endif
+
+extern struct BoozImu booz_imu;
+
+extern void booz_imu_init(void);
+
+#define BoozImuScaleGyro(_imu) { \
+ RATES_COPY(_imu.gyro_prev, _imu.gyro); \
+ _imu.gyro.p = ((_imu.gyro_unscaled.p -
_imu.gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN; \
+ _imu.gyro.q = ((_imu.gyro_unscaled.q -
_imu.gyro_neutral.q)*IMU_GYRO_Q_SIGN*IMU_GYRO_Q_SENS_NUM)/IMU_GYRO_Q_SENS_DEN; \
+ _imu.gyro.r = ((_imu.gyro_unscaled.r -
_imu.gyro_neutral.r)*IMU_GYRO_R_SIGN*IMU_GYRO_R_SENS_NUM)/IMU_GYRO_R_SENS_DEN; \
+ }
+
+
+#define BoozImuScaleAccel(_imu) { \
+ VECT3_COPY(_imu.accel_prev, _imu.accel); \
+ _imu.accel.x = ((_imu.accel_unscaled.x -
_imu.accel_neutral.x)*IMU_ACCEL_X_SIGN*IMU_ACCEL_X_SENS_NUM)/IMU_ACCEL_X_SENS_DEN;
\
+ _imu.accel.y = ((_imu.accel_unscaled.y -
_imu.accel_neutral.y)*IMU_ACCEL_Y_SIGN*IMU_ACCEL_Y_SENS_NUM)/IMU_ACCEL_Y_SENS_DEN;
\
+ _imu.accel.z = ((_imu.accel_unscaled.z -
_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(_imu) {
\
+ int32_t msx = ((_imu.mag_unscaled.x - _imu.mag_neutral.x) * IMU_MAG_X_SIGN
* IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
+ int32_t msy = ((_imu.mag_unscaled.y - _imu.mag_neutral.y) * IMU_MAG_Y_SIGN
* IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
+ _imu.mag.x = msx - msy; \
+ _imu.mag.y = msx + msy; \
+ _imu.mag.z = ((_imu.mag_unscaled.z - _imu.mag_neutral.z) * IMU_MAG_Z_SIGN
* IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
+ }
+#else
+#define BoozImuScaleMag(_imu) {
\
+ _imu.mag.x = ((_imu.mag_unscaled.x - _imu.mag_neutral.x) * IMU_MAG_X_SIGN
* IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; \
+ _imu.mag.y = ((_imu.mag_unscaled.y - _imu.mag_neutral.y) * IMU_MAG_Y_SIGN
* IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; \
+ _imu.mag.z = ((_imu.mag_unscaled.z - _imu.mag_neutral.z) * IMU_MAG_Z_SIGN
* IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; \
+ }
+#endif
+
+
+#endif /* BOOZ2_IMU_H */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5970] move booz imu to firmwares/rotorcraft and rename,
Felix Ruess <=