[paparazzi-commits] [5168] adding preliminary aspirin IMU support

[antoine drouin]

Revision: 5168
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5168
Author:   poine
Date:     2010-07-26 21:21:23 +0000 (Mon, 26 Jul 2010)
Log Message:
-----------
adding preliminary aspirin IMU support

Added Paths:
-----------
    paparazzi3/trunk/sw/airborne/booz/imu/booz_imu_aspirin.c
    paparazzi3/trunk/sw/airborne/booz/imu/booz_imu_aspirin.h

Added: paparazzi3/trunk/sw/airborne/booz/imu/booz_imu_aspirin.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/imu/booz_imu_aspirin.c                    
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/imu/booz_imu_aspirin.c    2010-07-26 
21:21:23 UTC (rev 5168)
@@ -0,0 +1,96 @@
+#include "booz_imu.h"
+
+#include "i2c.h"
+
+struct BoozImuAspirin imu_aspirin;
+
+/* initialize peripherals */
+static void configure_gyro(void);
+static void configure_mag(void);
+static void configure_accel(void);
+
+
+void booz_imu_impl_init(void) {
+
+  imu_aspirin.status = AspirinStatusUninit;
+  imu_aspirin.i2c_done = FALSE;
+  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;
+  
+  booz_imu_aspirin_arch_init();
+
+}
+
+
+void booz_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) {
+  
+  /* set gyro range to 2000deg/s and low pass at 256Hz */
+  i2c2.buf[0] = ITG3200_REG_DLPF_FS;
+  i2c2.buf[1] = 0x03;
+  i2c2_transmit(ITG3200_ADDR, 2, &imu_aspirin.i2c_done);
+  while (!imu_aspirin.i2c_done);
+  /* switch to gyroX clock */
+  i2c2.buf[0] = ITG3200_REG_PWR_MGM;
+  i2c2.buf[1] = 0x01;
+  i2c2_transmit(ITG3200_ADDR, 2, &imu_aspirin.i2c_done);
+  while (!imu_aspirin.i2c_done);
+  /* enable interrupt on data ready, idle hight */
+  i2c2.buf[0] = ITG3200_REG_INT_CFG;
+  i2c2.buf[1] = (0x01 | 0x01<<7);
+  i2c2_transmit(ITG3200_ADDR, 2, &imu_aspirin.i2c_done);
+  while (!imu_aspirin.i2c_done);
+
+}
+
+/* sends a serie of I2C commands to configure the ITG3200 gyro */
+static void configure_mag(void) {
+
+  /* set to rate to 50Hz */
+  i2c2.buf[0] = HMC5843_REG_CFGA; 
+  i2c2.buf[1] = 0x00 | (0x06 << 2);
+  i2c2_transmit(HMC5843_ADDR, 2, &imu_aspirin.i2c_done);
+  while (!imu_aspirin.i2c_done);
+  /* set to gain to 1 Gauss */
+  i2c2.buf[0] = HMC5843_REG_CFGB;
+  i2c2.buf[1] = 0x01<<5;
+  i2c2_transmit(HMC5843_ADDR, 2, &imu_aspirin.i2c_done);
+  while (!imu_aspirin.i2c_done);
+  /* set to continuous mode */
+  i2c2.buf[0] = HMC5843_REG_MODE; 
+  i2c2.buf[1] = 0x00;
+  i2c2_transmit(HMC5843_ADDR, 2, &imu_aspirin.i2c_done);
+  while (!imu_aspirin.i2c_done);
+
+}
+
+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();
+}

Added: paparazzi3/trunk/sw/airborne/booz/imu/booz_imu_aspirin.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/imu/booz_imu_aspirin.h                    
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/imu/booz_imu_aspirin.h    2010-07-26 
21:21:23 UTC (rev 5168)
@@ -0,0 +1,108 @@
+/*
+ * $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 BOOZ_IMU_ASPIRIN_H
+#define BOOZ_IMU_ASPIRIN_H
+
+#include "airframe.h"
+#include "booz_imu.h"
+
+#include "peripherals/booz_itg3200.h"
+#include "peripherals/booz_hmc5843.h"
+#include "peripherals/booz_adxl345.h"
+
+enum AspirinStatus 
+  { AspirinStatusUninit,
+    AspirinStatusIdle,
+    AspirinStatusReadingGyro,
+    AspirinStatusReadingMag
+  };
+
+struct BoozImuAspirin {
+  volatile enum AspirinStatus status;
+  volatile uint8_t i2c_done;
+  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 BoozImuAspirin imu_aspirin;
+
+#define BoozImuMagEvent(_mag_handler) {}
+
+
+#define BoozImuEvent(_gyro_accel_handler, _mag_handler) {              \
+    if (imu_aspirin.status == AspirinStatusReadingGyro && 
imu_aspirin.i2c_done) { \
+      int16_t gp = i2c2.buf[0]<<8 | i2c2.buf[1];                       \
+      int16_t gq = i2c2.buf[2]<<8 | i2c2.buf[3];                       \
+      int16_t gr = i2c2.buf[4]<<8 | i2c2.buf[5];                       \
+      RATES_ASSIGN(booz_imu.gyro_unscaled, gp, gq, gr);                        
\
+      if (imu_aspirin.mag_ready_for_read ) {                           \
+       /* read mag */                                                  \
+       i2c2_receive(HMC5843_ADDR, 7, &imu_aspirin.i2c_done);           \
+       imu_aspirin.mag_ready_for_read = FALSE;                         \
+       imu_aspirin.status = AspirinStatusReadingMag;                   \
+      }                                                                        
\
+      else {                                                           \
+       imu_aspirin.status = AspirinStatusIdle;                         \
+      }                                                                        
\
+    }                                                                  \
+    if (imu_aspirin.status == AspirinStatusReadingMag && imu_aspirin.i2c_done) 
{ \
+      int16_t mx   = i2c2.buf[0]<<8 | i2c2.buf[1];                     \
+      int16_t my   = i2c2.buf[2]<<8 | i2c2.buf[3];                     \
+      int16_t mz   = i2c2.buf[4]<<8 | i2c2.buf[5];                     \
+      VECT3_ASSIGN(booz_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(booz_imu.accel_unscaled, ax, ay, az);               \
+      _gyro_accel_handler();                                           \
+    }                                                                  \
+  }
+
+
+/* underlying architecture */
+#include "imu/booz_imu_aspirin_arch.h"
+/* must be implemented by underlying architecture */
+extern void booz_imu_b2_arch_init(void);
+
+#endif /* BOOZ_IMU_ASPIRIN_H */
+