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

Merge pull request #953 from paparazzi/mpu9250

Browse Source 
Driver for the MPU9250
This commit is contained in:
Gautier Hattenberger
2014-11-18 09:57:23 +01:00
parent 44e02c4152 18cbec7f15
commit 4258f43206
16 changed files with 1744 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/shared/imu_mpu9250_i2c.makefile
+54
View File
@@ -0,0 +1,54 @@
# Hey Emacs, this is a -*- makefile -*-
#
# MPU9250 IMU via I2C
#
# for fixedwing firmware and ap only
ifeq ($(TARGET), ap)
IMU_MPU9250_CFLAGS = -DUSE_IMU
endif
IMU_MPU9250_CFLAGS += -DIMU_TYPE_H=\"imu/imu_mpu9250_i2c.h\"
IMU_MPU9250_SRCS = $(SRC_SUBSYSTEMS)/imu.c
IMU_MPU9250_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_mpu9250_i2c.c
IMU_MPU9250_SRCS += peripherals/mpu9250.c
IMU_MPU9250_SRCS += peripherals/mpu9250_i2c.c
# Magnetometer
IMU_MPU9250_SRCS += peripherals/ak8963.c
# set default i2c bus
ifeq ($(ARCH), lpc21)
MPU9250_I2C_DEV ?= i2c0
else ifeq ($(ARCH), stm32)
MPU9250_I2C_DEV ?= i2c2
endif
ifeq ($(TARGET), ap)
ifndef MPU9250_I2C_DEV
$(error Error: MPU9250_I2C_DEV not configured!)
endif
endif
# convert i2cx to upper/lower case
MPU9250_I2C_DEV_UPPER=$(shell echo $(MPU9250_I2C_DEV) | tr a-z A-Z)
MPU9250_I2C_DEV_LOWER=$(shell echo $(MPU9250_I2C_DEV) | tr A-Z a-z)
IMU_MPU9250_CFLAGS += -DIMU_MPU9250_I2C_DEV=$(MPU9250_I2C_DEV_LOWER)
IMU_MPU9250_CFLAGS += -DUSE_$(MPU9250_I2C_DEV_UPPER)
# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
ap.CFLAGS += $(IMU_MPU9250_CFLAGS)
ap.srcs += $(IMU_MPU9250_SRCS)
test_imu.CFLAGS += $(IMU_MPU9250_CFLAGS)
test_imu.srcs += $(IMU_MPU9250_SRCS)
#
# NPS simulator
#
include $(CFG_SHARED)/imu_nps.makefile
conf/modules/imu_mpu9250.xml
+28
View File
@@ -0,0 +1,28 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="imu_mpu9250" dir="sensors">
<doc>
<description>
Test module for the mpu9250 with I2C
Report RAW values on telemetry
</description>
<configure name="IMU_MPU9250_I2C_DEV" value="i2c1" description="I2C device to use (e.g. i2c1)"/>
<define name="IMU_MPU9250_ADDR" value="MPU9250_ADDR|MPU9250_ADDR_ALT" description="Use regular or alternate I2C address"/>
</doc>
<header>
<file name="imu_mpu9250.h"/>
</header>
<init fun="imu_mpu9250_init()"/>
<periodic fun="imu_mpu9250_periodic()"/>
<periodic fun="imu_mpu9250_report()" freq="10" autorun="TRUE"/>
<event fun="imu_mpu9250_event()"/>
<makefile>
<file name="imu_mpu9250.c"/>
<file name="mpu9250.c" dir="peripherals"/>
<file name="mpu9250_i2c.c" dir="peripherals"/>
<file name="ak8963.c" dir="peripherals"/>
<define name="USE_I2C"/>
<define name="IMU_MPU9250_I2C_DEV" value="$(IMU_MPU9250_I2C_DEV)"/>
</makefile>
</module>
sw/airborne/modules/sensors/imu_mpu9250.c
+78
View File
@@ -0,0 +1,78 @@
/*
* Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file "modules/sensors/imu_mpu9250.c"
* @author Gautier Hattenberger
*
* Test module for the mpu9250
*/
#include "modules/sensors/imu_mpu9250.h"
// Default I2C address
#ifndef IMU_MPU9250_ADDR
#define IMU_MPU9250_ADDR MPU9250_ADDR
#endif
struct Mpu9250_I2c mpu9250;
void imu_mpu9250_init(void)
{
mpu9250_i2c_init(&mpu9250, &(IMU_MPU9250_I2C_DEV), IMU_MPU9250_ADDR);
}
void imu_mpu9250_periodic(void)
{
mpu9250_i2c_periodic(&mpu9250);
}
void imu_mpu9250_event(void)
{
mpu9250_i2c_event(&mpu9250);
}
#include "math/pprz_algebra_int.h"
#include "subsystems/datalink/downlink.h"
void imu_mpu9250_report(void)
{
struct Int32Vect3 accel = {
(int32_t)(mpu9250.data_accel.vect.x),
(int32_t)(mpu9250.data_accel.vect.y),
(int32_t)(mpu9250.data_accel.vect.z)
};
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &accel.x, &accel.y, &accel.z);
struct Int32Rates rates = {
(int32_t)(mpu9250.data_rates.rates.p),
(int32_t)(mpu9250.data_rates.rates.q),
(int32_t)(mpu9250.data_rates.rates.r)
};
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &rates.p, &rates.q, &rates.r);
struct Int32Vect3 mag = {
(int32_t)(mpu9250.akm.data.vect.x),
(int32_t)(mpu9250.akm.data.vect.y),
(int32_t)(mpu9250.akm.data.vect.z)
};
DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z);
}
sw/airborne/modules/sensors/imu_mpu9250.h
+41
View File
@@ -0,0 +1,41 @@
/*
* Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file "modules/sensors/imu_mpu9250.h"
* @author Gautier Hattenberger
*
* Test module for the mpu9250
*/
#ifndef IMU_MPU9250_H
#define IMU_MPU9250_H
#include "peripherals/mpu9250_i2c.h"
extern struct Mpu9250_I2c mpu9250;
extern void imu_mpu9250_init(void);
extern void imu_mpu9250_periodic(void);
extern void imu_mpu9250_event(void);
extern void imu_mpu9250_report(void);
#endif
sw/airborne/peripherals/ak8963.c
+142
View File
@@ -0,0 +1,142 @@
/*
* Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.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 peripherals/ak8963.c
*
* Driver for the AKM AK8963 magnetometer.
*/
#include "peripherals/ak8963.h"
#include "std.h"
/**
* Initialize AK8963 struct
*/
void ak8963_init(struct Ak8963 *ak, struct i2c_periph *i2c_p, uint8_t addr)
{
/* set i2c_peripheral */
ak->i2c_p = i2c_p;
/* set i2c address */
ak->i2c_trans.slave_addr = addr;
ak->i2c_trans.status = I2CTransDone;
ak->initialized = FALSE;
ak->init_status = AK_CONF_UNINIT;
ak->data_available = FALSE;
}
void ak8963_configure(struct Ak8963 *ak)
{
// Only configure when not busy
if (ak->i2c_trans.status != I2CTransSuccess && ak->i2c_trans.status != I2CTransFailed && ak->i2c_trans.status != I2CTransDone) {
return;
}
// Only when succesfull continue with next
if (ak->i2c_trans.status == I2CTransSuccess) {
ak->init_status++;
}
ak->i2c_trans.status = I2CTransDone;
switch (ak->init_status) {
// Soft Reset the device
case AK_CONF_UNINIT:
ak->i2c_trans.buf[0] = AK8963_REG_CNTL2;
ak->i2c_trans.buf[1] = 1;
i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2);
break;
// Set it to continious measuring mode 2
case AK_CONF_MODE:
ak->i2c_trans.buf[0] = AK8963_REG_CNTL1;
ak->i2c_trans.buf[1] = AK8963_CNTL1_CM_2;
i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2);
break;
// Initialization done
default:
ak->initialized = TRUE;
break;
}
}
void ak8963_read(struct Ak8963 *ak)
{
if (ak->status != AK_STATUS_IDLE) {
return;
}
// Read the status register
ak->i2c_trans.buf[0] = AK8963_REG_ST1;
i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 1);
}
#define Int16FromBuf(_buf,_idx) ((int16_t)(_buf[_idx] | (_buf[_idx+1] << 8)))
void ak8963_event(struct Ak8963 *ak)
{
if (!ak->initialized) {
return;
}
switch (ak->status) {
case AK_STATUS_IDLE:
// When DRDY start reading
if (ak->i2c_trans.status == I2CTransSuccess && ak->i2c_trans.buf[0] & 1) {
ak->i2c_trans.buf[0] = AK8963_REG_HXL;
i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 6);
ak->status++;
}
break;
case AK_STATUS_READ:
if (ak->i2c_trans.status == I2CTransSuccess) {
// Copy the data
ak->data.vect.x = Int16FromBuf(ak->i2c_trans.buf, 0);
ak->data.vect.y = Int16FromBuf(ak->i2c_trans.buf, 2);
ak->data.vect.z = Int16FromBuf(ak->i2c_trans.buf, 4);
ak->data_available = TRUE;
// Read second status register to be ready for reading again
ak->i2c_trans.buf[0] = AK8963_REG_ST2;
i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 1);
ak->status++;
break;
}
break;
default:
if (ak->i2c_trans.status == I2CTransSuccess || ak->i2c_trans.status == I2CTransFailed) {
// Goto idle
ak->i2c_trans.status = I2CTransDone;
ak->status = AK_STATUS_IDLE;
// check overrun
//if (bit_is_set(ak->i2c_trans.buf[0], 3)) {
// ak->data_available = FALSE;
//} else {
// ak->data_available = TRUE;
//}
}
break;
}
}
sw/airborne/peripherals/ak8963.h
+84
View File
@@ -0,0 +1,84 @@
/*
* Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.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 peripherals/ak8963.c
*
* Driver for the AKM AK8963 magnetometer.
*/
#ifndef AK8963_H
#define AK8963_H
#include "std.h"
#include "mcu_periph/i2c.h"
#include "math/pprz_algebra_int.h"
/* Address and register definitions */
#include "peripherals/ak8963_regs.h"
/** Config status states */
enum Ak8963ConfStatus {
AK_CONF_UNINIT,
AK_CONF_MODE,
AK_CONF_DONE
};
/** Normal status states */
enum Ak8963Status {
AK_STATUS_IDLE,
AK_STATUS_READ,
AK_STATUS_DONE
};
/** Default Ak8963 structure */
struct Ak8963 {
struct i2c_periph *i2c_p; ///< peripheral used for communcation
struct i2c_transaction i2c_trans; ///< i2c transaction used for communication with the ak8936
bool_t initialized; ///< config done flag
enum Ak8963Status status; ///< main status
enum Ak8963ConfStatus init_status; ///< init status
volatile bool_t data_available; ///< data ready flag
union {
struct Int16Vect3 vect; ///< data vector in mag coordinate system
int16_t value[3]; ///< data values accessible by channel index
} data;
};
// Functions
extern void ak8963_init(struct Ak8963 *ak, struct i2c_periph *i2c_p, uint8_t addr);
extern void ak8963_configure(struct Ak8963 *ak);
extern void ak8963_event(struct Ak8963 *ak);
extern void ak8963_read(struct Ak8963 *ak);
/// convenience function: read or start configuration if not already initialized
static inline void ak8963_periodic(struct Ak8963 *ak)
{
if (ak->initialized) {
ak8963_read(ak);
} else {
ak8963_configure(ak);
}
}
#endif /* AK8963_H */
sw/airborne/peripherals/ak8963_regs.h
+65
View File
@@ -0,0 +1,65 @@
/*
* Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.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 peripherals/ak8963.h
*
* Register and address definitions for AK8963 magnetometer.
*/
#ifndef AK8963_REGS_H
#define AK8963_REGS_H
#define AK8963_ADDR 0x1A
/* Compass device dependent definition */
#define AK8963_CNTL1_POWER_DOWN 0x10
#define AK8963_CNTL1_SNG_MEASURE 0x11
#define AK8963_CNTL1_CM_1 0x12
#define AK8963_CNTL1_CM_2 0x16
#define AK8963_CNTL1_EXT_TRIG 0x14
#define AK8963_CNTL1_SELF_TEST 0x18
#define AK8963_CNTL1_FUSE_ACCESS 0x1F
/* AK8963 register address */
#define AK8963_REG_WIA 0x00
#define AK8963_REG_INFO 0x01
#define AK8963_REG_ST1 0x02
#define AK8963_REG_HXL 0x03
#define AK8963_REG_HXH 0x04
#define AK8963_REG_HYL 0x05
#define AK8963_REG_HYH 0x06
#define AK8963_REG_HZL 0x07
#define AK8963_REG_HZH 0x08
#define AK8963_REG_ST2 0x09
#define AK8963_REG_CNTL1 0x0A
#define AK8963_REG_CNTL2 0x0B
#define AK8963_REG_ASTC 0x0C
#define AK8963_REG_TS1 0x0D
#define AK8963_REG_TS2 0x0E
#define AK8963_REG_I2CDIS 0x0F
/* AK8963 fuse-rom address */
#define AK8963_FUSE_ASAX 0x10
#define AK8963_FUSE_ASAY 0x11
#define AK8963_FUSE_ASAZ 0x12
#endif /* AK8963_REGS_H */
sw/airborne/peripherals/mpu9250.c
+118
View File
@@ -0,0 +1,118 @@
/*
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.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 peripherals/mpu9250.c
*
* MPU-9250 driver common functions (I2C and SPI).
*
* Still needs the either the I2C or SPI specific implementation.
*/
#include "peripherals/mpu9250.h"
void mpu9250_set_default_config(struct Mpu9250Config *c)
{
c->clk_sel = MPU9250_DEFAULT_CLK_SEL;
c->smplrt_div = MPU9250_DEFAULT_SMPLRT_DIV;
c->dlpf_gyro_cfg = MPU9250_DEFAULT_DLPF_GYRO_CFG;
c->dlpf_accel_cfg = MPU9250_DEFAULT_DLPF_ACCEL_CFG;
c->gyro_range = MPU9250_DEFAULT_FS_SEL;
c->accel_range = MPU9250_DEFAULT_AFS_SEL;
c->drdy_int_enable = FALSE;
/* Number of bytes to read starting with MPU9250_REG_INT_STATUS
* By default read only gyro and accel data -> 15 bytes.
* Increase to include slave data.
*/
c->nb_bytes = 15;
c->nb_slaves = 0;
c->i2c_bypass = FALSE;
}
void mpu9250_send_config(Mpu9250ConfigSet mpu_set, void* mpu, struct Mpu9250Config* config)
{
switch (config->init_status) {
case MPU9250_CONF_RESET:
/* device reset, set register values to defaults */
mpu_set(mpu, MPU9250_REG_PWR_MGMT_1, (1<<6));
config->init_status++;
break;
case MPU9250_CONF_USER_RESET:
/* trigger FIFO, I2C_MST and SIG_COND resets */
mpu_set(mpu, MPU9250_REG_USER_CTRL, ((1 << MPU9250_FIFO_RESET) |
(1 << MPU9250_I2C_MST_RESET) |
(1 << MPU9250_SIG_COND_RESET)));
config->init_status++;
break;
case MPU9250_CONF_PWR:
/* switch to gyroX clock by default */
mpu_set(mpu, MPU9250_REG_PWR_MGMT_1, ((config->clk_sel)|(0<<6)));
config->init_status++;
break;
case MPU9250_CONF_SD:
/* configure sample rate divider */
mpu_set(mpu, MPU9250_REG_SMPLRT_DIV, config->smplrt_div);
config->init_status++;
break;
case MPU9250_CONF_DLPF_GYRO:
/* configure digital low pass filter for gyroscope */
mpu_set(mpu, MPU9250_REG_CONFIG, config->dlpf_gyro_cfg);
config->init_status++;
break;
case MPU9250_CONF_DLPF_ACCEL:
/* configure digital low pass filter fir accelerometer */
mpu_set(mpu, MPU9250_REG_ACCEL_CONFIG_2, config->dlpf_accel_cfg);
config->init_status++;
break;
case MPU9250_CONF_GYRO:
/* configure gyro range */
mpu_set(mpu, MPU9250_REG_GYRO_CONFIG, (config->gyro_range<<3));
config->init_status++;
break;
case MPU9250_CONF_ACCEL:
/* configure accelerometer range */
mpu_set(mpu, MPU9250_REG_ACCEL_CONFIG, (config->accel_range<<3));
config->init_status++;
break;
case MPU9250_CONF_I2C_SLAVES:
/* if any, set MPU for I2C slaves and configure them*/
if (config->nb_slaves > 0) {
/* returns TRUE when all slaves are configured */
if (mpu9250_configure_i2c_slaves(mpu_set, mpu))
config->init_status++;
}
else
config->init_status++;
break;
case MPU9250_CONF_INT_ENABLE:
/* configure data ready interrupt */
mpu_set(mpu, MPU9250_REG_INT_ENABLE, (config->drdy_int_enable<<0));
config->init_status++;
break;
case MPU9250_CONF_DONE:
config->initialized = TRUE;
break;
default:
break;
}
}
sw/airborne/peripherals/mpu9250.h
+112
View File
@@ -0,0 +1,112 @@
/*
* Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file peripherals/mpu9250.h
*
* MPU-60X0 driver common interface (I2C and SPI).
*/
#ifndef MPU9250_H
#define MPU9250_H
#include "std.h"
/* Include address and register definition */
#include "peripherals/mpu9250_regs.h"
/// Default sample rate divider
#define MPU9250_DEFAULT_SMPLRT_DIV 0
/// Default gyro full scale range +- 2000°/s
#define MPU9250_DEFAULT_FS_SEL MPU9250_GYRO_RANGE_1000
/// Default accel full scale range +- 16g
#define MPU9250_DEFAULT_AFS_SEL MPU9250_ACCEL_RANGE_8G
/// Default internal sampling (1kHz, 42Hz LP Bandwidth)
#define MPU9250_DEFAULT_DLPF_ACCEL_CFG MPU9250_DLPF_ACCEL_41HZ
/// Default internal sampling (1kHz, 42Hz LP Bandwidth)
#define MPU9250_DEFAULT_DLPF_GYRO_CFG MPU9250_DLPF_GYRO_41HZ
/// Default interrupt config: DATA_RDY_EN
#define MPU9250_DEFAULT_INT_CFG 1
/// Default clock: PLL with X gyro reference
#define MPU9250_DEFAULT_CLK_SEL 1
enum Mpu9250ConfStatus {
MPU9250_CONF_UNINIT,
MPU9250_CONF_RESET,
MPU9250_CONF_USER_RESET,
MPU9250_CONF_PWR,
MPU9250_CONF_SD,
MPU9250_CONF_DLPF_ACCEL,
MPU9250_CONF_DLPF_GYRO,
MPU9250_CONF_GYRO,
MPU9250_CONF_ACCEL,
MPU9250_CONF_I2C_SLAVES,
MPU9250_CONF_INT_ENABLE,
MPU9250_CONF_DONE
};
/// Configuration function prototype
typedef void (*Mpu9250ConfigSet)(void* mpu, uint8_t _reg, uint8_t _val);
/// function prototype for configuration of a single I2C slave
typedef bool_t (*Mpu9250I2cSlaveConfigure)(Mpu9250ConfigSet mpu_set, void* mpu);
struct Mpu9250I2cSlave {
Mpu9250I2cSlaveConfigure configure;
};
struct Mpu9250Config {
uint8_t smplrt_div; ///< Sample rate divider
enum Mpu9250DLPFAccel dlpf_accel_cfg; ///< Digital Low Pass Filter for accelerometer
enum Mpu9250DLPFGyro dlpf_gyro_cfg; ///< Digital Low Pass Filter for gyroscope
enum Mpu9250GyroRanges gyro_range; ///< deg/s Range
enum Mpu9250AccelRanges accel_range; ///< g Range
bool_t drdy_int_enable; ///< Enable Data Ready Interrupt
uint8_t clk_sel; ///< Clock select
uint8_t nb_bytes; ///< number of bytes to read starting with MPU9250_REG_INT_STATUS
enum Mpu9250ConfStatus init_status; ///< init status
bool_t initialized; ///< config done flag
/** Bypass MPU I2C.
* Only effective if using the I2C implementation.
*/
bool_t i2c_bypass;
uint8_t nb_slaves; ///< number of used I2C slaves
struct Mpu9250I2cSlave slaves[5]; ///< I2C slaves
enum Mpu9250MstClk i2c_mst_clk; ///< MPU I2C master clock speed
uint8_t i2c_mst_delay; ///< MPU I2C slaves delayed sample rate
};
extern void mpu9250_set_default_config(struct Mpu9250Config *c);
/// Configuration sequence called once before normal use
extern void mpu9250_send_config(Mpu9250ConfigSet mpu_set, void* mpu, struct Mpu9250Config* config);
/**
* Configure I2C slaves of the MPU.
* This is I2C/SPI implementation specific.
* @param mpu_set configuration function
* @param mpu Mpu9250Spi or Mpu9250I2c peripheral
* @return TRUE when all slaves are configured
*/
extern bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void* mpu);
#endif // MPU9250_H
sw/airborne/peripherals/mpu9250_i2c.c
+218
View File
@@ -0,0 +1,218 @@
/*
* Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file peripherals/mpu9250_i2c.c
*
* Driver for the MPU-9250 using I2C.
*
*/
#include "peripherals/mpu9250_i2c.h"
bool_t imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set __attribute__ ((unused)), void* mpu __attribute__ ((unused)));
void mpu9250_i2c_init(struct Mpu9250_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 MPU9250 config options */
mpu9250_set_default_config(&(mpu->config));
mpu->data_available = FALSE;
mpu->config.initialized = FALSE;
mpu->config.init_status = MPU9250_CONF_UNINIT;
/* "internal" ak8963 magnetometer */
ak8963_init(&mpu->akm, i2c_p, MPU9250_MAG_ADDR);
/* mag is declared as slave to call the configure function,
* regardless if it is an actual MPU slave or passthrough
*/
mpu->config.nb_slaves = 1;
/* set callback function to configure mag */
mpu->config.slaves[0].configure = &imu_mpu9250_configure_mag_slave;
/* read the mag directly for now */
mpu->config.i2c_bypass = TRUE;
mpu->slave_init_status = MPU9250_I2C_CONF_UNINIT;
}
static void mpu9250_i2c_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) {
struct Mpu9250_I2c* mpu_i2c = (struct Mpu9250_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->i2c_trans.slave_addr, 2);
}
// Configuration function called once before normal use
void mpu9250_i2c_start_configure(struct Mpu9250_I2c *mpu)
{
if (mpu->config.init_status == MPU9250_CONF_UNINIT) {
mpu->config.init_status++;
if (mpu->i2c_trans.status == I2CTransSuccess || mpu->i2c_trans.status == I2CTransDone) {
mpu9250_send_config(mpu9250_i2c_write_to_reg, (void*)mpu, &(mpu->config));
}
}
}
void mpu9250_i2c_read(struct Mpu9250_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] = MPU9250_REG_INT_STATUS;
i2c_transceive(mpu->i2c_p, &(mpu->i2c_trans), mpu->i2c_trans.slave_addr, 1, mpu->config.nb_bytes);
/* read mag */
#ifdef MPU9250_MAG_PRESCALER
RunOnceEvery(MPU9250_MAG_PRESCALER, ak8963_read(&mpu->akm));
#else
ak8963_read(&mpu->akm);
#endif
}
}
#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
void mpu9250_i2c_event(struct Mpu9250_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);
// if we are reading slaves through the mpu, copy the ext_sens_data
if ((mpu->config.i2c_bypass == FALSE) && (mpu->config.nb_slaves > 0))
memcpy(mpu->data_ext, (void *) &(mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15);
mpu->data_available = TRUE;
}
mpu->i2c_trans.status = I2CTransDone;
}
}
else if (mpu->config.init_status != MPU9250_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:
mpu9250_send_config(mpu9250_i2c_write_to_reg, (void*)mpu, &(mpu->config));
if (mpu->config.initialized)
mpu->i2c_trans.status = I2CTransDone;
break;
default:
break;
}
}
// Ak8963 event function
ak8963_event(&mpu->akm);
}
/** callback function to configure ak8963 mag
* @return TRUE if mag configuration finished
*/
bool_t imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set __attribute__ ((unused)), void* mpu)
{
struct Mpu9250_I2c* mpu_i2c = (struct Mpu9250_I2c*)(mpu);
ak8963_configure(&mpu_i2c->akm);
if (mpu_i2c->akm.initialized)
return TRUE;
else
return FALSE;
}
/** @todo: only one slave so far. */
bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void* mpu)
{
struct Mpu9250_I2c* mpu_i2c = (struct Mpu9250_I2c*)(mpu);
if (mpu_i2c->slave_init_status == MPU9250_I2C_CONF_UNINIT)
mpu_i2c->slave_init_status++;
switch (mpu_i2c->slave_init_status) {
case MPU9250_I2C_CONF_I2C_MST_DIS:
mpu_set(mpu, MPU9250_REG_USER_CTRL, 0);
mpu_i2c->slave_init_status++;
break;
case MPU9250_I2C_CONF_I2C_BYPASS_EN:
/* switch to I2C passthrough */
mpu_set(mpu, MPU9250_REG_INT_PIN_CFG, (1<<1));
mpu_i2c->slave_init_status++;
break;
case MPU9250_I2C_CONF_SLAVES_CONFIGURE:
/* configure each slave. TODO: currently only one */
if (mpu_i2c->config.slaves[0].configure(mpu_set, mpu))
mpu_i2c->slave_init_status++;
break;
case MPU9250_I2C_CONF_I2C_BYPASS_DIS:
if (mpu_i2c->config.i2c_bypass) {
/* if bypassing I2C skip MPU I2C master setup */
mpu_i2c->slave_init_status = MPU9250_I2C_CONF_DONE;
}
else {
/* disable I2C passthrough again */
mpu_set(mpu, MPU9250_REG_INT_PIN_CFG, (0<<1));
mpu_i2c->slave_init_status++;
}
break;
case MPU9250_I2C_CONF_I2C_MST_CLK:
/* configure MPU I2C master clock and stop/start between slave reads */
mpu_set(mpu, MPU9250_REG_I2C_MST_CTRL,
((1<<4) | mpu_i2c->config.i2c_mst_clk));
mpu_i2c->slave_init_status++;
break;
case MPU9250_I2C_CONF_I2C_MST_DELAY:
/* Set I2C slaves delayed sample rate */
mpu_set(mpu, MPU9250_REG_I2C_MST_DELAY, mpu_i2c->config.i2c_mst_delay);
mpu_i2c->slave_init_status++;
break;
case MPU9250_I2C_CONF_I2C_SMPLRT:
/* I2C slave0 sample rate/2 = 100/2 = 50Hz */
mpu_set(mpu, MPU9250_REG_I2C_SLV4_CTRL, 0);
mpu_i2c->slave_init_status++;
break;
case MPU9250_I2C_CONF_I2C_MST_EN:
/* enable internal I2C master */
mpu_set(mpu, MPU9250_REG_USER_CTRL, (1 << MPU9250_I2C_MST_EN));
mpu_i2c->slave_init_status++;
break;
case MPU9250_I2C_CONF_DONE:
return TRUE;
default:
break;
}
return FALSE;
}
sw/airborne/peripherals/mpu9250_i2c.h
+87
View File
@@ -0,0 +1,87 @@
/*
* 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/mpu9250_i2c.h
*
* Driver for the MPU-9250 using I2C.
*/
#ifndef MPU9250_I2C_H
#define MPU9250_I2C_H
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "mcu_periph/i2c.h"
/* Include common MPU9250 options and definitions */
#include "peripherals/mpu9250.h"
#include "peripherals/ak8963.h"
#define MPU9250_BUFFER_EXT_LEN 16
enum Mpu9250I2cSlaveInitStatus {
MPU9250_I2C_CONF_UNINIT,
MPU9250_I2C_CONF_I2C_MST_DIS,
MPU9250_I2C_CONF_I2C_BYPASS_EN,
MPU9250_I2C_CONF_SLAVES_CONFIGURE,
MPU9250_I2C_CONF_I2C_BYPASS_DIS,
MPU9250_I2C_CONF_I2C_MST_CLK,
MPU9250_I2C_CONF_I2C_MST_DELAY,
MPU9250_I2C_CONF_I2C_SMPLRT,
MPU9250_I2C_CONF_I2C_MST_EN,
MPU9250_I2C_CONF_DONE
};
struct Mpu9250_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;
uint8_t data_ext[MPU9250_BUFFER_EXT_LEN];
struct Mpu9250Config config;
enum Mpu9250I2cSlaveInitStatus slave_init_status;
struct Ak8963 akm; ///< "internal" magnetometer
};
// Functions
extern void mpu9250_i2c_init(struct Mpu9250_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr);
extern void mpu9250_i2c_start_configure(struct Mpu9250_I2c *mpu);
extern void mpu9250_i2c_read(struct Mpu9250_I2c *mpu);
extern void mpu9250_i2c_event(struct Mpu9250_I2c *mpu);
/// convenience function: read or start configuration if not already initialized
static inline void mpu9250_i2c_periodic(struct Mpu9250_I2c *mpu) {
if (mpu->config.initialized)
mpu9250_i2c_read(mpu);
else
mpu9250_i2c_start_configure(mpu);
}
#endif // MPU9250_I2C_H
sw/airborne/peripherals/mpu9250_regs.h
+201
View File
@@ -0,0 +1,201 @@
/*
* 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/mpu9250_regs.h
*
* Register and address definitions for MPU-9250.
*/
#ifndef MPU9250_REGS_H
#define MPU9250_REGS_H
/* default I2C address */
#define MPU9250_ADDR 0xD0
#define MPU9250_ADDR_ALT 0xD2
#define MPU9250_MAG_ADDR 0x18
#define MPU9250_SPI_READ 0x80
// Power and Interface
#define MPU9250_REG_AUX_VDDIO 0x01 // Must be set to 0 on MPU6000
#define MPU9250_REG_USER_CTRL 0x6A
#define MPU9250_REG_PWR_MGMT_1 0x6B
#define MPU9250_REG_PWR_MGMT_2 0x6C
// FIFO
#define MPU9250_REG_FIFO_EN 0x23
#define MPU9250_REG_FIFO_COUNT_H 0x72
#define MPU9250_REG_FIFO_COUNT_L 0x73
#define MPU9250_REG_FIFO_R_W 0x74
// Measurement Settings
#define MPU9250_REG_SMPLRT_DIV 0x19
#define MPU9250_REG_CONFIG 0x1A
#define MPU9250_REG_GYRO_CONFIG 0x1B
#define MPU9250_REG_ACCEL_CONFIG 0x1C
#define MPU9250_REG_ACCEL_CONFIG_2 0x1D
// I2C Slave settings
#define MPU9250_REG_I2C_MST_CTRL 0x24
#define MPU9250_REG_I2C_MST_STATUS 0x36
#define MPU9250_REG_I2C_MST_DELAY 0x67
// Slave 0
#define MPU9250_REG_I2C_SLV0_ADDR 0X25 // i2c addr
#define MPU9250_REG_I2C_SLV0_REG 0X26 // slave reg
#define MPU9250_REG_I2C_SLV0_CTRL 0X27 // set-bits
#define MPU9250_REG_I2C_SLV0_DO 0X63 // DO
// Slave 1
#define MPU9250_REG_I2C_SLV1_ADDR 0X28 // i2c addr
#define MPU9250_REG_I2C_SLV1_REG 0X29 // slave reg
#define MPU9250_REG_I2C_SLV1_CTRL 0X2A // set-bits
#define MPU9250_REG_I2C_SLV1_DO 0X64 // DO
// Slave 2
#define MPU9250_REG_I2C_SLV2_ADDR 0X2B // i2c addr
#define MPU9250_REG_I2C_SLV2_REG 0X2C // slave reg
#define MPU9250_REG_I2C_SLV2_CTRL 0X2D // set-bits
#define MPU9250_REG_I2C_SLV2_DO 0X65 // DO
// Slave 3
#define MPU9250_REG_I2C_SLV3_ADDR 0X2E // i2c addr
#define MPU9250_REG_I2C_SLV3_REG 0X2F // slave reg
#define MPU9250_REG_I2C_SLV3_CTRL 0X30 // set-bits
#define MPU9250_REG_I2C_SLV3_DO 0X66 // DO
// Slave 4 - special
#define MPU9250_REG_I2C_SLV4_ADDR 0X31 // i2c addr
#define MPU9250_REG_I2C_SLV4_REG 0X32 // slave reg
#define MPU9250_REG_I2C_SLV4_DO 0X33 // DO
#define MPU9250_REG_I2C_SLV4_CTRL 0X34 // set-bits
#define MPU9250_REG_I2C_SLV4_DI 0X35 // DI
// Interrupt
#define MPU9250_REG_INT_PIN_CFG 0x37
#define MPU9250_REG_INT_ENABLE 0x38
#define MPU9250_REG_INT_STATUS 0x3A
// Accelero
#define MPU9250_REG_ACCEL_XOUT_H 0x3B
#define MPU9250_REG_ACCEL_XOUT_L 0x3C
#define MPU9250_REG_ACCEL_YOUT_H 0x3D
#define MPU9250_REG_ACCEL_YOUT_L 0x3E
#define MPU9250_REG_ACCEL_ZOUT_H 0x3F
#define MPU9250_REG_ACCEL_ZOUT_L 0x40
// Temperature
#define MPU9250_REG_TEMP_OUT_H 0x41
#define MPU9250_REG_TEMP_OUT_L 0x42
// Gyro
#define MPU9250_REG_GYRO_XOUT_H 0x43
#define MPU9250_REG_GYRO_XOUT_L 0x44
#define MPU9250_REG_GYRO_YOUT_H 0x45
#define MPU9250_REG_GYRO_YOUT_L 0x46
#define MPU9250_REG_GYRO_ZOUT_H 0x47
#define MPU9250_REG_GYRO_ZOUT_L 0x48
// External Sensor Data
#define MPU9250_EXT_SENS_DATA 0x49
#define MPU9250_EXT_SENS_DATA_SIZE 24
#define MPU9250_REG_WHO_AM_I 0x75
#define MPU9250_WHOAMI_REPLY 0x68
// Bit positions
#define MPU9250_I2C_BYPASS_EN 1
// in MPU9250_REG_USER_CTRL
#define MPU9250_SIG_COND_RESET 0
#define MPU9250_I2C_MST_RESET 1
#define MPU9250_FIFO_RESET 2
#define MPU9250_I2C_IF_DIS 4
#define MPU9250_I2C_MST_EN 5
#define MPU9250_FIFO_EN 6
// in MPU9250_REG_I2C_MST_STATUS
#define MPU9250_I2C_SLV4_DONE 6
/** Digital Low Pass Filter Options
*/
enum Mpu9250DLPFGyro {
MPU9250_DLPF_GYRO_250HZ = 0x0, // internal sampling rate 8kHz
MPU9250_DLPF_GYRO_184HZ = 0x1, // internal sampling rate 1kHz
MPU9250_DLPF_GYRO_92HZ = 0x2,
MPU9250_DLPF_GYRO_41HZ = 0x3,
MPU9250_DLPF_GYRO_20HZ = 0x4,
MPU9250_DLPF_GYRO_10HZ = 0x5,
MPU9250_DLPF_GYRO_05HZ = 0x6
};
enum Mpu9250DLPFAccel {
MPU9250_DLPF_ACCEL_460HZ = 0x0, // internal sampling rate 8kHz
MPU9250_DLPF_ACCEL_184HZ = 0x1, // internal sampling rate 1kHz
MPU9250_DLPF_ACCEL_92HZ = 0x2,
MPU9250_DLPF_ACCEL_41HZ = 0x3,
MPU9250_DLPF_ACCEL_20HZ = 0x4,
MPU9250_DLPF_ACCEL_10HZ = 0x5,
MPU9250_DLPF_ACCEL_05HZ = 0x6
};
/**
* Selectable gyro range
*/
enum Mpu9250GyroRanges {
MPU9250_GYRO_RANGE_250 = 0x00,
MPU9250_GYRO_RANGE_500 = 0x01,
MPU9250_GYRO_RANGE_1000 = 0x02,
MPU9250_GYRO_RANGE_2000 = 0x03
};
/**
* Selectable accel range
*/
enum Mpu9250AccelRanges {
MPU9250_ACCEL_RANGE_2G = 0x00,
MPU9250_ACCEL_RANGE_4G = 0x01,
MPU9250_ACCEL_RANGE_8G = 0x02,
MPU9250_ACCEL_RANGE_16G = 0x03
};
/**
* I2C Master clock
*/
enum Mpu9250MstClk {
MPU9250_MST_CLK_500KHZ = 0x9,
MPU9250_MST_CLK_471KHZ = 0xA,
MPU9250_MST_CLK_444KHZ = 0xB,
MPU9250_MST_CLK_421KHZ = 0xC,
MPU9250_MST_CLK_400KHZ = 0xD,
MPU9250_MST_CLK_381KHZ = 0xE,
MPU9250_MST_CLK_364KHZ = 0xF,
MPU9250_MST_CLK_348KHZ = 0x0,
MPU9250_MST_CLK_333KHZ = 0x1,
MPU9250_MST_CLK_320KHZ = 0x2,
MPU9250_MST_CLK_308KHZ = 0x3,
MPU9250_MST_CLK_296KHZ = 0x4,
MPU9250_MST_CLK_286KHZ = 0x5,
MPU9250_MST_CLK_276KHZ = 0x6,
MPU9250_MST_CLK_267KHZ = 0x7,
MPU9250_MST_CLK_258KHZ = 0x8
};
#endif /* MPU9250_REGS_H */
sw/airborne/peripherals/mpu9250_spi.c
+177
View File
@@ -0,0 +1,177 @@
/*
* 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/mpu9250_spi.c
*
* Driver for the MPU-9250 using SPI.
*
*/
#include "peripherals/mpu9250_spi.h"
void mpu9250_spi_init(struct Mpu9250_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 = 2;
mpu->spi_trans.input_length = MPU9250_BUFFER_LEN;
mpu->spi_trans.before_cb = NULL;
mpu->spi_trans.after_cb = NULL;
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 MPU9250 config options */
mpu9250_set_default_config(&(mpu->config));
mpu->data_available = FALSE;
mpu->config.initialized = FALSE;
mpu->config.init_status = MPU9250_CONF_UNINIT;
mpu->slave_init_status = MPU9250_SPI_CONF_UNINIT;
}
static void mpu9250_spi_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) {
struct Mpu9250_Spi* mpu_spi = (struct Mpu9250_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 mpu9250_spi_start_configure(struct Mpu9250_Spi *mpu)
{
if (mpu->config.init_status == MPU9250_CONF_UNINIT) {
mpu->config.init_status++;
if (mpu->spi_trans.status == SPITransSuccess || mpu->spi_trans.status == SPITransDone) {
mpu9250_send_config(mpu9250_spi_write_to_reg, (void*)mpu, &(mpu->config));
}
}
}
void mpu9250_spi_read(struct Mpu9250_Spi *mpu)
{
if (mpu->config.initialized && mpu->spi_trans.status == SPITransDone) {
mpu->spi_trans.output_length = 1;
mpu->spi_trans.input_length = 1 + mpu->config.nb_bytes;
/* set read bit and multiple byte bit, then address */
mpu->tx_buf[0] = MPU9250_REG_INT_STATUS | MPU9250_SPI_READ;
spi_submit(mpu->spi_p, &(mpu->spi_trans));
}
}
#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
void mpu9250_spi_event(struct Mpu9250_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[1], 0)) {
// new data
mpu->data_accel.vect.x = Int16FromBuf(mpu->rx_buf, 2);
mpu->data_accel.vect.y = Int16FromBuf(mpu->rx_buf, 4);
mpu->data_accel.vect.z = Int16FromBuf(mpu->rx_buf, 6);
mpu->data_rates.rates.p = Int16FromBuf(mpu->rx_buf, 10);
mpu->data_rates.rates.q = Int16FromBuf(mpu->rx_buf, 12);
mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf, 14);
// if we are reading slaves, copy the ext_sens_data
if (mpu->config.nb_slaves > 0)
memcpy(mpu->data_ext, (void *) &(mpu->rx_buf[16]), mpu->config.nb_bytes - 15);
mpu->data_available = TRUE;
}
mpu->spi_trans.status = SPITransDone;
}
}
else if (mpu->config.init_status != MPU9250_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:
mpu9250_send_config(mpu9250_spi_write_to_reg, (void*)mpu, &(mpu->config));
if (mpu->config.initialized)
mpu->spi_trans.status = SPITransDone;
break;
default:
break;
}
}
}
/** @todo: only one slave so far. */
bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void* mpu)
{
struct Mpu9250_Spi* mpu_spi = (struct Mpu9250_Spi*)(mpu);
if (mpu_spi->slave_init_status == MPU9250_SPI_CONF_UNINIT)
mpu_spi->slave_init_status++;
switch (mpu_spi->slave_init_status) {
case MPU9250_SPI_CONF_I2C_MST_CLK:
/* configure MPU I2C master clock and stop/start between slave reads */
mpu_set(mpu, MPU9250_REG_I2C_MST_CTRL, ((1<<4) | mpu_spi->config.i2c_mst_clk));
mpu_spi->slave_init_status++;
break;
case MPU9250_SPI_CONF_I2C_MST_DELAY:
/* Set I2C slaves delayed sample rate */
mpu_set(mpu, MPU9250_REG_I2C_MST_DELAY, mpu_spi->config.i2c_mst_delay);
mpu_spi->slave_init_status++;
break;
case MPU9250_SPI_CONF_I2C_MST_EN:
/* enable internal I2C master and disable primary I2C interface */
mpu_set(mpu, MPU9250_REG_USER_CTRL, ((1 << MPU9250_I2C_IF_DIS) |
(1 << MPU9250_I2C_MST_EN)));
mpu_spi->slave_init_status++;
break;
case MPU9250_SPI_CONF_SLAVES_CONFIGURE:
/* configure first slave, only one slave supported so far */
if (mpu_spi->config.slaves[0].configure(mpu_set, mpu))
mpu_spi->slave_init_status++;
break;
case MPU9250_SPI_CONF_DONE:
return TRUE;
default:
break;
}
return FALSE;
}
sw/airborne/peripherals/mpu9250_spi.h
+84
View File
@@ -0,0 +1,84 @@
/*
* 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/mpu9250_spi.h
*
* Driver for the MPU-9250 using SPI.
*/
#ifndef MPU9250_SPI_H
#define MPU9250_SPI_H
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "mcu_periph/spi.h"
/* Include common MPU9250 options and definitions */
#include "peripherals/mpu9250.h"
#define MPU9250_BUFFER_LEN 32
#define MPU9250_BUFFER_EXT_LEN 16
enum Mpu9250SpiSlaveInitStatus {
MPU9250_SPI_CONF_UNINIT,
MPU9250_SPI_CONF_I2C_MST_CLK,
MPU9250_SPI_CONF_I2C_MST_DELAY,
MPU9250_SPI_CONF_I2C_MST_EN,
MPU9250_SPI_CONF_SLAVES_CONFIGURE,
MPU9250_SPI_CONF_DONE
};
struct Mpu9250_Spi {
struct spi_periph *spi_p;
struct spi_transaction spi_trans;
volatile uint8_t tx_buf[2];
volatile uint8_t rx_buf[MPU9250_BUFFER_LEN];
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;
uint8_t data_ext[MPU9250_BUFFER_EXT_LEN];
struct Mpu9250Config config;
enum Mpu9250SpiSlaveInitStatus slave_init_status;
};
// Functions
extern void mpu9250_spi_init(struct Mpu9250_Spi *mpu, struct spi_periph *spi_p, uint8_t addr);
extern void mpu9250_spi_start_configure(struct Mpu9250_Spi *mpu);
extern void mpu9250_spi_read(struct Mpu9250_Spi *mpu);
extern void mpu9250_spi_event(struct Mpu9250_Spi *mpu);
/// convenience function: read or start configuration if not already initialized
static inline void mpu9250_spi_periodic(struct Mpu9250_Spi *mpu) {
if (mpu->config.initialized)
mpu9250_spi_read(mpu);
else
mpu9250_spi_start_configure(mpu);
}
#endif // MPU9250_SPI_H
sw/airborne/subsystems/imu/imu_mpu9250_i2c.c
+154
View File
@@ -0,0 +1,154 @@
/*
* Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file subsystems/imu/imu_mpu9250_i2c.c
*
* IMU driver for the MPU9250 using I2C
*
*/
#include "subsystems/imu.h"
#include "mcu_periph/i2c.h"
#if !defined IMU_MPU9250_GYRO_LOWPASS_FILTER && !defined IMU_MPU9250_ACCEL_LOWPASS_FILTER && !defined IMU_MPU9250_SMPLRT_DIV
#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120)
/* Accelerometer: Bandwidth 41Hz, Delay 5.9ms
* Gyroscope: Bandwidth 41Hz, Delay 5.9ms sampling 1kHz
* Output rate: 100Hz
*/
#define IMU_MPU9250_GYRO_LOWPASS_FILTER MPU9250_DLPF_GYRO_41HZ
#define IMU_MPU9250_ACCEL_LOWPASS_FILTER MPU9250_DLPF_ACCEL_41HZ
#define IMU_MPU9250_SMPLRT_DIV 9
PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
#elif PERIODIC_FREQUENCY == 512
/* Accelerometer: Bandwidth 184Hz, Delay 5.8ms
* Gyroscope: Bandwidth 250Hz, Delay 0.97ms sampling 8kHz
* Output rate: 2kHz
*/
#define IMU_MPU9250_GYRO_LOWPASS_FILTER MPU9250_DLPF_GYRO_250HZ
#define IMU_MPU9250_ACCEL_LOWPASS_FILTER MPU9250_DLPF_ACCEL_184HZ
#define IMU_MPU9250_SMPLRT_DIV 3
PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
#else
/* By default, don't go too fast */
#define IMU_MPU9250_SMPLRT_DIV 9
#define IMU_MPU9250_GYRO_LOWPASS_FILTER MPU9250_DLPF_GYRO_41HZ
#define IMU_MPU9250_ACCEL_LOWPASS_FILTER MPU9250_DLPF_ACCEL_41HZ
PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
#endif
#endif
PRINT_CONFIG_VAR(IMU_MPU9250_SMPLRT_DIV)
PRINT_CONFIG_VAR(IMU_MPU9250_GYRO_LOWPASS_FILTER)
PRINT_CONFIG_VAR(IMU_MPU9250_ACCEL_LOWPASS_FILTER)
#ifndef IMU_MPU9250_GYRO_RANGE
#define IMU_MPU9250_GYRO_RANGE MPU9250_GYRO_RANGE_1000
#endif
PRINT_CONFIG_VAR(IMU_MPU9250_GYRO_RANGE)
#ifndef IMU_MPU9250_ACCEL_RANGE
#define IMU_MPU9250_ACCEL_RANGE MPU9250_ACCEL_RANGE_8G
#endif
PRINT_CONFIG_VAR(IMU_MPU9250_ACCEL_RANGE)
#ifndef IMU_MPU9250_I2C_ADDR
#define IMU_MPU9250_I2C_ADDR MPU9250_ADDR_ALT
#endif
PRINT_CONFIG_VAR(IMU_MPU9250_I2C_ADDR)
// Default channels order
#ifndef IMU_MPU9250_CHAN_X
#define IMU_MPU9250_CHAN_X 0
#endif
PRINT_CONFIG_VAR(IMU_MPU9250_CHAN_X)
#ifndef IMU_MPU9250_CHAN_Y
#define IMU_MPU9250_CHAN_Y 1
#endif
PRINT_CONFIG_VAR(IMU_MPU9250_CHAN_Y)
#ifndef IMU_MPU9250_CHAN_Z
#define IMU_MPU9250_CHAN_Z 2
#endif
PRINT_CONFIG_VAR(IMU_MPU9250_CHAN_Z)
struct ImuMpu9250 imu_mpu9250;
void imu_impl_init(void)
{
/* MPU9250 */
mpu9250_i2c_init(&imu_mpu9250.mpu, &(IMU_MPU9250_I2C_DEV), IMU_MPU9250_I2C_ADDR);
// change the default configuration
imu_mpu9250.mpu.config.smplrt_div = IMU_MPU9250_SMPLRT_DIV;
imu_mpu9250.mpu.config.dlpf_gyro_cfg = IMU_MPU9250_GYRO_LOWPASS_FILTER;
imu_mpu9250.mpu.config.dlpf_accel_cfg = IMU_MPU9250_ACCEL_LOWPASS_FILTER;
imu_mpu9250.mpu.config.gyro_range = IMU_MPU9250_GYRO_RANGE;
imu_mpu9250.mpu.config.accel_range = IMU_MPU9250_ACCEL_RANGE;
imu_mpu9250.gyro_valid = FALSE;
imu_mpu9250.accel_valid = FALSE;
imu_mpu9250.mag_valid = FALSE;
}
void imu_periodic(void)
{
mpu9250_i2c_periodic(&imu_mpu9250.mpu);
}
void imu_mpu9250_event(void)
{
// If the MPU9250 I2C transaction has succeeded: convert the data
mpu9250_i2c_event(&imu_mpu9250.mpu);
if (imu_mpu9250.mpu.data_available) {
// set channel order
struct Int32Vect3 accel = {
(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_X]),
(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Y]),
(int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Z])
};
struct Int32Rates rates = {
(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_X]),
(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]),
(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z])
};
// unscaled vector
VECT3_COPY(imu.accel_unscaled, accel);
RATES_COPY(imu.gyro_unscaled, rates);
imu_mpu9250.mpu.data_available = FALSE;
imu_mpu9250.gyro_valid = TRUE;
imu_mpu9250.accel_valid = TRUE;
}
// Test if mag data are updated
if (imu_mpu9250.mpu.akm.data_available) {
struct Int32Vect3 mag = {
(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_X]),
(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Y]),
(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Z])
};
VECT3_COPY(imu.mag_unscaled, mag);
imu_mpu9250.mag_valid = TRUE;
imu_mpu9250.mpu.akm.data_available = FALSE;
}
}
sw/airborne/subsystems/imu/imu_mpu9250_i2c.h
+101
View File
@@ -0,0 +1,101 @@
/*
* Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* 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, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file subsystems/imu/imu_mpu9250_i2c.h
*
* IMU driver for the MPU9250 using I2C
*
*/
#ifndef IMU_MPU9250_I2C_H
#define IMU_MPU9250_I2C_H
#include "std.h"
#include "generated/airframe.h"
#include "subsystems/imu.h"
#include "peripherals/mpu9250_i2c.h"
/** default gyro sensitivy and neutral from the datasheet
* MPU with 1000 deg/s has 32.8 LSB/(deg/s)
* sens = 1/32.8 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/32.8 * pi/180 * 4096 = 2.17953
I*/
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
// FIXME
#define IMU_GYRO_P_SENS 2.17953
#define IMU_GYRO_P_SENS_NUM 18271
#define IMU_GYRO_P_SENS_DEN 8383
#define IMU_GYRO_Q_SENS 2.17953
#define IMU_GYRO_Q_SENS_NUM 18271
#define IMU_GYRO_Q_SENS_DEN 8383
#define IMU_GYRO_R_SENS 2.17953
#define IMU_GYRO_R_SENS_NUM 18271
#define IMU_GYRO_R_SENS_DEN 8383
#endif
/** default accel sensitivy from the datasheet
* MPU with 8g has 4096 LSB/g
* sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525
*/
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
// FIXME
#define IMU_ACCEL_X_SENS 2.4525
#define IMU_ACCEL_X_SENS_NUM 981
#define IMU_ACCEL_X_SENS_DEN 400
#define IMU_ACCEL_Y_SENS 2.4525
#define IMU_ACCEL_Y_SENS_NUM 981
#define IMU_ACCEL_Y_SENS_DEN 400
#define IMU_ACCEL_Z_SENS 2.4525
#define IMU_ACCEL_Z_SENS_NUM 981
#define IMU_ACCEL_Z_SENS_DEN 400
#endif
struct ImuMpu9250 {
volatile bool_t gyro_valid;
volatile bool_t accel_valid;
volatile bool_t mag_valid;
struct Mpu9250_I2c mpu;
};
extern struct ImuMpu9250 imu_mpu9250;
extern void imu_mpu9250_event(void);
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) {
imu_mpu9250_event();
if (imu_mpu9250.accel_valid) {
imu_mpu9250.accel_valid = FALSE;
_accel_handler();
}
if (imu_mpu9250.mag_valid) {
imu_mpu9250.mag_valid = FALSE;
_mag_handler();
}
if (imu_mpu9250.gyro_valid) {
imu_mpu9250.gyro_valid = FALSE;
_gyro_handler();
}
}
#endif /* IMU_MPU9250_I2C_H */