diff --git a/conf/firmwares/subsystems/shared/imu_mpu9250_i2c.makefile b/conf/firmwares/subsystems/shared/imu_mpu9250_i2c.makefile new file mode 100644 index 00000000000..e08d442631c --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_mpu9250_i2c.makefile @@ -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 diff --git a/conf/modules/imu_mpu9250.xml b/conf/modules/imu_mpu9250.xml new file mode 100644 index 00000000000..c1b39854ed7 --- /dev/null +++ b/conf/modules/imu_mpu9250.xml @@ -0,0 +1,28 @@ + + + + + + Test module for the mpu9250 with I2C + Report RAW values on telemetry + + + + +
+ +
+ + + + + + + + + + + + + + diff --git a/sw/airborne/modules/sensors/imu_mpu9250.c b/sw/airborne/modules/sensors/imu_mpu9250.c new file mode 100644 index 00000000000..1d72446d4fe --- /dev/null +++ b/sw/airborne/modules/sensors/imu_mpu9250.c @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2014 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, see + * . + */ + +/** + * @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); +} + diff --git a/sw/airborne/modules/sensors/imu_mpu9250.h b/sw/airborne/modules/sensors/imu_mpu9250.h new file mode 100644 index 00000000000..a5c272cf751 --- /dev/null +++ b/sw/airborne/modules/sensors/imu_mpu9250.h @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2014 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, see + * . + */ + +/** + * @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 + diff --git a/sw/airborne/peripherals/ak8963.c b/sw/airborne/peripherals/ak8963.c new file mode 100644 index 00000000000..f029394ec82 --- /dev/null +++ b/sw/airborne/peripherals/ak8963.c @@ -0,0 +1,142 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * 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; + } +} + diff --git a/sw/airborne/peripherals/ak8963.h b/sw/airborne/peripherals/ak8963.h new file mode 100644 index 00000000000..92db595306a --- /dev/null +++ b/sw/airborne/peripherals/ak8963.h @@ -0,0 +1,84 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * 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 */ diff --git a/sw/airborne/peripherals/ak8963_regs.h b/sw/airborne/peripherals/ak8963_regs.h new file mode 100644 index 00000000000..dbb926a2fc1 --- /dev/null +++ b/sw/airborne/peripherals/ak8963_regs.h @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * 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 */ diff --git a/sw/airborne/peripherals/mpu9250.c b/sw/airborne/peripherals/mpu9250.c new file mode 100644 index 00000000000..9a3df5c7793 --- /dev/null +++ b/sw/airborne/peripherals/mpu9250.c @@ -0,0 +1,118 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * 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; + } +} diff --git a/sw/airborne/peripherals/mpu9250.h b/sw/airborne/peripherals/mpu9250.h new file mode 100644 index 00000000000..70330b3e810 --- /dev/null +++ b/sw/airborne/peripherals/mpu9250.h @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2014 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, see + * . + */ + +/** + * @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 diff --git a/sw/airborne/peripherals/mpu9250_i2c.c b/sw/airborne/peripherals/mpu9250_i2c.c new file mode 100644 index 00000000000..3237c35224b --- /dev/null +++ b/sw/airborne/peripherals/mpu9250_i2c.c @@ -0,0 +1,218 @@ +/* + * Copyright (C) 2014 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, see + * . + */ + +/** + * @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; +} diff --git a/sw/airborne/peripherals/mpu9250_i2c.h b/sw/airborne/peripherals/mpu9250_i2c.h new file mode 100644 index 00000000000..a826e818e1a --- /dev/null +++ b/sw/airborne/peripherals/mpu9250_i2c.h @@ -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 diff --git a/sw/airborne/peripherals/mpu9250_regs.h b/sw/airborne/peripherals/mpu9250_regs.h new file mode 100644 index 00000000000..d680e8ecf4b --- /dev/null +++ b/sw/airborne/peripherals/mpu9250_regs.h @@ -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 */ diff --git a/sw/airborne/peripherals/mpu9250_spi.c b/sw/airborne/peripherals/mpu9250_spi.c new file mode 100644 index 00000000000..0fff4d53305 --- /dev/null +++ b/sw/airborne/peripherals/mpu9250_spi.c @@ -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; +} diff --git a/sw/airborne/peripherals/mpu9250_spi.h b/sw/airborne/peripherals/mpu9250_spi.h new file mode 100644 index 00000000000..d8135ba41be --- /dev/null +++ b/sw/airborne/peripherals/mpu9250_spi.h @@ -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 diff --git a/sw/airborne/subsystems/imu/imu_mpu9250_i2c.c b/sw/airborne/subsystems/imu/imu_mpu9250_i2c.c new file mode 100644 index 00000000000..24a49708c8d --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_mpu9250_i2c.c @@ -0,0 +1,154 @@ +/* + * Copyright (C) 2014 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, see + * . + */ + +/** + * @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; + } + +} + diff --git a/sw/airborne/subsystems/imu/imu_mpu9250_i2c.h b/sw/airborne/subsystems/imu/imu_mpu9250_i2c.h new file mode 100644 index 00000000000..2a39b804f7a --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_mpu9250_i2c.h @@ -0,0 +1,101 @@ +/* + * Copyright (C) 2014 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, see + * . + */ + +/** + * @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 */