mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-24 22:05:58 +08:00
[imu] new GL1 driver from Eduardo
IMU from Goodluckbuy with L3G4200, ADXL345 and HMC5883.
This commit is contained in:
Felix Ruess
@@ -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
|
||||
@@ -0,0 +1,154 @@
|
||||
/*
|
||||
* Copyright (C) 2010 Antoine Drouin <poinix@gmail.com>
|
||||
* 2013 Felix Ruess <felix.ruess@gmail.com>
|
||||
* 2013 Eduardo Lavratti <agressiva@hotmail.com>
|
||||
*
|
||||
* 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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,72 @@
|
||||
/*
|
||||
* Copyright (C) 2010 Antoine Drouin <poinix@gmail.com>
|
||||
* 2013 Felix Ruess <felix.ruess@gmail.com>
|
||||
* 2013 Eduardo Lavratti <agressiva@hotmail.com>
|
||||
*
|
||||
* 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 */
|
||||
@@ -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 */
|
||||
Reference in New Issue
Block a user