[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6211] move firmwares/rotorcraft/imu to a general su
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [6211] move firmwares/rotorcraft/imu to a general subsystems |
|
Date: |
Sat, 23 Oct 2010 21:11:31 +0000 |
Revision: 6211
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6211
Author: flixr
Date: 2010-10-23 21:11:30 +0000 (Sat, 23 Oct 2010)
Log Message:
-----------
move firmwares/rotorcraft/imu to a general subsystems
Modified Paths:
--------------
paparazzi3/trunk/conf/autopilot/rotorcraft.makefile
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/subsystems/
paparazzi3/trunk/sw/airborne/subsystems/imu/
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/imu_b2_arch.c
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/imu_b2_arch.h
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/imu_crista_arch.c
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/imu_crista_arch.h
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/imu_b2_arch.c
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/imu_b2_arch.h
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/imu_crista_arch.c
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/imu_crista_arch.h
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_aspirin_arch.c
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_aspirin_arch.h
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_b2_arch.c
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_b2_arch.h
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_crista_arch.c
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_crista_arch.h
paparazzi3/trunk/sw/airborne/subsystems/imu/imu.c
paparazzi3/trunk/sw/airborne/subsystems/imu/imu.h
paparazzi3/trunk/sw/airborne/subsystems/imu/imu_aspirin.c
paparazzi3/trunk/sw/airborne/subsystems/imu/imu_aspirin.h
paparazzi3/trunk/sw/airborne/subsystems/imu/imu_b2.c
paparazzi3/trunk/sw/airborne/subsystems/imu/imu_b2.h
paparazzi3/trunk/sw/airborne/subsystems/imu/imu_crista.c
paparazzi3/trunk/sw/airborne/subsystems/imu/imu_crista.h
Removed Paths:
-------------
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.h
Modified: paparazzi3/trunk/conf/autopilot/rotorcraft.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/rotorcraft.makefile 2010-10-23 21:11:01 UTC
(rev 6210)
+++ paparazzi3/trunk/conf/autopilot/rotorcraft.makefile 2010-10-23 21:11:30 UTC
(rev 6211)
@@ -35,11 +35,12 @@
SRC_BOOZ=booz
SRC_BOOZ_ARCH=$(SRC_BOOZ)/arch/$(ARCH)
SRC_BOOZ_TEST=$(SRC_BOOZ)/test
+SRC_BOOZ_PRIV=booz_priv
+
SRC_BOARD=boards/$(BOARD)
SRC_FIRMWARE=firmwares/rotorcraft
+SRC_SUBSYSTEMS=subsystems
-SRC_BOOZ_PRIV=booz_priv
-
SRC_ARCH=arch/$(ARCH)
CFG_BOOZ=$(PAPARAZZI_SRC)/conf/autopilot/
Modified:
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile
2010-10-23 21:11:01 UTC (rev 6210)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.0.makefile
2010-10-23 21:11:30 UTC (rev 6211)
@@ -37,15 +37,15 @@
# imu Booz2 v1
# add imu arch to include directories
-ap.CFLAGS += -I$(SRC_FIRMWARE)/imu/arch/$(ARCH)
+ap.CFLAGS += -I$(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)
ap.CFLAGS += -DIMU_TYPE_H=\"imu/imu_b2.h\"
ap.CFLAGS += -DIMU_B2_VERSION_1_0
ap.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601
ap.CFLAGS += -DSSP_VIC_SLOT=9
-ap.srcs += $(SRC_FIRMWARE)/imu.c \
- $(SRC_FIRMWARE)/imu/imu_b2.c \
- $(SRC_FIRMWARE)/imu/arch/$(ARCH)/imu_b2_arch.c
+ap.srcs += $(SRC_SUBSYSTEMS)/imu.c \
+ $(SRC_SUBSYSTEMS)/imu/imu_b2.c \
+ $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_b2_arch.c
ap.CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
ap.srcs += $(SRC_BOOZ)/peripherals/booz_max1168.c \
@@ -61,14 +61,14 @@
#
# add imu arch to include directories
-sim.CFLAGS += -I$(SRC_FIRMWARE)/imu/arch/$(ARCH)
+sim.CFLAGS += -I$(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)
sim.CFLAGS += -DIMU_TYPE_H=\"imu/imu_b2.h\"
sim.CFLAGS += -DIMU_B2_VERSION_1_0
sim.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601
-sim.srcs += $(SRC_FIRMWARE)/imu.c \
- $(SRC_FIRMWARE)/imu/imu_b2.c \
- $(SRC_FIRMWARE)/imu/arch/$(ARCH)/imu_b2_arch.c
+sim.srcs += $(SRC_SUBSYSTEMS)/imu.c \
+ $(SRC_SUBSYSTEMS)/imu/imu_b2.c \
+ $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_b2_arch.c
sim.srcs += $(SRC_BOOZ)/peripherals/booz_max1168.c \
$(SRC_BOOZ_SIM)/peripherals/booz_max1168_arch.c
Modified:
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile
2010-10-23 21:11:01 UTC (rev 6210)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.1.makefile
2010-10-23 21:11:30 UTC (rev 6211)
@@ -42,14 +42,14 @@
# imu Booz2 v1.1
# add imu arch to include directories
-ap.CFLAGS += -I$(SRC_FIRMWARE)/imu/arch/$(ARCH)
+ap.CFLAGS += -I$(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)
imu_CFLAGS += -DIMU_TYPE_H=\"imu/imu_b2.h\"
imu_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_MS2001
imu_CFLAGS += -DIMU_B2_VERSION_1_1
-ap.srcs += $(SRC_FIRMWARE)/imu.c \
- $(SRC_FIRMWARE)/imu/imu_b2.c \
- $(SRC_FIRMWARE)/imu/arch/$(ARCH)/imu_b2_arch.c
+ap.srcs += $(SRC_SUBSYSTEMS)/imu.c \
+ $(SRC_SUBSYSTEMS)/imu/imu_b2.c \
+ $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_b2_arch.c
imu_srcs += $(SRC_BOOZ)/peripherals/booz_max1168.c \
$(SRC_BOOZ_ARCH)/peripherals/booz_max1168_arch.c
@@ -77,14 +77,14 @@
#
# add imu arch to include directories
-sim.CFLAGS += -I$(SRC_FIRMWARE)/imu/arch/$(ARCH)
+sim.CFLAGS += -I$(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)
sim.CFLAGS += -DIMU_TYPE_H=\"imu/imu_b2.h\"
sim.CFLAGS += -DIMU_B2_VERSION_1_1
sim.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601
-sim.srcs += $(SRC_FIRMWARE)/imu.c \
- $(SRC_FIRMWARE)/imu/imu_b2.c \
- $(SRC_FIRMWARE)/imu/arch/$(ARCH)/imu_b2_arch.c
+sim.srcs += $(SRC_SUBSYSTEMS)/imu.c \
+ $(SRC_SUBSYSTEMS)/imu/imu_b2.c \
+ $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_b2_arch.c
sim.srcs += $(SRC_BOOZ)/peripherals/booz_max1168.c \
$(SRC_BOOZ_SIM)/peripherals/booz_max1168_arch.c
Modified:
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile
2010-10-23 21:11:01 UTC (rev 6210)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/imu_b2_v1.2.makefile
2010-10-23 21:11:30 UTC (rev 6211)
@@ -42,14 +42,14 @@
# imu Booz2 v1.2
# add imu arch to include directories
-imu_CFLAGS += -I$(SRC_FIRMWARE)/imu/arch/$(ARCH)
+imu_CFLAGS += -I$(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)
imu_CFLAGS += -DIMU_TYPE_H=\"imu/imu_b2.h\"
imu_CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_HMC5843
imu_CFLAGS += -DIMU_B2_VERSION_1_2
-imu_srcs += $(SRC_FIRMWARE)/imu.c \
- $(SRC_FIRMWARE)/imu/imu_b2.c \
- $(SRC_FIRMWARE)/imu/arch/$(ARCH)/imu_b2_arch.c
+imu_srcs += $(SRC_SUBSYSTEMS)/imu.c \
+ $(SRC_SUBSYSTEMS)/imu/imu_b2.c \
+ $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_b2_arch.c
imu_srcs += $(SRC_BOOZ)/peripherals/booz_max1168.c \
$(SRC_BOOZ_ARCH)/peripherals/booz_max1168_arch.c
@@ -79,14 +79,14 @@
#
# add imu arch to include directories
-sim.CFLAGS += -I$(SRC_FIRMWARE)/imu/arch/$(ARCH)
+sim.CFLAGS += -I$(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)
sim.CFLAGS += -DIMU_TYPE_H=\"imu/imu_b2.h\"
sim.CFLAGS += -DIMU_B2_VERSION_1_1
sim.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601
-sim.srcs += $(SRC_FIRMWARE)/imu.c \
- $(SRC_FIRMWARE)/imu/imu_b2.c \
- $(SRC_FIRMWARE)/imu/arch/$(ARCH)/imu_b2_arch.c
+sim.srcs += $(SRC_SUBSYSTEMS)/imu.c \
+ $(SRC_SUBSYSTEMS)/imu/imu_b2.c \
+ $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_b2_arch.c
sim.srcs += $(SRC_BOOZ)/peripherals/booz_max1168.c \
$(SRC_BOOZ_SIM)/peripherals/booz_max1168_arch.c
Modified:
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile
2010-10-23 21:11:01 UTC (rev 6210)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/imu_crista.makefile
2010-10-23 21:11:30 UTC (rev 6211)
@@ -61,12 +61,12 @@
# add imu arch to include directories
-ap.CFLAGS += -I$(SRC_FIRMWARE)/imu/arch/$(ARCH)
+ap.CFLAGS += -I$(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)
ap.CFLAGS += -DIMU_TYPE_H=\"imu/imu_crista.h\"
-ap.srcs += $(SRC_FIRMWARE)/imu.c \
- $(SRC_FIRMWARE)/imu/imu_crista.c \
- $(SRC_FIRMWARE)/imu/arch/$(ARCH)/imu_crista_arch.c
+ap.srcs += $(SRC_SUBSYSTEMS)/imu.c \
+ $(SRC_SUBSYSTEMS)/imu/imu_crista.c \
+ $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_crista_arch.c
ap.CFLAGS += -DUSE_AMI601
ap.srcs += $(SRC_BOOZ)/peripherals/booz_ami601.c
@@ -78,12 +78,12 @@
#
# add imu arch to include directories
-sim.CFLAGS += -I$(SRC_FIRMWARE)/imu/arch/$(ARCH)
+sim.CFLAGS += -I$(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)
sim.CFLAGS += -DIMU_TYPE_H=\"imu/imu_crista.h\"
-sim.srcs += $(SRC_FIRMWARE)/imu.c \
- $(SRC_FIRMWARE)/imu/imu_crista.c \
- $(SRC_FIRMWARE)/imu/arch/$(ARCH)/imu_crista_arch.c
+sim.srcs += $(SRC_SUBSYSTEMS)/imu.c \
+ $(SRC_SUBSYSTEMS)/imu/imu_crista.c \
+ $(SRC_SUBSYSTEMS)/imu/arch/$(ARCH)/imu_crista_arch.c
sim.CFLAGS += -DUSE_AMI601
sim.srcs += $(SRC_BOOZ)/peripherals/booz_ami601.c
Deleted: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.c 2010-10-23
21:11:01 UTC (rev 6210)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.c 2010-10-23
21:11:30 UTC (rev 6211)
@@ -1,52 +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 "imu.h"
-
-#include "airframe.h"
-
-struct Imu imu;
-
-void imu_init(void) {
-
- /* initialises neutrals */
- RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL,
IMU_GYRO_R_NEUTRAL);
- VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL,
IMU_ACCEL_Z_NEUTRAL);
- VECT3_ASSIGN(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(imu.body_to_imu_quat, body_to_imu_eulers);
- INT32_QUAT_NORMALISE(imu.body_to_imu_quat);
- INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);
-#endif
-
- imu_impl_init();
-}
Deleted: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.h 2010-10-23
21:11:01 UTC (rev 6210)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.h 2010-10-23
21:11:30 UTC (rev 6211)
@@ -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 IMU_H
-#define IMU_H
-
-#include "math/pprz_algebra_int.h"
-#include "math/pprz_algebra_float.h"
-
-/* must be defined by underlying hardware */
-extern void imu_impl_init(void);
-extern void imu_periodic(void);
-
-struct Imu {
- 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 ImuFloat {
- 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 IMU_TYPE_H
-#include IMU_TYPE_H
-#endif
-
-extern struct Imu imu;
-
-extern void imu_init(void);
-
-#define ImuScaleGyro(_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 ImuScaleAccel(_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 ImuScaleMag(_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 ImuScaleMag(_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 /* IMU_H */
Copied: paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/imu_b2_arch.c
(from rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_b2_arch.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/imu_b2_arch.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/imu_b2_arch.c
2010-10-23 21:11:30 UTC (rev 6211)
@@ -0,0 +1,189 @@
+/*
+ * $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 <firmwares/rotorcraft/imu.h>
+
+volatile uint8_t imu_ssp_status;
+static void SSP_ISR(void) __attribute__((naked));
+#if 0
+static inline bool_t isr_try_mag(void);
+#endif
+
+/* SSPCR0 settings */
+#define SSP_DDS8 0x07 << 0 /* data size : 8 bits
*/
+#define SSP_DDS16 0x0F << 0 /* data size : 16 bits
*/
+#define SSP_FRF 0x00 << 4 /* frame format : SPI
*/
+#define SSP_CPOL 0x00 << 6 /* clock polarity : data captured on first
clock transition */
+#define SSP_CPHA 0x00 << 7 /* clock phase : SCK idles low
*/
+#define SSP_SCR 0x0F << 8 /* serial clock rate : divide by 16
*/
+
+/* SSPCR1 settings */
+#define SSP_LBM 0x00 << 0 /* loopback mode : disabled
*/
+#define SSP_SSE 0x00 << 1 /* SSP enable : disabled
*/
+#define SSP_MS 0x00 << 2 /* master slave mode : master
*/
+#define SSP_SOD 0x00 << 3 /* slave output disable : don't care when master
*/
+
+#define SSPCR0_VAL8 (SSP_DDS8 | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR )
+#define SSPCR0_VAL16 (SSP_DDS16 | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR )
+#define SSPCR1_VAL (SSP_LBM | SSP_SSE | SSP_MS | SSP_SOD )
+
+#define SSP_PINSEL1_SCK (2<<2)
+#define SSP_PINSEL1_MISO (2<<4)
+#define SSP_PINSEL1_MOSI (2<<6)
+
+
+#define ImuSetSSP8bits() { \
+ SSPCR0 = SSPCR0_VAL8; \
+}
+
+#define ImuSetSSP16bits() { \
+ SSPCR0 = SSPCR0_VAL16; \
+}
+
+
+void imu_b2_arch_init(void) {
+
+ imu_ssp_status = IMU_SSP_STA_IDLE;
+
+ /* setup pins for SSP (SCK, MISO, MOSI) */
+ PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI;
+
+ /* setup SSP */
+ SSPCR0 = SSPCR0_VAL16;
+ SSPCR1 = SSPCR1_VAL;
+ SSPCPSR = 0x02;
+
+ /* initialize interrupt vector */
+ VICIntSelect &= ~VIC_BIT( VIC_SPI1 ); /* SPI1 selected as IRQ */
+ VICIntEnable = VIC_BIT( VIC_SPI1 ); /* enable it */
+ _VIC_CNTL(SSP_VIC_SLOT) = VIC_ENABLE | VIC_SPI1;
+ _VIC_ADDR(SSP_VIC_SLOT) = (uint32_t)SSP_ISR; /* address of the ISR */
+
+}
+
+
+void imu_periodic(void) {
+ // check ssp idle
+ // ASSERT((imu_status == IMU_STA_IDLE), DEBUG_IMU, IMU_ERR_OVERUN);
+
+ // setup 16 bits
+ ImuSetSSP16bits();
+ // read adc
+ imu_ssp_status = IMU_SSP_STA_BUSY_MAX1168;
+ booz_max1168_read();
+#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
+ RunOnceEvery(10, { ami601_read(); });
+#endif
+
+}
+
+
+
+#include "led.h"
+
+#if 0
+
+
+static inline bool_t isr_try_mag(void) {
+ switch (micromag_status) {
+ case MS2001_IDLE :
+ ImuSetSSP8bits();
+ Ms2001SendReq();
+ return TRUE;
+ case MS2001_GOT_EOC:
+ ImuSetSSP8bits();
+ Ms2001ReadRes();
+ return TRUE;
+ }
+ return FALSE;
+}
+
+static void SSP_ISR(void) {
+ ISR_ENTRY();
+
+ switch (imu_ssp_status) {
+ case IMU_SSP_STA_BUSY_MAX1168:
+ Max1168OnSpiInt();
+ if (isr_try_mag())
+ imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
+ else
+ imu_ssp_status = IMU_SSP_STA_IDLE;
+ break;
+ case IMU_SSP_STA_BUSY_MS2100:
+ Ms2001OnSpiIt();
+ imu_ssp_status = IMU_SSP_STA_IDLE;
+ break;
+ default:
+ // spurious interrupt
+ LED_ON(1);
+ }
+
+ VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
+ ISR_EXIT();
+}
+#endif
+
+
+static void SSP_ISR(void) {
+ ISR_ENTRY();
+
+ switch (imu_ssp_status) {
+ case IMU_SSP_STA_BUSY_MAX1168:
+ Max1168OnSpiInt();
+#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
+ if (ms2001_status == MS2001_IDLE || ms2001_status == MS2001_GOT_EOC) {
+ ImuSetSSP8bits();
+ if (ms2001_status == MS2001_IDLE) {
+ Ms2001SendReq();
+ }
+ else { /* MS2001_GOT_EOC */
+ Ms2001ReadRes();
+ }
+ imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
+ }
+ else {
+#endif
+ imu_ssp_status = IMU_SSP_STA_IDLE;
+#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
+ }
+#endif
+ break;
+#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
+ case IMU_SSP_STA_BUSY_MS2100:
+ Ms2001OnSpiIt();
+ if (ms2001_status == MS2001_IDLE) {
+ Ms2001SendReq();
+ imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
+ }
+ else
+ imu_ssp_status = IMU_SSP_STA_IDLE;
+ break;
+#endif
+ // default:
+ // spurious interrupt
+ // FIXME LED_ON(1);
+ }
+
+ VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
+ ISR_EXIT();
+}
Copied: paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/imu_b2_arch.h
(from rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_b2_arch.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/imu_b2_arch.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/imu_b2_arch.h
2010-10-23 21:11:30 UTC (rev 6211)
@@ -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.
+ */
+
+#ifndef IMU_B2_ARCH_H
+#define IMU_B2_ARCH_H
+
+/*
+
+ MAX1168 SPI ADC connected on SPI1
+ SS on P0.20
+ EOC on P0.16 ( EINT0 )
+
+ PNI mag on same bus
+ SS on p1.28
+ EOC P0.30 ( EINT3 )
+ RESET P1.19
+
+*/
+
+#include "std.h"
+#include "LPC21xx.h"
+#include "interrupt_hw.h"
+
+#define IMU_SSP_STA_IDLE 0
+#define IMU_SSP_STA_BUSY_MAX1168 1
+#define IMU_SSP_STA_BUSY_MS2100 2
+extern volatile uint8_t imu_ssp_status;
+
+
+
+
+#endif /* IMU_B2_ARCH_H */
Copied:
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/imu_crista_arch.c (from
rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_crista_arch.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/imu_crista_arch.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/imu_crista_arch.c
2010-10-23 21:11:30 UTC (rev 6211)
@@ -0,0 +1,123 @@
+/*
+ * $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 <firmwares/rotorcraft/imu.h>
+
+#include "LPC21xx.h"
+#include "armVIC.h"
+#include "ssp_hw.h"
+#include BOARD_CONFIG
+
+#define ADS8344_SS_IODIR IO0DIR
+#define ADS8344_SS_IOSET IO0SET
+#define ADS8344_SS_IOCLR IO0CLR
+#define ADS8344_SS_PIN 20
+
+#define ADS8344Select() SetBit(ADS8344_SS_IOCLR,ADS8344_SS_PIN)
+#define ADS8344Unselect() SetBit(ADS8344_SS_IOSET,ADS8344_SS_PIN)
+
+#define POWER_MODE (1 << 1 | 1)
+#define SGL_DIF 1 // Single ended
+
+/* SSPCR0 settings */
+#define SSP_DSS 0x07 << 0 /* data size : 8 bits */
+#define SSP_FRF 0x00 << 4 /* frame format : SPI */
+#define SSP_CPOL 0x00 << 6 /* clock polarity : idle low */
+#define SSP_CPHA 0x00 << 7 /* clock phase : 1 */
+#define SSP_SCR 0x09 << 8 /* serial clock rate : 1MHz */
+
+/* SSPCR1 settings */
+#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */
+#define SSP_SSE 0x00 << 1 /* SSP enable : disabled */
+#define SSP_MS 0x00 << 2 /* master slave mode : master */
+#define SSP_SOD 0x00 << 3 /* slave output disable : disabled */
+
+static void SPI1_ISR(void) __attribute__((naked));
+static uint8_t channel;
+
+void imu_crista_arch_init(void) {
+ channel = 0;
+
+ /* setup pins for SSP (SCK, MISO, MOSI) */
+ PINSEL1 |= 2 << 2 | 2 << 4 | 2 << 6;
+
+ /* setup SSP */
+ SSPCR0 = SSP_DSS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR;
+ SSPCR1 = SSP_LBM | SSP_MS | SSP_SOD;
+ SSPCPSR = 2; /* -> 50kHz */
+
+ /* initialize interrupt vector */
+ VICIntSelect &= ~VIC_BIT(VIC_SPI1); // SPI1 selected as IRQ
+ VICIntEnable = VIC_BIT(VIC_SPI1); // SPI1 interrupt enabled
+ VICVectCntl7 = VIC_ENABLE | VIC_SPI1;
+ VICVectAddr7 = (uint32_t)SPI1_ISR; // address of the ISR
+
+ /* setup slave select */
+ /* configure SS pin */
+ SetBit( ADS8344_SS_IODIR, ADS8344_SS_PIN); /* pin is output */
+ ADS8344Unselect(); /* pin low */
+}
+
+
+static inline void read_values( void ) {
+ uint8_t foo __attribute__ ((unused)) = SSPDR;
+ uint8_t msb = SSPDR;
+ uint8_t lsb = SSPDR;
+ uint8_t llsb = SSPDR;
+ ADS8344_values[channel] = (msb << 8 | lsb) << 1 | llsb >> 7;
+}
+
+static inline void send_request( void ) {
+ uint8_t control = 1 << 7 | channel << 4 | SGL_DIF << 2 | POWER_MODE;
+ SSP_Send(control);
+ SSP_Send(0);
+ SSP_Send(0);
+ SSP_Send(0);
+}
+
+void ADS8344_start( void ) {
+ ADS8344Select();
+ SSP_ClearRti();
+ SSP_EnableRti();
+ SSP_Enable();
+ send_request();
+}
+
+void SPI1_ISR(void) {
+ ISR_ENTRY();
+ read_values();
+ channel++;
+ if (channel > 7-1) {
+ channel = 0;
+ ADS8344_available = TRUE;
+ ADS8344Unselect();
+ }
+ else {
+ send_request();
+ }
+
+ SSP_ClearRti();
+
+ VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
+ ISR_EXIT();
+}
Copied:
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/imu_crista_arch.h (from
rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/arch/lpc21/imu_crista_arch.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/imu_crista_arch.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/arch/lpc21/imu_crista_arch.h
2010-10-23 21:11:30 UTC (rev 6211)
@@ -0,0 +1,37 @@
+/*
+ * $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 IMU_INT_HW_H
+#define IMU_INT_HW_H
+
+#include "std.h"
+
+
+
+#define ImuCristaArchPeriodic() { \
+ ADS8344_start(); \
+ }
+
+extern void ADS8344_start( void );
+
+#endif /* IMU_INT_HW_H */
Copied: paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/imu_b2_arch.c
(from rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_b2_arch.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/imu_b2_arch.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/imu_b2_arch.c
2010-10-23 21:11:30 UTC (rev 6211)
@@ -0,0 +1,61 @@
+/*
+ * $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 <firmwares/rotorcraft/imu.h>
+
+#include "airframe.h"
+
+void imu_b2_arch_init(void) {
+
+}
+
+void imu_periodic(void) {
+
+}
+
+#include "nps_sensors.h"
+
+void imu_feed_gyro_accel(void) {
+ booz_max1168_values[IMU_GYRO_P_CHAN] = sensors.gyro.value.x;
+ booz_max1168_values[IMU_GYRO_Q_CHAN] = sensors.gyro.value.y;
+ booz_max1168_values[IMU_GYRO_R_CHAN] = sensors.gyro.value.z;
+ booz_max1168_values[IMU_ACCEL_X_CHAN] = sensors.accel.value.x;
+ booz_max1168_values[IMU_ACCEL_Y_CHAN] = sensors.accel.value.y;
+ booz_max1168_values[IMU_ACCEL_Z_CHAN] = sensors.accel.value.z;
+ booz_max1168_status = STA_MAX1168_DATA_AVAILABLE;
+}
+
+
+void imu_feed_mag(void) {
+#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
+ ms2001_values[IMU_MAG_X_CHAN] = sensors.mag.value.x;
+ ms2001_values[IMU_MAG_Y_CHAN] = sensors.mag.value.y;
+ ms2001_values[IMU_MAG_Z_CHAN] = sensors.mag.value.z;
+ ms2001_status = MS2001_DATA_AVAILABLE;
+#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
+ ami601_values[IMU_MAG_X_CHAN] = sensors.mag.value.x;
+ ami601_values[IMU_MAG_Y_CHAN] = sensors.mag.value.y;
+ ami601_values[IMU_MAG_Z_CHAN] = sensors.mag.value.z;
+ ami601_status = AMI601_DATA_AVAILABLE;
+#endif
+}
Copied: paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/imu_b2_arch.h
(from rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_b2_arch.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/imu_b2_arch.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/imu_b2_arch.h
2010-10-23 21:11:30 UTC (rev 6211)
@@ -0,0 +1,37 @@
+/*
+ * $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.
+ */
+
+/*
+ *
+ * simulator plug for the booz2 v1 imu arch dependant functions
+ *
+ */
+#ifndef IMU_B2_ARCH_H
+#define IMU_B2_ARCH_H
+
+
+extern void imu_feed_gyro_accel(void);
+extern void imu_feed_mag(void);
+
+
+#endif /* IMU_B2_HW_H */
Copied: paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/imu_crista_arch.c
(from rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_crista_arch.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/imu_crista_arch.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/imu_crista_arch.c
2010-10-23 21:11:30 UTC (rev 6211)
@@ -0,0 +1,31 @@
+/*
+ * simulator ARCH for rotorcraft imu crista
+ */
+
+#include <firmwares/rotorcraft/imu.h>
+
+#include "airframe.h"
+
+void imu_crista_arch_init(void) {
+
+}
+
+
+#include "nps_sensors.h"
+
+void imu_feed_gyro_accel(void) {
+ ADS8344_values[IMU_GYRO_P_CHAN] = sensors.gyro.value.x;
+ ADS8344_values[IMU_GYRO_Q_CHAN] = sensors.gyro.value.y;
+ ADS8344_values[IMU_GYRO_R_CHAN] = sensors.gyro.value.z;
+ ADS8344_values[IMU_ACCEL_X_CHAN] = sensors.accel.value.x;
+ ADS8344_values[IMU_ACCEL_Y_CHAN] = sensors.accel.value.y;
+ ADS8344_values[IMU_ACCEL_Z_CHAN] = sensors.accel.value.z;
+ ADS8344_available = TRUE;
+}
+
+void imu_feed_mag(void) {
+ ami601_values[IMU_MAG_X_CHAN] = sensors.mag.value.x;
+ ami601_values[IMU_MAG_Y_CHAN] = sensors.mag.value.y;
+ ami601_values[IMU_MAG_Z_CHAN] = sensors.mag.value.z;
+ ami601_status = AMI601_DATA_AVAILABLE;
+}
Copied: paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/imu_crista_arch.h
(from rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/arch/sim/imu_crista_arch.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/imu_crista_arch.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/arch/sim/imu_crista_arch.h
2010-10-23 21:11:30 UTC (rev 6211)
@@ -0,0 +1,41 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * 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.
+ */
+
+/*
+ *
+ * simulator plug for the crista imu arch dependant functions
+ *
+ */
+#ifndef IMU_CRISTA_ARCH_H
+#define IMU_CRISTA_ARCH_H
+
+#include <firmwares/rotorcraft/imu.h>
+
+
+#define ImuCristaArchPeriodic() {}
+
+extern void imu_feed_gyro_accel(void);
+extern void imu_feed_mag(void);
+
+
+#endif /* IMU_CRISTA_HW_H */
Copied:
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_aspirin_arch.c (from
rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_aspirin_arch.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_aspirin_arch.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_aspirin_arch.c
2010-10-23 21:11:30 UTC (rev 6211)
@@ -0,0 +1,294 @@
+#include <firmwares/rotorcraft/imu.h>
+
+#include <stm32/gpio.h>
+#include <stm32/misc.h>
+#include <stm32/rcc.h>
+#include <stm32/exti.h>
+#include <stm32/spi.h>
+#include <stm32/dma.h>
+
+#include "i2c.h"
+
+/* gyro int handler */
+void exti15_10_irq_handler(void);
+/* mag int handler */
+void exti9_5_irq_handler(void);
+/* accelerometer int handler */
+void exti2_irq_handler(void);
+/* accelerometer SPI selection */
+#define Adxl345Unselect() GPIOB->BSRR = GPIO_Pin_12
+#define Adxl345Select() GPIOB->BRR = GPIO_Pin_12
+/* accelerometer dma end of rx handler */
+void dma1_c4_irq_handler(void);
+
+void imu_aspirin_arch_init(void) {
+
+ GPIO_InitTypeDef GPIO_InitStructure;
+ EXTI_InitTypeDef EXTI_InitStructure;
+ NVIC_InitTypeDef NVIC_InitStructure;
+ SPI_InitTypeDef SPI_InitStructure;
+
+ /* Set "mag ss" and "mag reset" as floating inputs ------------------------*/
+ /* "mag ss" (PC12) is shorted to I2C2 SDA */
+ /* "mag reset" (PC13) is shorted to I2C2 SCL */
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(GPIOC, &GPIO_InitStructure);
+
+ /* Gyro
--------------------------------------------------------------------*/
+ /* set "eeprom ss" as floating input (on PC14) = gyro int
---------*/
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(GPIOC, &GPIO_InitStructure);
+ /* configure external interrupt exti15_10 on PC14( gyro int ) */
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(GPIOC, &GPIO_InitStructure);
+
+ GPIO_EXTILineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource14);
+ EXTI_InitStructure.EXTI_Line = EXTI_Line14;
+ EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
+ EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
+ EXTI_InitStructure.EXTI_LineCmd = ENABLE;
+ EXTI_Init(&EXTI_InitStructure);
+
+ NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+
+ /* Accel */
+ /* set accel slave select as output and assert it ( on PB12) */
+ Adxl345Unselect();
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+ /* configure external interrupt exti2 on PD2( accel int ) */
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD | RCC_APB2Periph_AFIO, ENABLE);
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(GPIOD, &GPIO_InitStructure);
+ GPIO_EXTILineConfig(GPIO_PortSourceGPIOD, GPIO_PinSource2);
+
+ EXTI_InitStructure.EXTI_Line = EXTI_Line2;
+ EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
+ EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
+ EXTI_InitStructure.EXTI_LineCmd = ENABLE;
+ EXTI_Init(&EXTI_InitStructure);
+
+ NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+ /* Enable SPI2 Periph clock
-------------------------------------------------*/
+ RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
+
+ /* Configure GPIOs: SCK, MISO and MOSI --------------------------------*/
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO , ENABLE);
+ SPI_Cmd(SPI2, ENABLE);
+
+ /* configure SPI */
+ SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
+ SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
+ SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
+ SPI_InitStructure.SPI_CPOL = SPI_CPOL_High;
+ SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;
+ SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
+ SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_32;
+ SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
+ SPI_InitStructure.SPI_CRCPolynomial = 7;
+ SPI_Init(SPI2, &SPI_InitStructure);
+
+ /* Enable DMA1 channel4 IRQ Channel ( SPI RX) */
+ NVIC_InitTypeDef NVIC_init_struct = {
+ .NVIC_IRQChannel = DMA1_Channel4_IRQn,
+ .NVIC_IRQChannelPreemptionPriority = 0,
+ .NVIC_IRQChannelSubPriority = 0,
+ .NVIC_IRQChannelCmd = ENABLE
+ };
+ NVIC_Init(&NVIC_init_struct);
+
+ /* Enable SPI_2 DMA clock
---------------------------------------------------*/
+ RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
+
+
+ /* Mag */
+ /* configure external interrupt exti5 on PB5( mag int ) */
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE);
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+ GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource5);
+ EXTI_InitStructure.EXTI_Line = EXTI_Line5;
+ EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
+ EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
+ EXTI_InitStructure.EXTI_LineCmd = ENABLE;
+ EXTI_Init(&EXTI_InitStructure);
+
+ NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+}
+
+
+void adxl345_write_to_reg(uint8_t addr, uint8_t val) {
+
+ Adxl345Select();
+ SPI_I2S_SendData(SPI2, addr);
+ while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
+ SPI_I2S_SendData(SPI2, val);
+ while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET);
+ while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_BSY) == SET);
+ Adxl345Unselect();
+
+}
+
+void adxl345_clear_rx_buf(void) {
+ uint8_t __attribute__ ((unused)) ret = SPI_I2S_ReceiveData(SPI2);
+}
+
+void adxl345_start_reading_data(void) {
+ Adxl345Select();
+
+ imu_aspirin.accel_tx_buf[0] = (1<<7|1<<6|ADXL345_REG_DATA_X0);
+
+ /* SPI2_Rx_DMA_Channel configuration ------------------------------------*/
+ DMA_DeInit(DMA1_Channel4);
+ DMA_InitTypeDef DMA_initStructure_4 = {
+ .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C),
+ .DMA_MemoryBaseAddr = (uint32_t)imu_aspirin.accel_rx_buf,
+ .DMA_DIR = DMA_DIR_PeripheralSRC,
+ .DMA_BufferSize = 7,
+ .DMA_PeripheralInc = DMA_PeripheralInc_Disable,
+ .DMA_MemoryInc = DMA_MemoryInc_Enable,
+ .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
+ .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
+ .DMA_Mode = DMA_Mode_Normal,
+ .DMA_Priority = DMA_Priority_VeryHigh,
+ .DMA_M2M = DMA_M2M_Disable
+ };
+ DMA_Init(DMA1_Channel4, &DMA_initStructure_4);
+
+ /* SPI2_Tx_DMA_Channel configuration ------------------------------------*/
+ DMA_DeInit(DMA1_Channel5);
+ DMA_InitTypeDef DMA_initStructure_5 = {
+ .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C),
+ .DMA_MemoryBaseAddr = (uint32_t)imu_aspirin.accel_tx_buf,
+ .DMA_DIR = DMA_DIR_PeripheralDST,
+ .DMA_BufferSize = 7,
+ .DMA_PeripheralInc = DMA_PeripheralInc_Disable,
+ .DMA_MemoryInc = DMA_MemoryInc_Enable,
+ .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
+ .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
+ .DMA_Mode = DMA_Mode_Normal,
+ .DMA_Priority = DMA_Priority_Medium,
+ .DMA_M2M = DMA_M2M_Disable
+ };
+ DMA_Init(DMA1_Channel5, &DMA_initStructure_5);
+
+ /* Enable SPI_2 Rx request */
+ SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, ENABLE);
+ /* Enable DMA1 Channel4 */
+ DMA_Cmd(DMA1_Channel4, ENABLE);
+
+ /* Enable SPI_2 Tx request */
+ SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Tx, ENABLE);
+ /* Enable DMA1 Channel5 */
+ DMA_Cmd(DMA1_Channel5, ENABLE);
+
+ /* Enable DMA1 Channel4 Transfer Complete interrupt */
+ DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, ENABLE);
+}
+
+/*
+ *
+ * Gyro data ready
+ *
+ */
+void exti15_10_irq_handler(void) {
+
+ /* clear EXTI */
+ if(EXTI_GetITStatus(EXTI_Line14) != RESET)
+ EXTI_ClearITPendingBit(EXTI_Line14);
+
+ imu_aspirin.i2c_trans_gyro.type = I2CTransTxRx;
+ imu_aspirin.i2c_trans_gyro.buf[0] = ITG3200_REG_GYRO_XOUT_H;
+ imu_aspirin.i2c_trans_gyro.slave_addr = ITG3200_ADDR;
+ imu_aspirin.i2c_trans_gyro.len_w = 1;
+ imu_aspirin.i2c_trans_gyro.len_r = 6;
+ // if (!i2c_submit(&i2c2,&imu_aspirin.i2c_trans_gyro)) while(1);
+ i2c_submit(&i2c2,&imu_aspirin.i2c_trans_gyro);
+ imu_aspirin.status = AspirinStatusReadingGyro;
+
+}
+
+/*
+ *
+ * Mag data ready
+ *
+ */
+void exti9_5_irq_handler(void) {
+
+ /* clear EXTI */
+ if(EXTI_GetITStatus(EXTI_Line5) != RESET)
+ EXTI_ClearITPendingBit(EXTI_Line5);
+
+ imu_aspirin.mag_ready_for_read = TRUE;
+
+}
+
+/*
+ *
+ * Accel data ready
+ *
+ */
+void exti2_irq_handler(void) {
+
+ /* clear EXTI */
+ if(EXTI_GetITStatus(EXTI_Line2) != RESET)
+ EXTI_ClearITPendingBit(EXTI_Line2);
+
+ adxl345_start_reading_data();
+
+}
+
+/*
+ *
+ * Accel end of DMA transfert
+ *
+ */
+void dma1_c4_irq_handler(void) {
+ Adxl345Unselect();
+ DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE);
+ /* Disable SPI_2 Rx and TX request */
+ SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, DISABLE);
+ SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Tx, DISABLE);
+ /* Disable DMA1 Channel4 and 5 */
+ DMA_Cmd(DMA1_Channel4, DISABLE);
+ DMA_Cmd(DMA1_Channel5, DISABLE);
+
+ imu_aspirin.accel_available = TRUE;
+}
Copied:
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_aspirin_arch.h (from
rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_aspirin_arch.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_aspirin_arch.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_aspirin_arch.h
2010-10-23 21:11:30 UTC (rev 6211)
@@ -0,0 +1,14 @@
+#ifndef IMU_ASPIRIN_ARCH_H
+#define IMU_ASPIRIN_ARCH_H
+
+#include <firmwares/rotorcraft/imu.h>
+
+#include "led.h"
+
+extern void imu_aspirin_arch_init(void);
+extern void adxl345_write_to_reg(uint8_t addr, uint8_t val);
+extern void adxl345_clear_rx_buf(void);
+extern void adxl345_start_reading_data(void);
+
+
+#endif /* IMU_ASPIRIN_ARCH_H */
Copied: paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_b2_arch.c
(from rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_b2_arch.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_b2_arch.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_b2_arch.c
2010-10-23 21:11:30 UTC (rev 6211)
@@ -0,0 +1,123 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 20010 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 <firmwares/rotorcraft/imu.h>
+
+#include <stm32/gpio.h>
+#include <stm32/rcc.h>
+#include <stm32/spi.h>
+#include <stm32/exti.h>
+#include <stm32/misc.h>
+#include <stm32/dma.h>
+
+#define IMU_SSP_STA_IDLE 0
+#define IMU_SSP_STA_BUSY_MAX1168 1
+#define IMU_SSP_STA_BUSY_MS2100 2
+
+volatile uint8_t imu_ssp_status;
+
+void dma1_c4_irq_handler(void);
+void spi2_irq_handler(void);
+
+void imu_b2_arch_init(void) {
+
+ /* Enable SPI2 Periph clock
-------------------------------------------------*/
+ RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
+ /* Enable SPI_2 DMA clock
---------------------------------------------------*/
+ RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
+ /* Enable PORTB GPIO clock
--------------------------------------------------*/
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO , ENABLE);
+ /* Configure GPIOs: SCK, MISO and MOSI
-------------------------------------*/
+ GPIO_InitTypeDef GPIO_InitStructure;
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
+ /* Enable DMA1 channel4 IRQ Channel */
+ NVIC_InitTypeDef NVIC_init_struct = {
+ .NVIC_IRQChannel = DMA1_Channel4_IRQn,
+ .NVIC_IRQChannelPreemptionPriority = 0,
+ .NVIC_IRQChannelSubPriority = 0,
+ .NVIC_IRQChannelCmd = ENABLE
+ };
+ NVIC_Init(&NVIC_init_struct);
+ /* Enable SPI2 IRQ Channel */
+ NVIC_InitTypeDef NVIC_init_structure_spi = {
+ .NVIC_IRQChannel = SPI2_IRQn,
+ .NVIC_IRQChannelPreemptionPriority = 0,
+ .NVIC_IRQChannelSubPriority = 1,
+ .NVIC_IRQChannelCmd = ENABLE
+ };
+ NVIC_Init(&NVIC_init_structure_spi);
+
+ imu_ssp_status = IMU_SSP_STA_IDLE;
+}
+
+void imu_periodic(void) {
+ // check ssp idle
+ // ASSERT((imu_status == IMU_STA_IDLE), DEBUG_IMU, IMU_ERR_OVERUN);
+ imu_ssp_status = IMU_SSP_STA_BUSY_MAX1168;
+ Max1168ConfigureSPI();
+ SPI_Cmd(SPI2, ENABLE);
+ booz_max1168_read();
+#if IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC5843
+ hmc5843_periodic();
+#endif
+}
+
+/* used for spi2 */
+void dma1_c4_irq_handler(void) {
+ switch (imu_ssp_status) {
+ case IMU_SSP_STA_BUSY_MAX1168:
+ Max1168OnDmaIrq();
+ SPI_Cmd(SPI2, DISABLE);
+#if IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
+ if (ms2001_status == MS2001_IDLE) {
+ Ms2001SendReq();
+ imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
+ }
+ else if (ms2001_status == MS2001_WAITING_EOC && Ms2001HasEOC()) {
+ Ms2001ReadRes();
+ imu_ssp_status = IMU_SSP_STA_BUSY_MS2100;
+ }
+ else
+#endif
+ imu_ssp_status = IMU_SSP_STA_IDLE;
+ break;
+ case IMU_SSP_STA_BUSY_MS2100:
+#if IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
+ Ms2001OnDmaIrq();
+#endif
+ break;
+ default:
+ // POST_ERROR(DEBUG_IMU, IMU_ERR_SUPRIOUS_DMA1_C4_IRQ);
+ imu_ssp_status = IMU_SSP_STA_IDLE;
+ }
+}
+
+
+void spi2_irq_handler(void) {
+#if IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
+ Ms2001OnSpiIrq();
+#endif
+}
Copied: paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_b2_arch.h
(from rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_b2_arch.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_b2_arch.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_b2_arch.h
2010-10-23 21:11:30 UTC (rev 6211)
@@ -0,0 +1,29 @@
+/*
+ * $Id: imu_b2_arch.h 3732 2009-07-20 17:46:54Z poine $
+ *
+ * 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 IMU_B2_ARCH_H
+#define IMU_B2_ARCH_H
+
+
+
+#endif /* IMU_B2_ARCH_H */
Copied:
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_crista_arch.c (from
rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_crista_arch.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_crista_arch.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_crista_arch.c
2010-10-23 21:11:30 UTC (rev 6211)
@@ -0,0 +1,176 @@
+/*
+ * $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 <firmwares/rotorcraft/imu.h>
+
+#include <stm32/gpio.h>
+#include <stm32/rcc.h>
+#include <stm32/spi.h>
+#include <stm32/misc.h>
+#include <stm32/dma.h>
+
+#include "stm32_vector_table.h"
+
+static volatile uint8_t channel;
+static uint8_t buf_in[4];
+static uint8_t buf_out[4];
+
+#define POWER_MODE (1 << 1 | 1)
+#define SGL_DIF 1 // Single ended
+
+#define ADS8344Unselect() GPIOB->BSRR = GPIO_Pin_12
+#define ADS8344Select() GPIOB->BRR = GPIO_Pin_12
+
+extern void dma1_c4_irq_handler(void);
+static void ADS8344_read_channel( void );
+
+void imu_crista_arch_init(void) {
+
+ channel = 0;
+ /* Enable SPI2 Periph clock
-------------------------------------------------*/
+ RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
+ /* Enable SPI_2 DMA clock
---------------------------------------------------*/
+ RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
+ /* Enable PORTB GPIO clock
--------------------------------------------------*/
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO , ENABLE);
+ /* Configure GPIOs: SCK, MISO and MOSI
-------------------------------------*/
+ GPIO_InitTypeDef GPIO_InitStructure;
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
+ /* set slave select as output and assert it ( on PB12) */
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(GPIOB, &GPIO_InitStructure);
+ ADS8344Unselect();
+ /* configure SPI after enabling it*/
+ SPI_Cmd(SPI2, ENABLE);
+ SPI_InitTypeDef SPI_InitStructure;
+ SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
+ SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
+ SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
+ SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
+ SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
+ SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
+ SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16;
+ SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
+ SPI_InitStructure.SPI_CRCPolynomial = 7;
+ SPI_Init(SPI2, &SPI_InitStructure);
+
+ /* Enable DMA1 channel4 IRQ Channel */
+ NVIC_InitTypeDef NVIC_init_struct = {
+ .NVIC_IRQChannel = DMA1_Channel4_IRQn,
+ .NVIC_IRQChannelPreemptionPriority = 0,
+ .NVIC_IRQChannelSubPriority = 0,
+ .NVIC_IRQChannelCmd = ENABLE
+ };
+ NVIC_Init(&NVIC_init_struct);
+
+}
+
+
+void ADS8344_start( void ) {
+
+ ADS8344Select();
+ channel = 0;
+ ADS8344_read_channel();
+
+}
+
+static void ADS8344_read_channel( void ) {
+
+ // control byte
+ buf_out[0] = 1 << 7 | channel << 4 | SGL_DIF << 2 | POWER_MODE;
+
+ /* trigger 4 bytes read */
+ /* SPI2_Rx_DMA_Channel configuration ------------------------------------*/
+ DMA_DeInit(DMA1_Channel4);
+ DMA_InitTypeDef DMA_initStructure_4 = {
+ .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C),
+ .DMA_MemoryBaseAddr = (uint32_t)buf_in,
+ .DMA_DIR = DMA_DIR_PeripheralSRC,
+ .DMA_BufferSize = 4,
+ .DMA_PeripheralInc = DMA_PeripheralInc_Disable,
+ .DMA_MemoryInc = DMA_MemoryInc_Enable,
+ .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
+ .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
+ .DMA_Mode = DMA_Mode_Normal,
+ .DMA_Priority = DMA_Priority_VeryHigh,
+ .DMA_M2M = DMA_M2M_Disable
+ };
+ DMA_Init(DMA1_Channel4, &DMA_initStructure_4);
+
+ /* SPI2_Tx_DMA_Channel configuration ------------------------------------*/
+ DMA_DeInit(DMA1_Channel5);
+ DMA_InitTypeDef DMA_initStructure_5 = {
+ .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C),
+ .DMA_MemoryBaseAddr = (uint32_t)buf_out,
+ .DMA_DIR = DMA_DIR_PeripheralDST,
+ .DMA_BufferSize = 4,
+ .DMA_PeripheralInc = DMA_PeripheralInc_Disable,
+ .DMA_MemoryInc = DMA_MemoryInc_Enable,
+ .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
+ .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
+ .DMA_Mode = DMA_Mode_Normal,
+ .DMA_Priority = DMA_Priority_Medium,
+ .DMA_M2M = DMA_M2M_Disable
+ };
+ DMA_Init(DMA1_Channel5, &DMA_initStructure_5);
+
+ /* Enable SPI_2 Rx request */
+ SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, ENABLE);
+ /* Enable DMA1 Channel4 */
+ DMA_Cmd(DMA1_Channel4, ENABLE);
+
+ /* Enable SPI_2 Tx request */
+ SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Tx, ENABLE);
+ /* Enable DMA1 Channel5 */
+ DMA_Cmd(DMA1_Channel5, ENABLE);
+
+ /* Enable DMA1 Channel4 Transfer Complete interrupt */
+ DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, ENABLE);
+
+}
+
+
+void dma1_c4_irq_handler(void) {
+
+ ADS8344_values[channel] = (buf_in[1] << 8 | buf_in[2]) << 1 | buf_in[3] >> 7;
+ channel++;
+ if (channel > 6) {
+ ADS8344_available = TRUE;
+ ADS8344Unselect();
+ DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE);
+ /* Disable SPI_2 Rx and TX request */
+ SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, DISABLE);
+ SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Tx, DISABLE);
+ /* Disable DMA1 Channel4 and 5 */
+ DMA_Cmd(DMA1_Channel4, DISABLE);
+ DMA_Cmd(DMA1_Channel5, DISABLE);
+ }
+ else {
+ ADS8344_read_channel();
+ }
+}
Copied:
paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_crista_arch.h (from
rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/arch/stm32/imu_crista_arch.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_crista_arch.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/arch/stm32/imu_crista_arch.h
2010-10-23 21:11:30 UTC (rev 6211)
@@ -0,0 +1,34 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 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 IMU_CRISTA_ARCH_H
+#define IMU_CRISTA_ARCH_H
+
+
+#define ImuCristaArchPeriodic() { \
+ ADS8344_start(); \
+ }
+
+extern void ADS8344_start( void );
+
+
+#endif /* IMU_CRISTA_ARCH_H */
Copied: paparazzi3/trunk/sw/airborne/subsystems/imu/imu.c (from rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/imu.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/imu.c 2010-10-23 21:11:30 UTC
(rev 6211)
@@ -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 "imu.h"
+
+#include "airframe.h"
+
+struct Imu imu;
+
+void imu_init(void) {
+
+ /* initialises neutrals */
+ RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL,
IMU_GYRO_R_NEUTRAL);
+ VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL,
IMU_ACCEL_Z_NEUTRAL);
+ VECT3_ASSIGN(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(imu.body_to_imu_quat, body_to_imu_eulers);
+ INT32_QUAT_NORMALISE(imu.body_to_imu_quat);
+ INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);
+#endif
+
+ imu_impl_init();
+}
Copied: paparazzi3/trunk/sw/airborne/subsystems/imu/imu.h (from rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/imu.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/imu.h 2010-10-23 21:11:30 UTC
(rev 6211)
@@ -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 IMU_H
+#define IMU_H
+
+#include "math/pprz_algebra_int.h"
+#include "math/pprz_algebra_float.h"
+
+/* must be defined by underlying hardware */
+extern void imu_impl_init(void);
+extern void imu_periodic(void);
+
+struct Imu {
+ 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 ImuFloat {
+ 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 IMU_TYPE_H
+#include IMU_TYPE_H
+#endif
+
+extern struct Imu imu;
+
+extern void imu_init(void);
+
+#define ImuScaleGyro(_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 ImuScaleAccel(_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 ImuScaleMag(_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 ImuScaleMag(_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 /* IMU_H */
Copied: paparazzi3/trunk/sw/airborne/subsystems/imu/imu_aspirin.c (from rev
6208, paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/imu_aspirin.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/imu_aspirin.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/imu_aspirin.c 2010-10-23
21:11:30 UTC (rev 6211)
@@ -0,0 +1,122 @@
+#include <firmwares/rotorcraft/imu.h>
+
+#include "i2c.h"
+
+struct ImuAspirin imu_aspirin;
+
+/* initialize peripherals */
+static void configure_gyro(void);
+static void configure_mag(void);
+static void configure_accel(void);
+
+
+void imu_impl_init(void) {
+
+ imu_aspirin.status = AspirinStatusUninit;
+ imu_aspirin.gyro_available = FALSE;
+ imu_aspirin.gyro_available_blaaa = FALSE;
+ imu_aspirin.mag_ready_for_read = FALSE;
+ imu_aspirin.mag_available = FALSE;
+ imu_aspirin.accel_available = FALSE;
+
+ imu_aspirin_arch_init();
+
+}
+
+
+void imu_periodic(void) {
+ if (imu_aspirin.status == AspirinStatusUninit) {
+ configure_gyro();
+ configure_mag();
+ configure_accel();
+ imu_aspirin.status = AspirinStatusIdle;
+ }
+ else
+ imu_aspirin.gyro_available_blaaa = TRUE;
+}
+
+
+/* sends a serie of I2C commands to configure the ITG3200 gyro */
+static void configure_gyro(void) {
+
+ struct i2c_transaction t;
+ t.type = I2CTransTx;
+ t.slave_addr = ITG3200_ADDR;
+ /* set gyro range to 2000deg/s and low pass at 256Hz */
+ t.buf[0] = ITG3200_REG_DLPF_FS;
+ t.buf[1] = (0x03<<3);
+ t.len_w = 2;
+ i2c_submit(&i2c2,&t);
+ while (t.status != I2CTransSuccess);
+ /* set sample rate to 533Hz */
+ t.buf[0] = ITG3200_REG_SMPLRT_DIV;
+ t.buf[1] = 0x0E;
+ i2c_submit(&i2c2,&t);
+ while (t.status != I2CTransSuccess);
+ /* switch to gyroX clock */
+ t.buf[0] = ITG3200_REG_PWR_MGM;
+ t.buf[1] = 0x01;
+ i2c_submit(&i2c2,&t);
+ while (t.status != I2CTransSuccess);
+ /* enable interrupt on data ready, idle hight */
+ t.buf[0] = ITG3200_REG_INT_CFG;
+ t.buf[1] = (0x01 | 0x01<<7);
+ i2c_submit(&i2c2,&t);
+ while (t.status != I2CTransSuccess);
+
+}
+
+/* sends a serie of I2C commands to configure the ITG3200 gyro */
+static void configure_mag(void) {
+
+ struct i2c_transaction t;
+ t.type = I2CTransTx;
+ t.slave_addr = HMC5843_ADDR;
+ /* set to rate to 50Hz */
+ t.buf[0] = HMC5843_REG_CFGA;
+ t.buf[1] = 0x00 | (0x06 << 2);
+ i2c_submit(&i2c2,&t);
+ while (t.status != I2CTransSuccess);
+ /* set to gain to 1 Gauss */
+ t.buf[0] = HMC5843_REG_CFGB;
+ t.buf[1] = 0x01<<5;
+ i2c_submit(&i2c2,&t);
+ while (t.status != I2CTransSuccess);
+ /* set to continuous mode */
+ t.buf[0] = HMC5843_REG_MODE;
+ t.buf[1] = 0x00;
+ i2c_submit(&i2c2,&t);
+ while (t.status != I2CTransSuccess);
+
+}
+
+
+static void send_i2c_msg_with_retry(struct i2c_transaction* t) {
+ uint8_t max_retry = 8;
+ uint8_t nb_retry = 0;
+ do {
+ i2c_submit(&i2c2,&t);
+ while (t.status == I2CTransPending || t.status == I2CTransRunning);
+ if (t.status == I2CTransFailed)
+ nb_retry++;
+ }
+ while (t.status != I2CTransSuccess || nb_retry < max_retry);
+}
+
+
+static void configure_accel(void) {
+
+ /* set data rate to 800Hz */
+ adxl345_write_to_reg(ADXL345_REG_BW_RATE, 0x0D);
+ /* switch to measurememnt mode */
+ adxl345_write_to_reg(ADXL345_REG_POWER_CTL, 1<<3);
+ /* enable data ready interrupt */
+ adxl345_write_to_reg(ADXL345_REG_INT_ENABLE, 1<<7);
+ /* Enable full res and interrupt active low */
+ adxl345_write_to_reg(ADXL345_REG_DATA_FORMAT, 1<<3|1<<5);
+ /* clear spi rx reg to make DMA happy */
+ adxl345_clear_rx_buf();
+ /* reads data once to bring interrupt line up */
+ adxl345_start_reading_data();
+
+}
Copied: paparazzi3/trunk/sw/airborne/subsystems/imu/imu_aspirin.h (from rev
6208, paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/imu_aspirin.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/imu_aspirin.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/imu_aspirin.h 2010-10-23
21:11:30 UTC (rev 6211)
@@ -0,0 +1,131 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 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 IMU_ASPIRIN_H
+#define IMU_ASPIRIN_H
+
+#include "airframe.h"
+#include <firmwares/rotorcraft/imu.h>
+
+#include "i2c.h"
+#include "booz/peripherals/booz_itg3200.h"
+#include "booz/peripherals/booz_hmc5843.h"
+#include "booz/peripherals/booz_adxl345.h"
+
+
+#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
+
+enum AspirinStatus
+ { AspirinStatusUninit,
+ AspirinStatusIdle,
+ AspirinStatusReadingGyro,
+ AspirinStatusReadingMag
+ };
+
+struct ImuAspirin {
+ volatile enum AspirinStatus status;
+ struct i2c_transaction i2c_trans_gyro;
+ struct i2c_transaction i2c_trans_mag;
+ uint8_t gyro_available;
+ uint8_t gyro_available_blaaa;
+ uint8_t mag_available;
+ volatile uint8_t mag_ready_for_read;
+ volatile uint8_t accel_available;
+ volatile uint8_t accel_tx_buf[7];
+ volatile uint8_t accel_rx_buf[7];
+};
+
+extern struct ImuAspirin imu_aspirin;
+
+#define ImuMagEvent(_mag_handler) {}
+
+
+#define ImuEvent(_gyro_accel_handler, _mag_handler) { \
+ if (imu_aspirin.status == AspirinStatusReadingGyro && \
+ imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) { \
+ int16_t gp = imu_aspirin.i2c_trans_gyro.buf[0]<<8 |
imu_aspirin.i2c_trans_gyro.buf[1]; \
+ int16_t gq = imu_aspirin.i2c_trans_gyro.buf[2]<<8 |
imu_aspirin.i2c_trans_gyro.buf[3]; \
+ int16_t gr = imu_aspirin.i2c_trans_gyro.buf[4]<<8 |
imu_aspirin.i2c_trans_gyro.buf[5]; \
+ RATES_ASSIGN(imu.gyro_unscaled, gp, gq, gr); \
+ if (imu_aspirin.mag_ready_for_read ) { \
+ /* read mag */ \
+ imu_aspirin.i2c_trans_mag.type = I2CTransRx; \
+ imu_aspirin.i2c_trans_mag.slave_addr = HMC5843_ADDR; \
+ imu_aspirin.i2c_trans_mag.len_r = 7; \
+ i2c_submit(&i2c2,&imu_aspirin.i2c_trans_mag); \
+ imu_aspirin.mag_ready_for_read = FALSE; \
+ imu_aspirin.status = AspirinStatusReadingMag; \
+ }
\
+ else { \
+ imu_aspirin.status = AspirinStatusIdle; \
+ }
\
+ } \
+ if (imu_aspirin.status == AspirinStatusReadingMag && \
+ imu_aspirin.i2c_trans_mag.status == I2CTransSuccess) { \
+ int16_t mx = imu_aspirin.i2c_trans_mag.buf[0]<<8 |
imu_aspirin.i2c_trans_mag.buf[1]; \
+ int16_t my = imu_aspirin.i2c_trans_mag.buf[2]<<8 |
imu_aspirin.i2c_trans_mag.buf[3]; \
+ int16_t mz = imu_aspirin.i2c_trans_mag.buf[4]<<8 |
imu_aspirin.i2c_trans_mag.buf[5]; \
+ VECT3_ASSIGN(imu.mag_unscaled, mx, my, mz); \
+ imu_aspirin.mag_available = TRUE;
\
+ imu_aspirin.status = AspirinStatusIdle; \
+ \
+ } \
+ if (imu_aspirin.gyro_available_blaaa) { \
+ imu_aspirin.gyro_available_blaaa = FALSE;
\
+ _gyro_accel_handler(); \
+ } \
+ if (imu_aspirin.mag_available) { \
+ imu_aspirin.mag_available = FALSE; \
+ _mag_handler(); \
+ } \
+ if (imu_aspirin.accel_available) { \
+ imu_aspirin.accel_available = FALSE; \
+ const int16_t ax = imu_aspirin.accel_rx_buf[1] |
(imu_aspirin.accel_rx_buf[2]<<8); \
+ const int16_t ay = imu_aspirin.accel_rx_buf[3] |
(imu_aspirin.accel_rx_buf[4]<<8); \
+ const int16_t az = imu_aspirin.accel_rx_buf[5] |
(imu_aspirin.accel_rx_buf[6]<<8); \
+ VECT3_ASSIGN(imu.accel_unscaled, ax, ay, az); \
+ _gyro_accel_handler(); \
+ } \
+ }
+
+
+/* underlying architecture */
+#include "imu_aspirin_arch.h"
+/* must be implemented by underlying architecture */
+extern void imu_b2_arch_init(void);
+
+#endif /* IMU_ASPIRIN_H */
Copied: paparazzi3/trunk/sw/airborne/subsystems/imu/imu_b2.c (from rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/imu_b2.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/imu_b2.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/imu_b2.c 2010-10-23
21:11:30 UTC (rev 6211)
@@ -0,0 +1,39 @@
+/*
+ * $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 <firmwares/rotorcraft/imu.h>
+
+void imu_impl_init(void) {
+
+ imu_b2_arch_init();
+
+ booz_max1168_init();
+#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
+ ms2001_init();
+#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
+ ami601_init();
+#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC5843
+ hmc5843_init();
+#endif
+
+}
Copied: paparazzi3/trunk/sw/airborne/subsystems/imu/imu_b2.h (from rev 6208,
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/imu_b2.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/imu_b2.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/imu_b2.h 2010-10-23
21:11:30 UTC (rev 6211)
@@ -0,0 +1,208 @@
+/*
+ * $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 IMU_B2_H
+#define IMU_B2_H
+
+#include <firmwares/rotorcraft/imu.h>
+#include "airframe.h"
+
+#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
+
+
+#ifdef IMU_B2_VERSION_1_0
+/* Default IMU b2 sensors connection */
+#if !defined IMU_GYRO_P_CHAN & !defined IMU_GYRO_Q_CHAN & !defined
IMU_GYRO_R_CHAN
+#define IMU_GYRO_P_CHAN 1
+#define IMU_GYRO_Q_CHAN 0
+#define IMU_GYRO_R_CHAN 2
+#endif
+#if !defined IMU_ACCEL_X_CHAN & !defined IMU_ACCEL_Y_CHAN & !defined
IMU_ACCEL_Z_CHAN
+#define IMU_ACCEL_X_CHAN 3
+#define IMU_ACCEL_Y_CHAN 5
+#define IMU_ACCEL_Z_CHAN 6
+#endif
+#if !defined IMU_MAG_X_CHAN & !defined IMU_MAG_Y_CHAN & !defined IMU_MAG_Z_CHAN
+#define IMU_MAG_X_CHAN 0
+#define IMU_MAG_Y_CHAN 1
+#define IMU_MAG_Z_CHAN 2
+#endif
+
+#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
+#endif /* IMU_B2_VERSION_1_0 */
+
+
+#ifdef IMU_B2_VERSION_1_1
+/* Default IMU b2 sensors connection */
+#if !defined IMU_GYRO_P_CHAN & !defined IMU_GYRO_Q_CHAN & !defined
IMU_GYRO_R_CHAN
+#define IMU_GYRO_P_CHAN 1
+#define IMU_GYRO_Q_CHAN 0
+#define IMU_GYRO_R_CHAN 2
+#endif
+#if !defined IMU_ACCEL_X_CHAN & !defined IMU_ACCEL_Y_CHAN & !defined
IMU_ACCEL_Z_CHAN
+#define IMU_ACCEL_X_CHAN 5
+#define IMU_ACCEL_Y_CHAN 3
+#define IMU_ACCEL_Z_CHAN 4
+#endif
+#if !defined IMU_MAG_X_CHAN & !defined IMU_MAG_Y_CHAN & !defined IMU_MAG_Z_CHAN
+#define IMU_MAG_X_CHAN 0
+#define IMU_MAG_Y_CHAN 1
+#define IMU_MAG_Z_CHAN 2
+#endif
+
+#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
+#endif /* IMU_B2_VERSION_1_1 */
+
+#ifdef IMU_B2_VERSION_1_2
+/* Default IMU b2 sensors connection */
+#if !defined IMU_GYRO_P_CHAN & !defined IMU_GYRO_Q_CHAN & !defined
IMU_GYRO_R_CHAN
+#define IMU_GYRO_P_CHAN 1
+#define IMU_GYRO_Q_CHAN 0
+#define IMU_GYRO_R_CHAN 2
+#endif
+#if !defined IMU_ACCEL_X_CHAN & !defined IMU_ACCEL_Y_CHAN & !defined
IMU_ACCEL_Z_CHAN
+#define IMU_ACCEL_X_CHAN 4
+#define IMU_ACCEL_Y_CHAN 5
+#define IMU_ACCEL_Z_CHAN 3
+#endif
+#if !defined IMU_MAG_X_CHAN & !defined IMU_MAG_Y_CHAN & !defined IMU_MAG_Z_CHAN
+#define IMU_MAG_X_CHAN 0
+#define IMU_MAG_Y_CHAN 1
+#define IMU_MAG_Z_CHAN 2
+#endif
+
+#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
+#endif /* IMU_B2_VERSION_1_2 */
+
+
+#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2001
+#include "peripherals/booz_ms2001.h"
+#define ImuMagEvent(_mag_handler) { \
+ if (ms2001_status == MS2001_DATA_AVAILABLE) { \
+ imu.mag_unscaled.x = ms2001_values[IMU_MAG_X_CHAN]; \
+ imu.mag_unscaled.y = ms2001_values[IMU_MAG_Y_CHAN]; \
+ imu.mag_unscaled.z = ms2001_values[IMU_MAG_Z_CHAN]; \
+ ms2001_status = MS2001_IDLE; \
+ _mag_handler(); \
+ } \
+ }
+#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
+#include "peripherals/booz_ami601.h"
+#define foo_handler() {}
+#define ImuMagEvent(_mag_handler) { \
+ AMI601Event(foo_handler); \
+ if (ami601_status == AMI601_DATA_AVAILABLE) { \
+ imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; \
+ imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN]; \
+ imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN]; \
+ ami601_status = AMI601_IDLE; \
+ _mag_handler(); \
+ } \
+ }
+#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC5843
+#include "peripherals/booz_hmc5843.h"
+#define foo_handler() {}
+#define ImuMagEvent(_mag_handler) { \
+ MagEvent(foo_handler); \
+ if (hmc5843.data_available) { \
+ imu.mag_unscaled.x = hmc5843.data.value[IMU_MAG_X_CHAN]; \
+ imu.mag_unscaled.y = hmc5843.data.value[IMU_MAG_Y_CHAN]; \
+ imu.mag_unscaled.z = hmc5843.data.value[IMU_MAG_Z_CHAN]; \
+ _mag_handler(); \
+ hmc5843.data_available = FALSE; \
+ } \
+ }
+#else
+#define ImuMagEvent(_mag_handler) {}
+#endif
+
+
+#define ImuEvent(_gyro_accel_handler, _mag_handler) { \
+ if (booz_max1168_status == STA_MAX1168_DATA_AVAILABLE) { \
+ imu.gyro_unscaled.p = booz_max1168_values[IMU_GYRO_P_CHAN]; \
+ imu.gyro_unscaled.q = booz_max1168_values[IMU_GYRO_Q_CHAN]; \
+ imu.gyro_unscaled.r = booz_max1168_values[IMU_GYRO_R_CHAN]; \
+ imu.accel_unscaled.x = booz_max1168_values[IMU_ACCEL_X_CHAN]; \
+ imu.accel_unscaled.y = booz_max1168_values[IMU_ACCEL_Y_CHAN]; \
+ imu.accel_unscaled.z = booz_max1168_values[IMU_ACCEL_Z_CHAN]; \
+ booz_max1168_status = STA_MAX1168_IDLE; \
+ _gyro_accel_handler(); \
+ } \
+ ImuMagEvent(_mag_handler); \
+ }
+
+
+/* underlying architecture */
+#include "imu_b2_arch.h"
+/* must be implemented by underlying architecture */
+extern void imu_b2_arch_init(void);
+
+
+#endif /* IMU_B2_H */
Copied: paparazzi3/trunk/sw/airborne/subsystems/imu/imu_crista.c (from rev
6208, paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/imu_crista.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/imu_crista.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/imu_crista.c 2010-10-23
21:11:30 UTC (rev 6211)
@@ -0,0 +1,48 @@
+/*
+ * $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 <firmwares/rotorcraft/imu.h>
+
+volatile bool_t ADS8344_available;
+uint16_t ADS8344_values[ADS8344_NB_CHANNELS];
+
+void imu_impl_init(void) {
+
+ ADS8344_available = FALSE;
+
+ imu_crista_arch_init();
+
+#ifdef USE_AMI601
+ ami601_init();
+#endif
+
+}
+
+void imu_periodic(void) {
+
+ ImuCristaArchPeriodic();
+#ifdef USE_AMI601
+ RunOnceEvery(10, { ami601_read(); });
+#endif
+
+}
Copied: paparazzi3/trunk/sw/airborne/subsystems/imu/imu_crista.h (from rev
6208, paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/imu/imu_crista.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu/imu_crista.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu/imu_crista.h 2010-10-23
21:11:30 UTC (rev 6211)
@@ -0,0 +1,73 @@
+/*
+ * $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 IMU_CRISTA_H
+#define IMU_CRISTA_H
+
+#include <firmwares/rotorcraft/imu.h>
+#include "airframe.h"
+
+#define ADS8344_NB_CHANNELS 8
+extern uint16_t ADS8344_values[ADS8344_NB_CHANNELS];
+extern volatile bool_t ADS8344_available;
+
+#define ImuEvent(_gyro_accel_handler, _mag_handler) { \
+ if (ADS8344_available) { \
+ ADS8344_available = FALSE; \
+ imu.gyro_unscaled.p = ADS8344_values[IMU_GYRO_P_CHAN]; \
+ imu.gyro_unscaled.q = ADS8344_values[IMU_GYRO_Q_CHAN]; \
+ imu.gyro_unscaled.r = ADS8344_values[IMU_GYRO_R_CHAN]; \
+ imu.accel_unscaled.x = ADS8344_values[IMU_ACCEL_X_CHAN]; \
+ imu.accel_unscaled.y = ADS8344_values[IMU_ACCEL_Y_CHAN]; \
+ imu.accel_unscaled.z = ADS8344_values[IMU_ACCEL_Z_CHAN]; \
+ /* spare 3, temp 7 */ \
+ _gyro_accel_handler(); \
+ } \
+ ImuMagEvent(_mag_handler); \
+ }
+
+#ifdef USE_AMI601
+#include "peripherals/booz_ami601.h"
+#define foo_handler() {}
+#define ImuMagEvent(_mag_handler) { \
+ AMI601Event(foo_handler); \
+ if (ami601_status == AMI601_DATA_AVAILABLE) { \
+ imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; \
+ imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN]; \
+ imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN]; \
+ ami601_status = AMI601_IDLE; \
+ _mag_handler(); \
+ } \
+ }
+#else
+#define ImuMagEvent(_mag_handler) {}
+#endif
+
+/* underlying architecture */
+#include "imu_crista_arch.h"
+/* must be defined by underlying architecture */
+extern void imu_crista_arch_init(void);
+
+
+
+#endif /* IMU_CRISTA_H */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6211] move firmwares/rotorcraft/imu to a general subsystems,
Felix Ruess <=