diff --git a/sw/airborne/booz/imu/booz_imu_aspirin.c b/sw/airborne/booz/imu/booz_imu_aspirin.c new file mode 100644 index 0000000000..485e8eb5d3 --- /dev/null +++ b/sw/airborne/booz/imu/booz_imu_aspirin.c @@ -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(); +} diff --git a/sw/airborne/booz/imu/booz_imu_aspirin.h b/sw/airborne/booz/imu/booz_imu_aspirin.h new file mode 100644 index 0000000000..71d817aba7 --- /dev/null +++ b/sw/airborne/booz/imu/booz_imu_aspirin.h @@ -0,0 +1,108 @@ +/* + * $Id$ + * + * Copyright (C) 2010 Antoine Drouin + * + * 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 */ +