mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-05 15:30:08 +08:00
Merge remote-tracking branch 'paparazzi/mpu60x0' into stm32f4
This commit is contained in:
Gautier Hattenberger
@@ -33,7 +33,9 @@ twog_1.0 + aspirin + ETS baro + ETS speed
|
||||
<subsystem name="radio_control" type="ppm"/> <!-- Radio -->
|
||||
<subsystem name="gps" type="ublox"/> <!-- GPS -->
|
||||
<subsystem name="telemetry" type="transparent"/> <!-- Communication -->
|
||||
<subsystem name="imu" type="aspirin_i2c_v1.5"/> <!-- IMU -->
|
||||
<subsystem name="imu" type="aspirin_i2c_v1.5"> <!-- IMU -->
|
||||
<configure name="ASPIRIN_I2C_DEV" value="i2c0"/>
|
||||
</subsystem>
|
||||
<subsystem name="ahrs" type="int_cmpl_quat"/>
|
||||
<subsystem name="ins" type="alt_float"/>
|
||||
<subsystem name="control" type="energyadaptive"/>
|
||||
@@ -134,9 +136,6 @@ twog_1.0 + aspirin + ETS baro + ETS speed
|
||||
<define name="MAG_X_SENS" value="1." integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="1." integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="1." integer="16"/>
|
||||
<define name="MAG_XY_SENS" value="0.0" integer="16"/>
|
||||
<define name="MAG_XZ_SENS" value="0.0" integer="16"/>
|
||||
<define name="MAG_YZ_SENS" value="0.0" integer="16"/>
|
||||
|
||||
<define name="BODY_TO_IMU_PHI" value="-2" unit="deg"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="2" unit="deg"/>
|
||||
|
||||
@@ -49,15 +49,24 @@ IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE0
|
||||
IMU_ASPIRIN_CFLAGS += -DASPIRIN_SPI_SLAVE_IDX=SPI_SLAVE0
|
||||
IMU_ASPIRIN_CFLAGS += -DASPIRIN_SPI_DEV=spi1
|
||||
IMU_ASPIRIN_CFLAGS += -DUSE_SPI1
|
||||
IMU_ASPIRIN_CFLAGS += -DASPIRIN_I2C_DEV=i2c1
|
||||
IMU_ASPIRIN_CFLAGS += -DUSE_I2C1
|
||||
ifndef ASPIRIN_I2C_DEV
|
||||
ASPIRIN_I2C_DEV=i2c0
|
||||
endif
|
||||
else ifeq ($(ARCH), stm32)
|
||||
IMU_ASPIRIN_CFLAGS += -DUSE_I2C2
|
||||
IMU_ASPIRIN_CFLAGS += -DUSE_SPI2
|
||||
# Slave select configuration
|
||||
# SLAVE2 is on PB12 (NSS) (ADXL345 CS)
|
||||
IMU_ASPIRIN_CFLAGS += -DUSE_SPI_SLAVE2
|
||||
ifndef ASPIRIN_I2C_DEV
|
||||
ASPIRIN_I2C_DEV=i2c2
|
||||
endif
|
||||
endif
|
||||
|
||||
# convert i2cx to upper case
|
||||
ASPIRIN_I2C_DEV_UPPER=$(shell echo $(ASPIRIN_I2C_DEV) | tr a-z A-Z)
|
||||
|
||||
IMU_ASPIRIN_CFLAGS += -DASPIRIN_I2C_DEV=$(ASPIRIN_I2C_DEV)
|
||||
IMU_ASPIRIN_CFLAGS += -DUSE_$(ASPIRIN_I2C_DEV_UPPER)
|
||||
|
||||
#
|
||||
# NPS simulator
|
||||
|
||||
@@ -38,12 +38,19 @@ IMU_ASPIRIN_SRCS += peripherals/itg3200.c
|
||||
# Magnetometer
|
||||
IMU_ASPIRIN_SRCS += peripherals/hmc58xx.c
|
||||
|
||||
# set default i2c bus
|
||||
ifndef ASPIRIN_I2C_DEV
|
||||
ifeq ($(ARCH), lpc21)
|
||||
IMU_ASPIRIN_CFLAGS += -DASPIRIN_I2C_DEV=i2c1
|
||||
IMU_ASPIRIN_CFLAGS += -DUSE_I2C1
|
||||
IMU_ASPIRIN_CFLAGS += -DI2C1_VIC_SLOT=12
|
||||
ASPIRIN_I2C_DEV=i2c0
|
||||
else ifeq ($(ARCH), stm32)
|
||||
IMU_ASPIRIN_CFLAGS += -DUSE_I2C2
|
||||
ASPIRIN_I2C_DEV=i2c2
|
||||
endif
|
||||
endif
|
||||
|
||||
# convert i2cx to upper case
|
||||
ASPIRIN_I2C_DEV_UPPER=$(shell echo $(ASPIRIN_I2C_DEV) | tr a-z A-Z)
|
||||
|
||||
IMU_ASPIRIN_CFLAGS += -DASPIRIN_I2C_DEV=$(ASPIRIN_I2C_DEV)
|
||||
IMU_ASPIRIN_CFLAGS += -DUSE_$(ASPIRIN_I2C_DEV_UPPER)
|
||||
|
||||
include $(CFG_SHARED)/imu_nps.makefile
|
||||
|
||||
@@ -1,97 +0,0 @@
|
||||
#ifndef MPU60X0
|
||||
#define MPU60X0
|
||||
|
||||
/* default I2C address */
|
||||
#define MPU60X0_ADDR 0xD0
|
||||
#define MPU60X0_ADDR_ALT 0xD2
|
||||
|
||||
#define MPU60X0_SPI_READ 0x80
|
||||
|
||||
// Power and Interface
|
||||
#define MPU60X0_REG_AUX_VDDIO 0x01 // Must be set to 0 on MPU6000
|
||||
#define MPU60X0_REG_USER_CTRL 0x6A
|
||||
#define MPU60X0_REG_PWR_MGMT_1 0x6B
|
||||
#define MPU60X0_REG_PWR_MGMT_2 0x6C
|
||||
|
||||
// FIFO
|
||||
#define MPU60X0_REG_FIFO_EN 0X23
|
||||
#define MPU60X0_REG_FIFO_COUNT_H 0x72
|
||||
#define MPU60X0_REG_FIFO_COUNT_L 0x73
|
||||
#define MPU60X0_REG_FIFO_R_W 0x74
|
||||
|
||||
// Measurement Settings
|
||||
#define MPU60X0_REG_SMPLRT_DIV 0X19
|
||||
#define MPU60X0_REG_CONFIG 0X1A
|
||||
#define MPU60X0_REG_GYRO_CONFIG 0X1B
|
||||
#define MPU60X0_REG_ACCEL_CONFIG 0X1C
|
||||
|
||||
// I2C Slave settings
|
||||
#define MPU60X0_REG_I2C_MST_CTRL 0X24
|
||||
#define MPU60X0_REG_I2C_MST_STATUS 0X36
|
||||
#define MPU60X0_REG_I2C_MST_DELAY 0X67
|
||||
// Slave 0
|
||||
#define MPU60X0_REG_I2C_SLV0_ADDR 0X25 // i2c addr
|
||||
#define MPU60X0_REG_I2C_SLV0_REG 0X26 // slave reg
|
||||
#define MPU60X0_REG_I2C_SLV0_CTRL 0X27 // set-bits
|
||||
#define MPU60X0_REG_I2C_SLV0_DO 0X63 // DO
|
||||
// Slave 1
|
||||
#define MPU60X0_REG_I2C_SLV1_ADDR 0X28 // i2c addr
|
||||
#define MPU60X0_REG_I2C_SLV1_REG 0X29 // slave reg
|
||||
#define MPU60X0_REG_I2C_SLV1_CTRL 0X2A // set-bits
|
||||
#define MPU60X0_REG_I2C_SLV1_DO 0X64 // DO
|
||||
// Slave 2
|
||||
#define MPU60X0_REG_I2C_SLV2_ADDR 0X2B // i2c addr
|
||||
#define MPU60X0_REG_I2C_SLV2_REG 0X2C // slave reg
|
||||
#define MPU60X0_REG_I2C_SLV2_CTRL 0X2D // set-bits
|
||||
#define MPU60X0_REG_I2C_SLV2_DO 0X65 // DO
|
||||
// Slave 3
|
||||
#define MPU60X0_REG_I2C_SLV3_ADDR 0X2E // i2c addr
|
||||
#define MPU60X0_REG_I2C_SLV3_REG 0X2F // slave reg
|
||||
#define MPU60X0_REG_I2C_SLV3_CTRL 0X30 // set-bits
|
||||
#define MPU60X0_REG_I2C_SLV3_DO 0X66 // DO
|
||||
// Slave 4 - special
|
||||
#define MPU60X0_REG_I2C_SLV4_ADDR 0X31 // i2c addr
|
||||
#define MPU60X0_REG_I2C_SLV4_REG 0X32 // slave reg
|
||||
#define MPU60X0_REG_I2C_SLV4_DO 0X33 // DO
|
||||
#define MPU60X0_REG_I2C_SLV4_CTRL 0X34 // set-bits
|
||||
#define MPU60X0_REG_I2C_SLV4_DI 0X35 // DI
|
||||
|
||||
// Interrupt
|
||||
#define MPU60X0_REG_INT_PIN_CFG 0X37
|
||||
#define MPU60X0_REG_INT_ENABLE 0X38
|
||||
#define MPU60X0_REG_INT_STATUS 0X3A
|
||||
|
||||
// Accelero
|
||||
#define MPU60X0_REG_ACCEL_XOUT_H 0X3B
|
||||
#define MPU60X0_REG_ACCEL_XOUT_L 0X3C
|
||||
#define MPU60X0_REG_ACCEL_YOUT_H 0X3D
|
||||
#define MPU60X0_REG_ACCEL_YOUT_L 0X3E
|
||||
#define MPU60X0_REG_ACCEL_ZOUT_H 0X3F
|
||||
#define MPU60X0_REG_ACCEL_ZOUT_L 0X40
|
||||
|
||||
// Temperature
|
||||
#define MPU60X0_REG_TEMP_OUT_H 0X41
|
||||
#define MPU60X0_REG_TEMP_OUT_L 0X42
|
||||
|
||||
// Gyro
|
||||
#define MPU60X0_REG_GYRO_XOUT_H 0X43
|
||||
#define MPU60X0_REG_GYRO_XOUT_L 0X44
|
||||
#define MPU60X0_REG_GYRO_YOUT_H 0X45
|
||||
#define MPU60X0_REG_GYRO_YOUT_L 0X46
|
||||
#define MPU60X0_REG_GYRO_ZOUT_H 0X47
|
||||
#define MPU60X0_REG_GYRO_ZOUT_L 0X48
|
||||
|
||||
// External Sensor Data
|
||||
#define MPU60X0_EXT_SENS_DATA 0X49
|
||||
#define MPU60X0_EXT_SENS_DATA_SIZE 24
|
||||
|
||||
|
||||
/////////////////////////////////////////////////
|
||||
// MPU60X0 Definitions
|
||||
|
||||
#define MPU60X0_REG_WHO_AM_I 0X75
|
||||
#define MPU60X0_WHOAMI_REPLY 0x68
|
||||
|
||||
|
||||
|
||||
#endif /* MPU60X0 */
|
||||
@@ -0,0 +1,128 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Gautier Hattenberger
|
||||
*
|
||||
* 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 peripherals/mpu60x0.h
|
||||
*
|
||||
* MPU-60X0 driver common interface (I2C and SPI).
|
||||
*/
|
||||
|
||||
#ifndef MPU60X0_H
|
||||
#define MPU60X0_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
/* Include address and register definition */
|
||||
#include "peripherals/mpu60x0_regs.h"
|
||||
|
||||
/// Default sample rate divider
|
||||
#define MPU60X0_DEFAULT_SMPLRT_DIV 0
|
||||
/// Default gyro full scale range +- 2000°/s
|
||||
#define MPU60X0_DEFAULT_FS_SEL MPU60X0_GYRO_RANGE_2000
|
||||
/// Default accel full scale range +- 16g
|
||||
#define MPU60X0_DEFAULT_AFS_SEL MPU60X0_ACCEL_RANGE_16G
|
||||
/// Default internal sampling (1kHz, 42Hz LP Bandwidth)
|
||||
#define MPU60X0_DEFAULT_DLPF_CFG MPU60X0_DLPF_42HZ
|
||||
/// Default interrupt config: RAW_RDY_EN
|
||||
#define MPU60X0_DEFAULT_INT_CFG 1
|
||||
/// Default clock: PLL with X gyro reference
|
||||
#define MPU60X0_DEFAULT_CLK_SEL 1
|
||||
|
||||
enum Mpu60x0ConfStatus {
|
||||
MPU60X0_CONF_UNINIT,
|
||||
MPU60X0_CONF_SD,
|
||||
MPU60X0_CONF_CONFIG,
|
||||
MPU60X0_CONF_GYRO,
|
||||
MPU60X0_CONF_ACCEL,
|
||||
MPU60X0_CONF_INT_PIN,
|
||||
MPU60X0_CONF_INT_ENABLE,
|
||||
MPU60X0_CONF_PWR,
|
||||
MPU60X0_CONF_DONE
|
||||
};
|
||||
|
||||
struct Mpu60x0Config {
|
||||
uint8_t smplrt_div; ///< Sample rate divider
|
||||
enum Mpu60x0DLPF dlpf_cfg; ///< Digital Low Pass Filter
|
||||
enum Adxl345GyroRanges gyro_range; ///< deg/s Range
|
||||
enum Adxl345AccelRanges accel_range; ///< g Range
|
||||
bool_t i2c_bypass; ///< bypass mpu i2c
|
||||
bool_t drdy_int_enable; ///< Enable Data Ready Interrupt
|
||||
uint8_t clk_sel; ///< Clock select
|
||||
enum Mpu60x0ConfStatus init_status; ///< init status
|
||||
bool_t initialized; ///< config done flag
|
||||
};
|
||||
|
||||
static inline void mpu60X0_set_default_config(struct Mpu60x0Config *c)
|
||||
{
|
||||
c->smplrt_div = MPU60X0_DEFAULT_SMPLRT_DIV;
|
||||
c->dlpf_cfg = MPU60X0_DEFAULT_DLPF_CFG;
|
||||
c->gyro_range = MPU60X0_DEFAULT_FS_SEL;
|
||||
c->accel_range = MPU60X0_DEFAULT_AFS_SEL;
|
||||
c->i2c_bypass = TRUE;
|
||||
c->drdy_int_enable = FALSE;
|
||||
c->clk_sel = MPU60X0_DEFAULT_CLK_SEL;
|
||||
}
|
||||
|
||||
/// Configuration function prototype
|
||||
typedef void (*Mpu60x0ConfigSet)(void* mpu, uint8_t _reg, uint8_t _val);
|
||||
|
||||
/// Configuration sequence called once before normal use
|
||||
static inline void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Config* config)
|
||||
{
|
||||
switch (config->init_status) {
|
||||
case MPU60X0_CONF_SD:
|
||||
mpu_set(mpu, MPU60X0_REG_SMPLRT_DIV, config->smplrt_div);
|
||||
config->init_status++;
|
||||
break;
|
||||
case MPU60X0_CONF_CONFIG:
|
||||
mpu_set(mpu, MPU60X0_REG_CONFIG, config->dlpf_cfg);
|
||||
config->init_status++;
|
||||
break;
|
||||
case MPU60X0_CONF_GYRO:
|
||||
mpu_set(mpu, MPU60X0_REG_GYRO_CONFIG, (config->gyro_range<<3));
|
||||
config->init_status++;
|
||||
break;
|
||||
case MPU60X0_CONF_ACCEL:
|
||||
mpu_set(mpu, MPU60X0_REG_ACCEL_CONFIG, (config->accel_range<<3));
|
||||
config->init_status++;
|
||||
break;
|
||||
case MPU60X0_CONF_INT_PIN:
|
||||
mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (config->i2c_bypass<<1));
|
||||
config->init_status++;
|
||||
break;
|
||||
case MPU60X0_CONF_INT_ENABLE:
|
||||
mpu_set(mpu, MPU60X0_REG_INT_ENABLE, (config->drdy_int_enable<<0));
|
||||
config->init_status++;
|
||||
break;
|
||||
case MPU60X0_CONF_PWR:
|
||||
mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, ((config->config.clk_sel)|(0<<6));
|
||||
config->init_status++;
|
||||
break;
|
||||
case MPU60X0_CONF_DONE:
|
||||
config->initialized = TRUE;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#endif // MPU60X0_H
|
||||
@@ -0,0 +1,114 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Gautier Hattenberger
|
||||
*
|
||||
* 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 peripherals/mpu60x0_i2c.c
|
||||
*
|
||||
* Driver for the MPU-60X0 using I2C.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "peripherals/mpu60x0_i2c.h"
|
||||
|
||||
void mpu60x0_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr)
|
||||
{
|
||||
/* set i2c_peripheral */
|
||||
mpu->i2c_p = i2c_p;
|
||||
|
||||
/* slave address */
|
||||
mpu->i2c_trans.slave_addr = addr;
|
||||
/* set inital status: Success or Done */
|
||||
mpu->i2c_trans.status = I2CTransDone;
|
||||
|
||||
/* set default MPU60X0 config options */
|
||||
mpu60x0_set_default_config(&(mpu->config));
|
||||
|
||||
mpu->data_available = FALSE;
|
||||
mpu->config.initialized = FALSE;
|
||||
mpu->config.init_status = MPU60X0_CONF_UNINIT;
|
||||
}
|
||||
|
||||
|
||||
static void mpu60x0_i2c_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) {
|
||||
struct Mpu60x0_I2c* mpu_i2c = (struct Mpu60x0_I2c*)(mpu);
|
||||
mpu_i2c->i2c_trans.buf[0] = _reg;
|
||||
mpu_i2c->i2c_trans.buf[1] = _val;
|
||||
i2c_transmit(mpu_i2c->i2c_p, &(mpu_i2c->i2c_trans), mpu->i2c_trans.slave_addr, 2);
|
||||
}
|
||||
|
||||
// Configuration function called once before normal use
|
||||
void mpu60x0_i2c_start_configure(struct Mpu60x0_I2c *mpu)
|
||||
{
|
||||
if (mpu->config.init_status == MPU60X0_CONF_UNINIT) {
|
||||
mpu->config.init_status++;
|
||||
if (mpu->i2c_trans.status == I2CTransSuccess || mpu->i2c_trans.status == I2CTransDone) {
|
||||
mpu60x0_send_config(mpu60x0_i2c_write_to_reg, (void*)mpu, mpu->config);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void mpu60x0_i2c_read(struct Mpu60x0_I2c *mpu)
|
||||
{
|
||||
if (mpu->config.initialized && mpu->i2c_trans.status == I2CTransDone) {
|
||||
/* set read bit and multiple byte bit, then address */
|
||||
mpu->i2c_trans.buf[0] = MPU60X0_REG_INT_STATUS;
|
||||
i2c_transceive(mpu->i2c_p, &(mpu->i2c_trans), mpu->i2c_trans.slave_addr, 1, 15);
|
||||
}
|
||||
}
|
||||
|
||||
#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
|
||||
|
||||
void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu)
|
||||
{
|
||||
if (mpu->config.initialized) {
|
||||
if (mpu->i2c_trans.status == I2CTransFailed) {
|
||||
mpu->i2c_trans.status = I2CTransDone;
|
||||
}
|
||||
else if (mpu->i2c_trans.status == I2CTransSuccess) {
|
||||
// Successfull reading
|
||||
if (bit_is_set(mpu->i2c_trans.buf[0],0)) {
|
||||
// new data
|
||||
mpu->data_accel.vect.x = Int16FromBuf(mpu->i2c_trans.buf,1);
|
||||
mpu->data_accel.vect.y = Int16FromBuf(mpu->i2c_trans.buf,3);
|
||||
mpu->data_accel.vect.z = Int16FromBuf(mpu->i2c_trans.buf,5);
|
||||
mpu->data_rates.rates.p = Int16FromBuf(mpu->i2c_trans.buf,9);
|
||||
mpu->data_rates.rates.q = Int16FromBuf(mpu->i2c_trans.buf,11);
|
||||
mpu->data_rates.rates.r = Int16FromBuf(mpu->i2c_trans.buf,13);
|
||||
mpu->data_available = TRUE;
|
||||
}
|
||||
mpu->i2c_trans.status = I2CTransDone;
|
||||
}
|
||||
}
|
||||
else if (mpu->config.init_status != MPU60X0_CONF_UNINIT) { // Configuring but not yet initialized
|
||||
switch (mpu->i2c_trans.status) {
|
||||
case I2CTransFailed:
|
||||
mpu->config.init_status--; // Retry config (TODO max retry)
|
||||
case I2CTransSuccess:
|
||||
case I2CTransDone:
|
||||
mpu->i2c_trans.status = I2CTransDone;
|
||||
mpu60x0_send_config(mpu60x0_i2c_write_to_reg, (void*)mpu, mpu->config);
|
||||
if (mpu->config.initialized) mpu->i2c_trans.status = I2CTransDone;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Gautier Hattenberger
|
||||
*
|
||||
* 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 peripherals/mpu60x0_i2c.h
|
||||
*
|
||||
* Driver for the MPU-60X0 using I2C.
|
||||
*/
|
||||
|
||||
#ifndef MPU60X0_I2C_H
|
||||
#define MPU60X0_I2C_H
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_algebra_int.h"
|
||||
#include "mcu_periph/i2c.h"
|
||||
|
||||
/* Include common MPU60X0 options and definitions */
|
||||
#include "peripherals/mpu60x0.h"
|
||||
|
||||
|
||||
struct Mpu60x0_I2c {
|
||||
struct i2c_periph *i2c_p;
|
||||
struct i2c_transaction i2c_trans;
|
||||
volatile bool_t data_available; ///< data ready flag
|
||||
union {
|
||||
struct Int16Vect3 vect; ///< accel data vector in accel coordinate system
|
||||
int16_t value[3]; ///< accel data values accessible by channel index
|
||||
} data_accel;
|
||||
union {
|
||||
struct Int16Rates rates; ///< rates data as angular rates in gyro coordinate system
|
||||
int16_t value[3]; ///< rates data values accessible by channel index
|
||||
} data_rates;
|
||||
struct Mpu60x0Config config;
|
||||
};
|
||||
|
||||
// Functions
|
||||
extern void mpu60x0_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr);
|
||||
extern void mpu60x0_i2c_start_configure(struct Mpu60x0_I2c *mpu);
|
||||
extern void mpu60x0_i2c_read(struct Mpu60x0_I2c *mpu);
|
||||
extern void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu);
|
||||
|
||||
/// convenience function: read or start configuration if not already initialized
|
||||
static inline void mpu60x0_i2c_periodic(struct Mpu60x0_I2c *mpu) {
|
||||
if (mpu->config.initialized)
|
||||
mpu60x0_i2c_read(mpu);
|
||||
else
|
||||
mpu60x0_i2c_start_configure(mpu);
|
||||
}
|
||||
|
||||
#endif // MPU60X0_I2C_H
|
||||
@@ -0,0 +1,156 @@
|
||||
/*
|
||||
* 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 peripherals/mpu60x0_regs.h
|
||||
*
|
||||
* Register and address definitions for MPU-6000 and MPU-6050.
|
||||
*/
|
||||
|
||||
#ifndef MPU60X0_REGS_H
|
||||
#define MPU60X0_REGS_H
|
||||
|
||||
/* default I2C address */
|
||||
#define MPU60X0_ADDR 0xD0
|
||||
#define MPU60X0_ADDR_ALT 0xD2
|
||||
|
||||
#define MPU60X0_SPI_READ 0x80
|
||||
|
||||
// Power and Interface
|
||||
#define MPU60X0_REG_AUX_VDDIO 0x01 // Must be set to 0 on MPU6000
|
||||
#define MPU60X0_REG_USER_CTRL 0x6A
|
||||
#define MPU60X0_REG_PWR_MGMT_1 0x6B
|
||||
#define MPU60X0_REG_PWR_MGMT_2 0x6C
|
||||
|
||||
// FIFO
|
||||
#define MPU60X0_REG_FIFO_EN 0X23
|
||||
#define MPU60X0_REG_FIFO_COUNT_H 0x72
|
||||
#define MPU60X0_REG_FIFO_COUNT_L 0x73
|
||||
#define MPU60X0_REG_FIFO_R_W 0x74
|
||||
|
||||
// Measurement Settings
|
||||
#define MPU60X0_REG_SMPLRT_DIV 0X19
|
||||
#define MPU60X0_REG_CONFIG 0X1A
|
||||
#define MPU60X0_REG_GYRO_CONFIG 0X1B
|
||||
#define MPU60X0_REG_ACCEL_CONFIG 0X1C
|
||||
|
||||
// I2C Slave settings
|
||||
#define MPU60X0_REG_I2C_MST_CTRL 0X24
|
||||
#define MPU60X0_REG_I2C_MST_STATUS 0X36
|
||||
#define MPU60X0_REG_I2C_MST_DELAY 0X67
|
||||
// Slave 0
|
||||
#define MPU60X0_REG_I2C_SLV0_ADDR 0X25 // i2c addr
|
||||
#define MPU60X0_REG_I2C_SLV0_REG 0X26 // slave reg
|
||||
#define MPU60X0_REG_I2C_SLV0_CTRL 0X27 // set-bits
|
||||
#define MPU60X0_REG_I2C_SLV0_DO 0X63 // DO
|
||||
// Slave 1
|
||||
#define MPU60X0_REG_I2C_SLV1_ADDR 0X28 // i2c addr
|
||||
#define MPU60X0_REG_I2C_SLV1_REG 0X29 // slave reg
|
||||
#define MPU60X0_REG_I2C_SLV1_CTRL 0X2A // set-bits
|
||||
#define MPU60X0_REG_I2C_SLV1_DO 0X64 // DO
|
||||
// Slave 2
|
||||
#define MPU60X0_REG_I2C_SLV2_ADDR 0X2B // i2c addr
|
||||
#define MPU60X0_REG_I2C_SLV2_REG 0X2C // slave reg
|
||||
#define MPU60X0_REG_I2C_SLV2_CTRL 0X2D // set-bits
|
||||
#define MPU60X0_REG_I2C_SLV2_DO 0X65 // DO
|
||||
// Slave 3
|
||||
#define MPU60X0_REG_I2C_SLV3_ADDR 0X2E // i2c addr
|
||||
#define MPU60X0_REG_I2C_SLV3_REG 0X2F // slave reg
|
||||
#define MPU60X0_REG_I2C_SLV3_CTRL 0X30 // set-bits
|
||||
#define MPU60X0_REG_I2C_SLV3_DO 0X66 // DO
|
||||
// Slave 4 - special
|
||||
#define MPU60X0_REG_I2C_SLV4_ADDR 0X31 // i2c addr
|
||||
#define MPU60X0_REG_I2C_SLV4_REG 0X32 // slave reg
|
||||
#define MPU60X0_REG_I2C_SLV4_DO 0X33 // DO
|
||||
#define MPU60X0_REG_I2C_SLV4_CTRL 0X34 // set-bits
|
||||
#define MPU60X0_REG_I2C_SLV4_DI 0X35 // DI
|
||||
|
||||
// Interrupt
|
||||
#define MPU60X0_REG_INT_PIN_CFG 0X37
|
||||
#define MPU60X0_REG_INT_ENABLE 0X38
|
||||
#define MPU60X0_REG_INT_STATUS 0X3A
|
||||
|
||||
// Accelero
|
||||
#define MPU60X0_REG_ACCEL_XOUT_H 0X3B
|
||||
#define MPU60X0_REG_ACCEL_XOUT_L 0X3C
|
||||
#define MPU60X0_REG_ACCEL_YOUT_H 0X3D
|
||||
#define MPU60X0_REG_ACCEL_YOUT_L 0X3E
|
||||
#define MPU60X0_REG_ACCEL_ZOUT_H 0X3F
|
||||
#define MPU60X0_REG_ACCEL_ZOUT_L 0X40
|
||||
|
||||
// Temperature
|
||||
#define MPU60X0_REG_TEMP_OUT_H 0X41
|
||||
#define MPU60X0_REG_TEMP_OUT_L 0X42
|
||||
|
||||
// Gyro
|
||||
#define MPU60X0_REG_GYRO_XOUT_H 0X43
|
||||
#define MPU60X0_REG_GYRO_XOUT_L 0X44
|
||||
#define MPU60X0_REG_GYRO_YOUT_H 0X45
|
||||
#define MPU60X0_REG_GYRO_YOUT_L 0X46
|
||||
#define MPU60X0_REG_GYRO_ZOUT_H 0X47
|
||||
#define MPU60X0_REG_GYRO_ZOUT_L 0X48
|
||||
|
||||
// External Sensor Data
|
||||
#define MPU60X0_EXT_SENS_DATA 0X49
|
||||
#define MPU60X0_EXT_SENS_DATA_SIZE 24
|
||||
|
||||
|
||||
/////////////////////////////////////////////////
|
||||
// MPU60X0 Definitions
|
||||
|
||||
#define MPU60X0_REG_WHO_AM_I 0X75
|
||||
#define MPU60X0_WHOAMI_REPLY 0x68
|
||||
|
||||
/** Digital Low Pass Filter Options
|
||||
* DLFP is affecting both gyro and accels,
|
||||
* with slightly different bandwidth
|
||||
*/
|
||||
enum Mpu60x0DLPF {
|
||||
MPU60X0_DLPF_256HZ = 0x0, // internal sampling rate 8kHz
|
||||
MPU60X0_DLPF_188HZ = 0x1, // internal sampling rate 1kHz
|
||||
MPU60X0_DLPF_98HZ = 0x2,
|
||||
MPU60X0_DLPF_42HZ = 0x3,
|
||||
MPU60X0_DLPF_20HZ = 0x4,
|
||||
MPU60X0_DLPF_10HZ = 0x5,
|
||||
MPU60X0_DLPF_05HZ = 0x6
|
||||
};
|
||||
|
||||
/**
|
||||
* Selectable gyro range
|
||||
*/
|
||||
enum Mpu60x0GyroRanges {
|
||||
MPU60X0_GYRO_RANGE_250 = 0x00,
|
||||
MPU60X0_GYRO_RANGE_500 = 0x01,
|
||||
MPU60X0_GYRO_RANGE_1000 = 0x02,
|
||||
MPU60X0_GYRO_RANGE_2000 = 0x03
|
||||
};
|
||||
|
||||
/**
|
||||
* Selectable accel range
|
||||
*/
|
||||
enum Mpu60x0AccelRanges {
|
||||
MPU60X0_ACCEL_RANGE_2G = 0x00,
|
||||
MPU60X0_ACCEL_RANGE_4G = 0x01,
|
||||
MPU60X0_ACCEL_RANGE_8G = 0x02,
|
||||
MPU60X0_ACCEL_RANGE_16G = 0x03
|
||||
};
|
||||
|
||||
#endif /* MPU60X0_REGS_H */
|
||||
@@ -0,0 +1,134 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Gautier Hattenberger
|
||||
*
|
||||
* 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 peripherals/mpu60x0_spi.c
|
||||
*
|
||||
* Driver for the MPU-60X0 using SPI.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "peripherals/mpu60x0_spi.h"
|
||||
|
||||
static void trans_cb( struct spi_transaction *trans );
|
||||
|
||||
void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t slave_idx)
|
||||
{
|
||||
/* set spi_peripheral */
|
||||
mpu->spi_p = spi_p;
|
||||
|
||||
/* configure spi transaction */
|
||||
mpu->spi_trans.cpol = SPICpolIdleHigh;
|
||||
mpu->spi_trans.cpha = SPICphaEdge2;
|
||||
mpu->spi_trans.dss = SPIDss8bit;
|
||||
mpu->spi_trans.bitorder = SPIMSBFirst;
|
||||
mpu->spi_trans.cdiv = SPIDiv64;
|
||||
|
||||
mpu->spi_trans.select = SPISelectUnselect;
|
||||
mpu->spi_trans.slave_idx = slave_idx;
|
||||
mpu->spi_trans.output_length = MPU60X0_BUFFER_LEN; //FIXME
|
||||
mpu->spi_trans.input_length = MPU60X0_BUFFER_LEN;
|
||||
mpu->spi_trans.before_cb = NULL;
|
||||
mpu->spi_trans.after_cb = trans_cb;
|
||||
mpu->spi_trans.input_buf = &(mpu->rx_buf[0]);
|
||||
mpu->spi_trans.output_buf = &(mpu->tx_buf[0]);
|
||||
|
||||
/* set inital status: Success or Done */
|
||||
mpu->spi_trans.status = SPITransDone;
|
||||
|
||||
/* set default MPU60X0 config options */
|
||||
mpu60x0_set_default_config(&(mpu->config));
|
||||
|
||||
mpu->data_available = FALSE;
|
||||
mpu->config.initialized = FALSE;
|
||||
mpu->config.init_status = MPU60X0_CONF_UNINIT;
|
||||
}
|
||||
|
||||
|
||||
static void mpu60x0_spi_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) {
|
||||
struct Mpu60x0_Spi* mpu_spi = (struct Mpu60x0_Spi*)(mpu);
|
||||
mpu_spi->spi_trans.output_length = 2;
|
||||
mpu_spi->spi_trans.input_length = 0;
|
||||
mpu_spi->tx_buf[0] = _reg;
|
||||
mpu_spi->tx_buf[1] = _val;
|
||||
spi_submit(mpu_spi->spi_p, &(mpu_spi->spi_trans));
|
||||
}
|
||||
|
||||
// Configuration function called once before normal use
|
||||
void mpu60x0_spi_start_configure(struct Mpu60x0_Spi *mpu)
|
||||
{
|
||||
if (mpu->config.init_status == MPU60X0_CONF_UNINIT) {
|
||||
mpu->config.init_status++;
|
||||
if (mpu->spi_trans.status == SPITransSuccess || mpu->spi_trans.status == SPITransDone) {
|
||||
mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void*)mpu, mpu->config);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void mpu60x0_spi_read(struct Mpu60x0_Spi *mpu)
|
||||
{
|
||||
if (mpu->config.initialized && mpu->spi_trans.status == SPITransDone) {
|
||||
mpu->spi_trans.output_length = 1;
|
||||
mpu->spi_trans.input_length = 15; // FIXME external data
|
||||
/* set read bit and multiple byte bit, then address */
|
||||
mpu->tx_buf[0] = MPU60X0_REG_INT_STATUS + MPU60X0_SPI_READ;
|
||||
spi_submit(mpu->spi_p, &(mpu->spi_trans));
|
||||
}
|
||||
}
|
||||
|
||||
#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
|
||||
|
||||
void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu)
|
||||
{
|
||||
if (mpu->config.initialized) {
|
||||
if (mpu->spi_trans.status == SPITransFailed) {
|
||||
mpu->spi_trans.status = SPITransDone;
|
||||
}
|
||||
else if (mpu->spi_trans.status == SPITransSuccess) {
|
||||
// Successfull reading
|
||||
if (bit_is_set(mpu->rx_buf[0],0)) {
|
||||
// new data
|
||||
mpu->data_accel.vect.x = Int16FromBuf(mpu->rx_buf,1);
|
||||
mpu->data_accel.vect.y = Int16FromBuf(mpu->rx_buf,3);
|
||||
mpu->data_accel.vect.z = Int16FromBuf(mpu->rx_buf,5);
|
||||
mpu->data_rates.rates.p = Int16FromBuf(mpu->rx_buf,9);
|
||||
mpu->data_rates.rates.q = Int16FromBuf(mpu->rx_buf,11);
|
||||
mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf,13);
|
||||
mpu->data_available = TRUE;
|
||||
}
|
||||
mpu->spi_trans.status = SPITransDone;
|
||||
}
|
||||
}
|
||||
else if (mpu->config.init_status != MPU60X0_CONF_UNINIT) { // Configuring but not yet initialized
|
||||
switch (mpu->spi_trans.status) {
|
||||
case SPITransFailed:
|
||||
mpu->config.init_status--; // Retry config (TODO max retry)
|
||||
case SPITransSuccess:
|
||||
case SPITransDone:
|
||||
mpu->spi_trans.status = SPITransDone;
|
||||
mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void*)mpu, mpu->config);
|
||||
if (mpu->config.initialized) mpu->spi_trans.status = SPITransDone;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,72 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Gautier Hattenberger
|
||||
*
|
||||
* 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 peripherals/mpu60x0_spi.h
|
||||
*
|
||||
* Driver for the MPU-60X0 using SPI.
|
||||
*/
|
||||
|
||||
#ifndef MPU60X0_SPI_H
|
||||
#define MPU60X0_SPI_H
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_algebra_int.h"
|
||||
#include "mcu_periph/spi.h"
|
||||
|
||||
/* Include common MPU60X0 options and definitions */
|
||||
#include "peripherals/mpu60x0.h"
|
||||
|
||||
|
||||
#define MPU60X0_BUFFER_LEN 32
|
||||
|
||||
struct Mpu60x0_Spi {
|
||||
struct spi_periph *spi_p;
|
||||
struct spi_transaction spi_trans;
|
||||
volatile uint8_t tx_buf[MPU60X0_BUFFER_LEN]; // FIXME correct length
|
||||
volatile uint8_t rx_buf[MPU60X0_BUFFER_LEN]; // FIXME idem
|
||||
volatile bool_t data_available; ///< data ready flag
|
||||
union {
|
||||
struct Int16Vect3 vect; ///< accel data vector in accel coordinate system
|
||||
int16_t value[3]; ///< accel data values accessible by channel index
|
||||
} data_accel;
|
||||
union {
|
||||
struct Int16Rates rates; ///< rates data as angular rates in gyro coordinate system
|
||||
int16_t value[3]; ///< rates data values accessible by channel index
|
||||
} data_rates;
|
||||
struct Mpu60x0Config config;
|
||||
};
|
||||
|
||||
// Functions
|
||||
extern void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t addr);
|
||||
extern void mpu60x0_spi_start_configure(struct Mpu60x0_Spi *mpu);
|
||||
extern void mpu60x0_spi_read(struct Mpu60x0_Spi *mpu);
|
||||
extern void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu);
|
||||
|
||||
/// convenience function: read or start configuration if not already initialized
|
||||
static inline void mpu60x0_spi_periodic(struct Mpu60x0_Spi *mpu) {
|
||||
if (mpu->config.initialized)
|
||||
mpu60x0_spi_read(mpu);
|
||||
else
|
||||
mpu60x0_spi_start_configure(mpu);
|
||||
}
|
||||
|
||||
#endif // MPU60X0_SPI_H
|
||||
+1
-1
Submodule sw/ext/luftboot updated: 3f40726c72...75c3107284
Reference in New Issue
Block a user