diff --git a/conf/firmwares/subsystems/shared/imu_gl1.makefile b/conf/firmwares/subsystems/shared/imu_gl1.makefile new file mode 100644 index 0000000000..041f92f684 --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_gl1.makefile @@ -0,0 +1,51 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# IMU from Goodluckbuy +# + + +IMU_GL1_CFLAGS = -DIMU_TYPE_H=\"imu/imu_gl1.h\" +IMU_GL1_SRCS = $(SRC_SUBSYSTEMS)/imu.c +IMU_GL1_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_gl1.c + +# for fixedwing firmware and ap only +ifeq ($(TARGET), ap) + IMU_GL1_CFLAGS += -DUSE_IMU +endif + +# Accelerometer +IMU_GL1_SRCS += peripherals/adxl345_i2c.c + +# Gyro +IMU_GL1_SRCS += peripherals/l3g4200.c + +# Magnetometer +IMU_GL1_SRCS += peripherals/hmc58xx.c + +ifeq ($(ARCH), lpc21) +ifndef GL1_I2C_DEV +GL1_I2C_DEV=i2c0 +endif +else ifeq ($(ARCH), stm32) +ifndef GL1_I2C_DEV +GL1_I2C_DEV=i2c2 +endif +endif + +# convert i2cx to upper case +GL1_I2C_DEV_UPPER=$(shell echo $(GL1_I2C_DEV) | tr a-z A-Z) + +IMU_GL1_CFLAGS += -DGL1_I2C_DEV=$(GL1_I2C_DEV) +IMU_GL1_CFLAGS += -DUSE_$(GL1_I2C_DEV_UPPER) + +# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets +# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example +ap.CFLAGS += $(IMU_GL1_CFLAGS) +ap.srcs += $(IMU_GL1_SRCS) + + + +# +# NPS simulator +# +include $(CFG_SHARED)/imu_nps.makefile diff --git a/sw/airborne/subsystems/imu/imu_gl1.c b/sw/airborne/subsystems/imu/imu_gl1.c new file mode 100644 index 0000000000..eb035d6704 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_gl1.c @@ -0,0 +1,154 @@ +/* + * Copyright (C) 2010 Antoine Drouin + * 2013 Felix Ruess + * 2013 Eduardo Lavratti + * + * 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. + */ + +/** + * @file subsystems/imu/imu_gl1.c + * Driver for I2C IMU using L3G4200, ADXL345, HMC5883 and BMP085. + */ + +#include "subsystems/imu.h" +#include "mcu_periph/i2c.h" + +PRINT_CONFIG_VAR(GL1_I2C_DEV) + +/** adxl345 accelerometer output rate, lowpass is set to half of rate */ +#ifndef GL1_ACCEL_RATE +# if PERIODIC_FREQUENCY <= 60 +# define GL1_ACCEL_RATE ADXL345_RATE_50HZ +# elif PERIODIC_FREQUENCY <= 120 +# define GL1_ACCEL_RATE ADXL345_RATE_100HZ +# else +# define GL1_ACCEL_RATE ADXL345_RATE_200HZ +# endif +#endif +PRINT_CONFIG_VAR(GL1_ACCEL_RATE) + +/** gyro internal lowpass frequency */ +#ifndef GL1_GYRO_LOWPASS +# if PERIODIC_FREQUENCY <= 60 +# define GL1_GYRO_LOWPASS L3G4200_DLPF_1 +# elif PERIODIC_FREQUENCY <= 120 +# define GL1_GYRO_LOWPASS L3G4200_DLPF_2 +# else +# define GL1_GYRO_LOWPASS L3G4200_DLPF_3 +# endif +#endif +PRINT_CONFIG_VAR(GL1_GYRO_LOWPASS) + +/** gyro sample rate divider */ +#ifndef GL1_GYRO_SMPLRT +# if PERIODIC_FREQUENCY <= 60 +# define GL1_GYRO_SMPLRT L3G4200_DR_100Hz +PRINT_CONFIG_MSG("Gyro output rate is 100Hz") +# else +# define GL1_GYRO_SMPLRT L3G4200_DR_100Hz +PRINT_CONFIG_MSG("Gyro output rate is 100Hz") +# endif +#endif +PRINT_CONFIG_VAR(GL1_GYRO_SMPLRT) + +#ifdef GL1_GYRO_SCALE +# define L3G4200_SCALE GL1_GYRO_SCALE +# else +# define L3G4200_SCALE L3G4200_SCALE_2000 +#endif +PRINT_CONFIG_VAR(L3G4200_SCALE) + +struct ImuGL1I2c imu_gl1; + +void imu_impl_init(void) +{ + imu_gl1.accel_valid = FALSE; + imu_gl1.gyro_valid = FALSE; + imu_gl1.mag_valid = FALSE; + + /* Set accel configuration */ + adxl345_i2c_init(&imu_gl1.acc_adxl, &(GL1_I2C_DEV), ADXL345_ADDR); + // set the data rate + imu_gl1.acc_adxl.config.rate = GL1_ACCEL_RATE; + /// @todo drdy int handling for adxl345 + //imu_aspirin.acc_adxl.config.drdy_int_enable = TRUE; + + + /* Gyro configuration and initalization */ + l3g4200_init(&imu_gl1.gyro_l3g, &(GL1_I2C_DEV), L3G4200_ADDR_ALT); + /* change the default config */ + // output data rate, bandwidth, enable axis (0x1f = 100 ODR, 25hz) (0x5f = 200hz ODR, 25hz) + imu_gl1.gyro_l3g.config.ctrl_reg1 = ((GL1_GYRO_SMPLRT<<6) | (GL1_GYRO_LOWPASS<<4) | 0xf); + // senstivity + imu_gl1.gyro_l3g.config.ctrl_reg4 = (L3G4200_SCALE<<4) | 0x00; + // filter config + imu_gl1.gyro_l3g.config.ctrl_reg5 = 0x00; // only first LPF active + + + /* initialize mag and set default options */ + hmc58xx_init(&imu_gl1.mag_hmc, &(GL1_I2C_DEV), HMC58XX_ADDR); + imu_gl1.mag_hmc.type = HMC_TYPE_5883; +} + + +void imu_periodic(void) +{ + adxl345_i2c_periodic(&imu_gl1.acc_adxl); + + // Start reading the latest gyroscope data + l3g4200_periodic(&imu_gl1.gyro_l3g); + + // Read HMC58XX at 50Hz (main loop for rotorcraft: 512Hz) + RunOnceEvery(10, hmc58xx_periodic(&imu_gl1.mag_hmc)); +} + +void imu_gl1_i2c_event(void) +{ + adxl345_i2c_event(&imu_gl1.acc_adxl); + if (imu_gl1.acc_adxl.data_available) { + VECT3_COPY(imu.accel_unscaled, imu_gl1.acc_adxl.data.vect); + imu.accel_unscaled.x = imu_gl1.acc_adxl.data.vect.y; + imu.accel_unscaled.y = imu_gl1.acc_adxl.data.vect.x; + imu.accel_unscaled.z = -imu_gl1.acc_adxl.data.vect.z; + imu_gl1.acc_adxl.data_available = FALSE; + imu_gl1.accel_valid = TRUE; + } + + /* If the lg34200 I2C transaction has succeeded: convert the data */ + l3g4200_event(&imu_gl1.gyro_l3g); + if (imu_gl1.gyro_l3g.data_available) { + RATES_COPY(imu.gyro_unscaled, imu_gl1.gyro_l3g.data.rates); + imu.gyro_unscaled.p = imu_gl1.gyro_l3g.data.rates.q; + imu.gyro_unscaled.q = imu_gl1.gyro_l3g.data.rates.p; + imu.gyro_unscaled.r = -imu_gl1.gyro_l3g.data.rates.r; + imu_gl1.gyro_l3g.data_available = FALSE; + imu_gl1.gyro_valid = TRUE; + } + + /* HMC58XX event task */ + hmc58xx_event(&imu_gl1.mag_hmc); + if (imu_gl1.mag_hmc.data_available) { + // VECT3_COPY(imu.mag_unscaled, imu_gl1.mag_hmc.data.vect); + imu.mag_unscaled.y = imu_gl1.mag_hmc.data.vect.x; + imu.mag_unscaled.x = imu_gl1.mag_hmc.data.vect.y; + imu.mag_unscaled.z = -imu_gl1.mag_hmc.data.vect.z; + imu_gl1.mag_hmc.data_available = FALSE; + imu_gl1.mag_valid = TRUE; + } +} diff --git a/sw/airborne/subsystems/imu/imu_gl1.h b/sw/airborne/subsystems/imu/imu_gl1.h new file mode 100644 index 0000000000..a222ca6459 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_gl1.h @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2010 Antoine Drouin + * 2013 Felix Ruess + * 2013 Eduardo Lavratti + * + * 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. + */ + +/** + * @file subsystems/imu/imu_gl1.h + * Interface for I2c IMU using using L3G4200, ADXL345, HMC5883 and BMP085. + */ + + +#ifndef IMU_GL1_H +#define IMU_GL1_H + +#include "generated/airframe.h" +#include "subsystems/imu.h" + +/* include default GL1 sensitivity/channel definitions */ +#include "subsystems/imu/imu_gl1_defaults.h" + +#include "peripherals/l3g4200.h" +#include "peripherals/hmc58xx.h" +#include "peripherals/adxl345_i2c.h" + +struct ImuGL1I2c { + volatile uint8_t accel_valid; + volatile uint8_t gyro_valid; + volatile uint8_t mag_valid; + struct Adxl345_I2c acc_adxl; + struct L3g4200 gyro_l3g; + struct Hmc58xx mag_hmc; +}; + +extern struct ImuGL1I2c imu_gl1; + +extern void imu_gl1_i2c_event(void); + +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { + imu_gl1_i2c_event(); + if (imu_gl1.gyro_valid) { + imu_gl1.gyro_valid = FALSE; + _gyro_handler(); + } + if (imu_gl1.accel_valid) { + imu_gl1.accel_valid = FALSE; + _accel_handler(); + } + if (imu_gl1.mag_valid) { + imu_gl1.mag_valid = FALSE; + _mag_handler(); + } +} + +#endif /* IMU_GL1_H */ diff --git a/sw/airborne/subsystems/imu/imu_gl1_defaults.h b/sw/airborne/subsystems/imu/imu_gl1_defaults.h new file mode 100644 index 0000000000..874afd3cd3 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_gl1_defaults.h @@ -0,0 +1,94 @@ +/* + * Copyright (C) 2010-2013 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. + */ + +/** + * @file subsystems/imu/imu_gl1_defaults.h + * Default sensitivity definitions for IMU GL1. + */ + + +#ifndef IMU_GL1_DEFAULTS_H +#define IMU_GL1_DEFAULTS_H + +#include "generated/airframe.h" + +#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 +#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 + + +/** default gyro sensitivy and neutral from the datasheet + * L3G4200 has 8.75 LSB/(deg/s) + * sens = 1/xxx * pi/180 * 2^INT32_RATE_FRAC + * sens = 1/xxx * pi/180 * 4096 = ????? + * + * 250deg = 114.28 = 0.625 + * 500deg = 57.14 = 1.25 + * 2000deg = 14.28 = 5.006 + */ +#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS +#define IMU_GYRO_P_SENS 5.006 +#define IMU_GYRO_P_SENS_NUM 2503 +#define IMU_GYRO_P_SENS_DEN 500 +#define IMU_GYRO_Q_SENS 5.006 +#define IMU_GYRO_Q_SENS_NUM 2503 +#define IMU_GYRO_Q_SENS_DEN 500 +#define IMU_GYRO_R_SENS 5.006 +#define IMU_GYRO_R_SENS_NUM 2503 +#define IMU_GYRO_R_SENS_DEN 500 +#endif + +/** default accel sensitivy from the ADXL345 datasheet + * sensitivity of x & y axes depends on supply voltage: + * - 256 LSB/g @ 2.5V + * - 265 LSB/g @ 3.3V + * z sensitivity stays at 256 LSB/g + * fixed point sens: 9.81 [m/s^2] / 256 [LSB/g] * 2^INT32_ACCEL_FRAC + * x/y sens = 9.81 / 265 * 1024 = 37.91 + * z sens = 9.81 / 256 * 1024 = 39.24 + * + * what about the offset at 3.3V? + */ +#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS +#define IMU_ACCEL_X_SENS 37.91 +#define IMU_ACCEL_X_SENS_NUM 3791 +#define IMU_ACCEL_X_SENS_DEN 100 +#define IMU_ACCEL_Y_SENS 37.91 +#define IMU_ACCEL_Y_SENS_NUM 3791 +#define IMU_ACCEL_Y_SENS_DEN 100 +#define IMU_ACCEL_Z_SENS 39.24 +#define IMU_ACCEL_Z_SENS_NUM 3924 +#define IMU_ACCEL_Z_SENS_DEN 100 +#endif + +#endif /* IMU_GL1_DEFAULTS_H */