Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 19:47:50 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

umarim imu using new drivers for adxl345 and itg3200

Browse Source
This commit is contained in:
Gautier Hattenberger
2011-07-19 19:24:13 +02:00
parent a558036ceb
commit c52c431d89
2 changed files with 39 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/subsystems/shared/imu_umarim.makefile
+13 -3
View File
@@ -5,10 +5,20 @@ IMU_UMARIM_CFLAGS += -DIMU_TYPE_H=\"boards/umarim/imu_umarim.h\"
IMU_UMARIM_SRCS = $(SRC_SUBSYSTEMS)/imu.c \
$(SRC_BOARD)/imu_umarim.c
IMU_UMARIM_I2C_DEVICE=i2c1
IMU_UMARIM_CFLAGS += -DUSE_I2C
IMU_UMARIM_CFLAGS += -DUSE_I2C1
IMU_UMARIM_CFLAGS += -DIMU_UMARIM_I2C_DEVICE=i2c1
IMU_UMARIM_CFLAGS += -DUSE_I2C1 -DI2C1_SCLL=25 -DI2C1_SCLH=25
IMU_UMARIM_CFLAGS += -DITG3200_I2C_DEVICE=$(IMU_UMARIM_I2C_DEVICE)
IMU_UMARIM_CFLAGS += -DITG3200_I2C_ADDR=ITG3200_ADDR_ALT
IMU_UMARIM_CFLAGS += -DITG3200_SMPLRT_DIV=19
IMU_UMARIM_CFLAGS += -DITG3200_DLPF_CFG=4
IMU_UMARIM_SRCS += peripherals/itg3200.c
IMU_UMARIM_CFLAGS += -DADXL345_I2C_DEVICE=$(IMU_UMARIM_I2C_DEVICE)
IMU_UMARIM_CFLAGS += -DADXL345_I2C_ADDR=ADXL345_ADDR_ALT
IMU_UMARIM_CFLAGS += -DADXL345_BW_RATE=0x09
IMU_UMARIM_SRCS += peripherals/adxl345.i2c.c
ap.CFLAGS += $(IMU_UMARIM_CFLAGS)
ap.srcs += $(IMU_UMARIM_SRCS)
sw/airborne/boards/umarim/imu_umarim.c
+26 -133
View File
@@ -35,121 +35,44 @@
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#endif
// Peripherials
#include "peripherals/itg3200.h"
#include "peripherals/adxl345.h"
// Configure ITG3200
// ITG3200_I2C_DEVICE IMU_UMARIM_I2C_DEVICE
// ITG3200_I2C_ADDR ITG3200_ADDR_ALT
// ITG3200_SMPLRT_DIV 19
// ITG3200_DLPF_CFG 4
#include "peripherals/itg3200.extra.h"
// Configure ADXL345
// ADXL345_I2C_DEVICE IMU_UMARIM_I2C_DEVICE
// ADXL345_I2C_ADDR ADXL345_ADDR_ALT
// ADXL345_BW_RATE 0x09 (50Hz rate, 25Hz BW)
#include "peripherals/adxl345.extra_i2c.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);
#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);
itg3200_init();
/////////////////////////////////////////////////////////////////////
// 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);
adxl345_init();
}
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);
}
itg3200_periodic();
// 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);
}
adxl345_periodic();
RunOnceEvery(10,imu_umarim_downlink_raw());
}
@@ -164,51 +87,21 @@ void imu_umarim_downlink_raw( void )
void imu_umarim_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;
itg3200_event();
if (itg3200_data_available) {
RATES_COPY(imu.gyro_unscaled, itg3200_data);
itg3200_data_available = FALSE;
gyr_valid = TRUE;
}
// Reset status if transaction fails
if (imu_umarim_itg3200.status == I2CTransFailed) {
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 * */y),
(/*IMU_ACCEL_Y_SIGN * */-x),
(/*IMU_ACCEL_Z_SIGN * */z));
adxl345_event();
if (adxl345_data_available) {
VECT3_ASSIGN(imu.accel_unscaled, adxl345_data.y, -adxl345_data.x, adxl345_data.z);
adxl345_data_available = FALSE;
acc_valid = TRUE;
imu_umarim_adxl345.status = I2CTransDone;
}
// Reset status if transaction fails
if (imu_umarim_adxl345.status == I2CTransFailed) {
imu_umarim_adxl345.status = I2CTransDone;
}
}