diff --git a/conf/autopilot/subsystems/fixedwing/imu_umarim.makefile b/conf/autopilot/subsystems/fixedwing/imu_umarim.makefile new file mode 100644 index 0000000000..a31c9f9be2 --- /dev/null +++ b/conf/autopilot/subsystems/fixedwing/imu_umarim.makefile @@ -0,0 +1,15 @@ + +IMU_UMARIM_CFLAGS = -DUSE_IMU +IMU_UMARIM_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_umarim.h\" + +IMU_UMARIM_SRCS = $(SRC_SUBSYSTEMS)/imu.c \ + $(SRC_SUBSYSTEMS)/imu/imu_umarim.c + + +IMU_UMARIM_CFLAGS += -DUSE_I2C +IMU_UMARIM_CFLAGS += -DUSE_I2C1 +IMU_UMARIM_CFLAGS += -DIMU_UMARIM_I2C_DEVICE=i2c1 + +ap.CFLAGS += $(IMU_UMARIM_CFLAGS) +ap.srcs += $(IMU_UMARIM_SRCS) + diff --git a/conf/boards/umarim_1.0.makefile b/conf/boards/umarim_1.0.makefile new file mode 100644 index 0000000000..793cc91778 --- /dev/null +++ b/conf/boards/umarim_1.0.makefile @@ -0,0 +1,44 @@ +# +# unami_1.0.makefile +# +# prototype for Umarim board +# + +ARCH=lpc21 + +BOARD=umarim +BOARD_VERSION=1.0 + +BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\" + +ifndef FLASH_MODE +FLASH_MODE = IAP +endif + + +LPC21ISP_BAUD = 38400 +LPC21ISP_XTAL = 12000 + + +### default settings +ifndef GPS_PORT +GPS_PORT = UART0 +endif +ifndef GPS_BAUD +GPS_BAUD = B38400 +endif +GPS_LED = 2 + +ifndef MODEM_PORT +MODEM_PORT = UART1 +endif +ifndef MODEM_BAUD +MODEM_BAUD = B57600 +endif + +ADC_GENERIC_NB_SAMPLES = 16 + +# All targets on the Umarim board run on the same processor achitecture +$(TARGET).ARCHDIR = $(ARCH) + +ACTUATORS = actuators_4017 diff --git a/sw/airborne/boards/umarim_1.0.h b/sw/airborne/boards/umarim_1.0.h new file mode 100644 index 0000000000..a2d343252f --- /dev/null +++ b/sw/airborne/boards/umarim_1.0.h @@ -0,0 +1,68 @@ +#ifndef CONFIG_UMARIM_V1_0_H +#define CONFIG_UMARIM_V1_0_H + +/* Master oscillator freq. */ +#define FOSC (12000000) + +/* PLL multiplier */ +#define PLL_MUL (5) + +/* CPU clock freq. */ +#define CCLK (FOSC * PLL_MUL) + +/* Peripheral bus speed mask 0x00->4, 0x01-> 1, 0x02 -> 2 */ +#define PBSD_BITS 0x02 +#define PBSD_VAL 2 + +/* Peripheral bus clock freq. */ +#define PCLK (CCLK / PBSD_VAL) + +/* Onboard LEDs */ +#define LED_1_BANK 1 +#define LED_1_PIN 25 + +#define LED_2_BANK 1 +#define LED_2_PIN 24 + +/* P0.5 aka MAT0.1 */ +#define SERVO_CLOCK_PIN 5 +#define SERVO_CLOCK_PINSEL PINSEL0 +#define SERVO_CLOCK_PINSEL_VAL 0x02 +#define SERVO_CLOCK_PINSEL_BIT 10 +/* p1.20 */ +#define SERVO_RESET_PIN 20 + +/* PPM : rc rx on P0.28 ( CAP0.2 ) */ +#define PPM_PINSEL PINSEL1 +#define PPM_PINSEL_VAL 0x02 +#define PPM_PINSEL_BIT 24 +#define PPM_CRI TIR_CR2I +#define PPM_CCR_CRF TCCR_CR2_F +#define PPM_CCR_CRR TCCR_CR2_R +#define PPM_CCR_CRI TCCR_CR2_I +#define PPM_CR T0CR2 + +/* ADC */ + +/* battery */ +#define ADC_CHANNEL_VSUPPLY AdcBank0(2) +#ifndef USE_AD0 +#define USE_AD0 +#endif +#define USE_AD0_2 + +#define DefaultVoltageOfAdc(adc) (0.01787109375*adc) + +/* SPI (SSP) */ +#define SPI_SELECT_SLAVE0_PORT 0 +#define SPI_SELECT_SLAVE0_PIN 20 + +#define SPI1_DRDY_PINSEL PINSEL1 +#define SPI1_DRDY_PINSEL_BIT 0 +#define SPI1_DRDY_PINSEL_VAL 1 +#define SPI1_DRDY_EINT 0 +#define SPI1_DRDY_VIC_IT VIC_EINT0 + +#define BOARD_HAS_BARO + +#endif /* CONFIG_UMARIM_V1_0_H */ diff --git a/sw/airborne/peripherals/adxl345.h b/sw/airborne/peripherals/adxl345.h index 5e0b5559ef..24b399e1b7 100644 --- a/sw/airborne/peripherals/adxl345.h +++ b/sw/airborne/peripherals/adxl345.h @@ -1,9 +1,11 @@ #ifndef ADXL345_H #define ADXL345_H +/* default I2C address */ #define ADXL345_ADDR 0xA6 #define ADXL345_ADDR_ALT 0x3A +/* Registers */ #define ADXL345_REG_BW_RATE 0x2C #define ADXL345_REG_POWER_CTL 0x2D #define ADXL345_REG_INT_ENABLE 0x2E diff --git a/sw/airborne/peripherals/itg3200.h b/sw/airborne/peripherals/itg3200.h index 38365c32fe..a95439a736 100644 --- a/sw/airborne/peripherals/itg3200.h +++ b/sw/airborne/peripherals/itg3200.h @@ -2,7 +2,8 @@ #define ITG3200 /* default I2C address */ -#define ITG3200_ADDR 0xD0 +#define ITG3200_ADDR 0xD0 +#define ITG3200_ADDR_ALT 0xD1 /* Registers */ #define ITG3200_REG_WHO_AM_I 0X00 diff --git a/sw/airborne/subsystems/imu/imu_umarim.c b/sw/airborne/subsystems/imu/imu_umarim.c new file mode 100644 index 0000000000..da46b220d4 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_umarim.c @@ -0,0 +1,214 @@ +/* + * Copyright (C) 2011 Gautier Hattenberger + * Derived from Aspirin and ppzuavimu drivers + * + * 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 +#include "subsystems/imu/imu_umarim.h" +#include "mcu_periph/i2c.h" +#include "led.h" + +// Downlink +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + + +// Peripherials +#include "peripherals/itg3199.h" +#include "peripherals/hmc5843.h" + +// Results +volatile bool_t gyr_valid; +volatile bool_t acc_valid; + +// Communication +struct i2c_transaction imu_umarim_itg3200; +struct i2c_transaction imu_umarim_adxl345; + +#ifndef PERIODIC_FREQUENCY +#define PERIODIC_FREQUENCY 60 +#endif + +void imu_impl_init(void) +{ + ///////////////////////////////////////////////////////////////////// + // ITG3200 + imu_umarim_itg3200.type = I2CTransTx; + imu_umarim_itg3200.slave_addr = ITG3200_ADDR_ALT; + imu_umarim_itg3200.buf[0] = ITG3200_REG_DLPF_FS; +#if PERIODIC_FREQUENCY == 60 + /* set gyro range to 2000deg/s and low pass at 20Hz (< 60Hz/2) internal sampling at 1kHz */ + imu_umarim_itg3200.buf[1] = (0x03<<3) | (0x04<<0); +# warning ITG3200 read at 50Hz +#else +# if PERIODIC_FREQUENCY == 120 +# warning ITG3200 read at 100Hz + /* set gyro range to 2000deg/s and low pass at 42Hz (< 120Hz/2) internal sampling at 1kHz */ + imu_umarim_itg3200.buf[1] = (0x03<<3) | (0x03<<0); +# else +# error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates +# endif +#endif + imu_umarim_itg3200.len_w = 2; + i2c_submit(&IMU_UMARIM_I2C_DEVICE,&imu_umarim_itg3200); + while(imu_umarim_itg3200.status == I2CTransPending); + + /* set sample rate to 66Hz: so at 60Hz there is always a new sample ready and you loose little */ + imu_umarim_itg3200.buf[0] = ITG3200_REG_SMPLRT_DIV; +#if PERIODIC_FREQUENCY == 60 + imu_umarim_itg3200.buf[1] = 19; // 50Hz +#else + imu_umarim_itg3200.buf[1] = 9; // 100Hz +#endif + i2c_submit(&IMU_UMARIM_I2C_DEVICE,&imu_umarim_itg3200); + while(imu_umarim_itg3200.status == I2CTransPending); + + /* switch to gyroX clock */ + imu_umarim_itg3200.buf[0] = ITG3200_REG_PWR_MGM; + imu_umarim_itg3200.buf[1] = 0x01; + i2c_submit(&IMU_UMARIM_I2C_DEVICE,&imu_umarim_itg3200); + while(imu_umarim_itg3200.status == I2CTransPending); + + /* no interrupts for now, but set data ready interrupt to enable reading status bits */ + imu_umarim_itg3200.buf[0] = ITG3200_REG_INT_CFG; + imu_umarim_itg3200.buf[1] = 0x00; + i2c_submit(&IMU_UMARIM_I2C_DEVICE,&imu_umarim_itg3200); + while(imu_umarim_itg3200.status == I2CTransPending); + + ///////////////////////////////////////////////////////////////////// + // ADXL345 + + /* set data rate to 100Hz */ + imu_umarim_adxl345.slave_addr = ADXL345_ADDR_ALT; + imu_umarim_adxl345.type = I2CTransTx; + imu_umarim_adxl345.buf[0] = ADXL345_REG_BW_RATE; +#if PERIODIC_FREQUENCY == 60 + imu_umarim_adxl345.buf[1] = 0x09; // normal power and 50Hz sampling, 50Hz BW +#else + imu_umarim_adxl345.buf[1] = 0x0a; // normal power and 100Hz sampling, 50Hz BW +#endif + imu_umarim_adxl345.len_w = 2; + i2c_submit(&IMU_UMARIM_I2C_DEVICE,&imu_umarim_adxl345); + while(imu_umarim_adxl345.status == I2CTransPending); + + /* switch to measurement mode */ + imu_umarim_adxl345.type = I2CTransTx; + imu_umarim_adxl345.buf[0] = ADXL345_REG_POWER_CTL; + imu_umarim_adxl345.buf[1] = 1<<3; + imu_umarim_adxl345.len_w = 2; + i2c_submit(&IMU_UMARIM_I2C_DEVICE,&imu_umarim_adxl345); + while(imu_umarim_adxl345.status == I2CTransPending); + + /* Set range to 16g but keeping full resolution of 3.9 mV/g */ + imu_umarim_adxl345.type = I2CTransTx; + imu_umarim_adxl345.buf[0] = ADXL345_REG_DATA_FORMAT; + imu_umarim_adxl345.buf[1] = 1<<3 | 0<<2 | 0x03; // bit 3 is full resolution bit, bit 2 is left justify bit 0,1 are range: 00=2g 01=4g 10=8g 11=16g + imu_umarim_adxl345.len_w = 2; + i2c_submit(&IMU_UMARIM_I2C_DEVICE,&imu_umarim_adxl345); + while(imu_umarim_adxl345.status == I2CTransPending); + +} + +void imu_periodic( void ) +{ + // Start reading the latest gyroscope data + if (imu_umarim_itg3200.status = I2CTransDone) { + imu_umarim_itg3200.type = I2CTransTxRx; + imu_umarim_itg3200.len_r = 9; + imu_umarim_itg3200.len_w = 1; + imu_umarim_itg3200.buf[0] = ITG3200_REG_INT_STATUS; + i2c_submit(&IMU_UMARIM_I2C_DEVICE, &imu_umarim_itg3200); + } + + // Start reading the latest accelerometer data + if (imu_umarim_adxl345.status = I2CTransDone) { + imu_umarim_adxl345.type = I2CTransTxRx; + imu_umarim_adxl345.len_r = 6; + imu_umarim_adxl345.len_w = 1; + imu_umarim_adxl345.buf[0] = ADXL345_REG_DATA_X0; + i2c_submit(&IMU_UMARIM_I2C_DEVICE, &imu_umarim_adxl345); + } + + //RunOnceEvery(10,imu_umarim_module_downlink_raw()); +} + +void imu_umarim_module_downlink_raw( void ) +{ + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r); + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z); +} + +void imu_umarim_module_event( void ) +{ + int32_t x, y, z; + + // If the itg3200 I2C transaction has succeeded: convert the data + if (imu_umarim_itg3200.status == I2CTransSuccess) { +#define ITG_STA_DAT_OFFSET 3 + x = (int16_t) ((imu_umarim_itg3200.buf[0+ITG_STA_DAT_OFFSET] << 8) | imu_umarim_itg3200.buf[1+ITG_STA_DAT_OFFSET]); + y = (int16_t) ((imu_umarim_itg3200.buf[2+ITG_STA_DAT_OFFSET] << 8) | imu_umarim_itg3200.buf[3+ITG_STA_DAT_OFFSET]); + z = (int16_t) ((imu_umarim_itg3200.buf[4+ITG_STA_DAT_OFFSET] << 8) | imu_umarim_itg3200.buf[5+ITG_STA_DAT_OFFSET]); + + // Is this is new data + if (imu_umarim_itg3200.buf[0] & 0x01) { + gyr_valid = TRUE; + } + + RATES_ASSIGN(imu.gyro_unscaled, + (IMU_GYRO_P_SIGN * x), + (IMU_GYRO_Q_SIGN * y), + (IMU_GYRO_R_SIGN * z)); + + imu_umarim_itg3200.status = I2CTransDone; + } + // Reset status if transaction fails + if (imu_umarim_itg3200.status = I2CTransFail) { + imu_umarim_itg3200.status = I2CTransDone; + } + + + // If the adxl345 I2C transaction has succeeded: convert the data + if (imu_umarim_adxl345.status == I2CTransSuccess) + { + x = (int16_t) ((imu_umarim_adxl345.buf[1] << 8) | imu_umarim_adxl345.buf[0]); + y = (int16_t) ((imu_umarim_adxl345.buf[3] << 8) | imu_umarim_adxl345.buf[2]); + z = (int16_t) ((imu_umarim_adxl345.buf[5] << 8) | imu_umarim_adxl345.buf[4]); + + VECT3_ASSIGN(imu.accel_unscaled, + (IMU_ACCEL_X_SIGN * x), + (IMU_ACCEL_Y_SIGN * y), + (IMU_ACCEL_Z_SIGN * z)); + + acc_valid = TRUE; + imu_umarim_adxl345.status = I2CTransDone; + } + // Reset status if transaction fails + if (imu_umarim_adxl345.status = I2CTransFail) { + imu_umarim_adxl345.status = I2CTransDone; + } + +} + diff --git a/sw/airborne/subsystems/imu/imu_umarim.h b/sw/airborne/subsystems/imu/imu_umarim.h new file mode 100644 index 0000000000..c2d1e5838c --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_umarim.h @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2011 Gautier Hattenberger + * Derived from Aspirin and ppzuavimu drivers + * + * 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_UMARIM_H +#define IMU_UMARIM_H + +#include "std.h" +#include "generated/airframe.h" +#include "subsystems/imu.h" + +// Default configuration +#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 + +extern volatile bool_t gyr_valid; +extern volatile bool_t acc_valid; + +/* must be defined in order to be IMU code: declared in imu.h +extern void imu_impl_init(void); +extern void imu_periodic(void); +*/ + +#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ + imu_umarim_event(); \ + if (gyr_valid) { \ + gyr_valid = FALSE; \ + _gyro_handler(); \ + } \ + if (acc_valid) { \ + acc_valid = FALSE; \ + _accel_handler(); \ + } \ +} + +/* Own Extra Functions */ +extern void imu_umarim_event( void ); + +#endif // PPZUAVIMU_H