mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-05 23:49:00 +08:00
[peripheral] start mpu driver
This commit is contained in:
committed by
Felix Ruess
parent
d89e21542b
commit
27f28a2228
@@ -1,97 +1,82 @@
|
|||||||
#ifndef MPU60X0
|
/*
|
||||||
#define MPU60X0
|
* 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.
|
||||||
|
*/
|
||||||
|
|
||||||
/* default I2C address */
|
/**
|
||||||
#define MPU60X0_ADDR 0xD0
|
* @file peripherals/mpu60x0.h
|
||||||
#define MPU60X0_ADDR_ALT 0xD2
|
*
|
||||||
|
* MPU-60X0 driver common interface (I2C and SPI).
|
||||||
|
*/
|
||||||
|
|
||||||
#define MPU60X0_SPI_READ 0x80
|
#ifndef MPU60X0_H
|
||||||
|
#define MPU60X0_H
|
||||||
|
|
||||||
// Power and Interface
|
#include "std.h"
|
||||||
#define MPU60X0_REG_AUX_VDDIO 0x01 // Must be set to 0 on MPU6000
|
|
||||||
#define MPU60X0_REG_USER_CTRL 0x6A
|
|
||||||
#define MPU60X0_REG_PWR_MGMT_1 0x6B
|
|
||||||
#define MPU60X0_REG_PWR_MGMT_2 0x6C
|
|
||||||
|
|
||||||
// FIFO
|
/* Include address and register definition */
|
||||||
#define MPU60X0_REG_FIFO_EN 0X23
|
#include "peripherals/mpu60x0_regs.h"
|
||||||
#define MPU60X0_REG_FIFO_COUNT_H 0x72
|
|
||||||
#define MPU60X0_REG_FIFO_COUNT_L 0x73
|
|
||||||
#define MPU60X0_REG_FIFO_R_W 0x74
|
|
||||||
|
|
||||||
// Measurement Settings
|
/// Default sample rate divider
|
||||||
#define MPU60X0_REG_SMPLRT_DIV 0X19
|
#define MPU60X0_DEFAULT_SMPLRT_DIV 0
|
||||||
#define MPU60X0_REG_CONFIG 0X1A
|
/// Default gyro full scale range +- 2000°/s
|
||||||
#define MPU60X0_REG_GYRO_CONFIG 0X1B
|
#define MPU60X0_DEFAULT_FS_SEL MPU60X0_GYRO_RANGE_2000
|
||||||
#define MPU60X0_REG_ACCEL_CONFIG 0X1C
|
/// Default accel full scale range +- 16g
|
||||||
|
#define MPU60X0_DEFAULT_AFS_SEL MPU60X0_ACCEL_RANGE_16G
|
||||||
|
/// Default internal sampling (1kHz, 42Hz LP Bandwidth)
|
||||||
|
#define MPU60X0_DEFAULT_DLPF_CFG MPU60X0_DLPF_42HZ
|
||||||
|
/// Default interrupt config: RAW_RDY_EN
|
||||||
|
#define MPU60X0_DEFAULT_INT_CFG 1
|
||||||
|
/// Default clock: PLL with X gyro reference
|
||||||
|
#define MPU60X0_DEFAULT_CLK_SEL 1
|
||||||
|
|
||||||
// I2C Slave settings
|
enum Mpu60x0ConfStatus {
|
||||||
#define MPU60X0_REG_I2C_MST_CTRL 0X24
|
MPU60X0_CONF_UNINIT,
|
||||||
#define MPU60X0_REG_I2C_MST_STATUS 0X36
|
MPU60X0_CONF_SD,
|
||||||
#define MPU60X0_REG_I2C_MST_DELAY 0X67
|
MPU60X0_CONF_CONFIG,
|
||||||
// Slave 0
|
MPU60X0_CONF_GYRO,
|
||||||
#define MPU60X0_REG_I2C_SLV0_ADDR 0X25 // i2c addr
|
MPU60X0_CONF_ACCEL,
|
||||||
#define MPU60X0_REG_I2C_SLV0_REG 0X26 // slave reg
|
MPU60X0_CONF_INT_PIN,
|
||||||
#define MPU60X0_REG_I2C_SLV0_CTRL 0X27 // set-bits
|
MPU60X0_CONF_INT_ENABLE,
|
||||||
#define MPU60X0_REG_I2C_SLV0_DO 0X63 // DO
|
MPU60X0_CONF_PWR,
|
||||||
// Slave 1
|
MPU60X0_CONF_DONE
|
||||||
#define MPU60X0_REG_I2C_SLV1_ADDR 0X28 // i2c addr
|
};
|
||||||
#define MPU60X0_REG_I2C_SLV1_REG 0X29 // slave reg
|
|
||||||
#define MPU60X0_REG_I2C_SLV1_CTRL 0X2A // set-bits
|
|
||||||
#define MPU60X0_REG_I2C_SLV1_DO 0X64 // DO
|
|
||||||
// Slave 2
|
|
||||||
#define MPU60X0_REG_I2C_SLV2_ADDR 0X2B // i2c addr
|
|
||||||
#define MPU60X0_REG_I2C_SLV2_REG 0X2C // slave reg
|
|
||||||
#define MPU60X0_REG_I2C_SLV2_CTRL 0X2D // set-bits
|
|
||||||
#define MPU60X0_REG_I2C_SLV2_DO 0X65 // DO
|
|
||||||
// Slave 3
|
|
||||||
#define MPU60X0_REG_I2C_SLV3_ADDR 0X2E // i2c addr
|
|
||||||
#define MPU60X0_REG_I2C_SLV3_REG 0X2F // slave reg
|
|
||||||
#define MPU60X0_REG_I2C_SLV3_CTRL 0X30 // set-bits
|
|
||||||
#define MPU60X0_REG_I2C_SLV3_DO 0X66 // DO
|
|
||||||
// Slave 4 - special
|
|
||||||
#define MPU60X0_REG_I2C_SLV4_ADDR 0X31 // i2c addr
|
|
||||||
#define MPU60X0_REG_I2C_SLV4_REG 0X32 // slave reg
|
|
||||||
#define MPU60X0_REG_I2C_SLV4_DO 0X33 // DO
|
|
||||||
#define MPU60X0_REG_I2C_SLV4_CTRL 0X34 // set-bits
|
|
||||||
#define MPU60X0_REG_I2C_SLV4_DI 0X35 // DI
|
|
||||||
|
|
||||||
// Interrupt
|
struct Mpu60x0Config {
|
||||||
#define MPU60X0_REG_INT_PIN_CFG 0X37
|
uint8_t smplrt_div; ///< Sample rate divider
|
||||||
#define MPU60X0_REG_INT_ENABLE 0X38
|
enum Mpu60x0DLPF dlpf_cfg; ///< Digital Low Pass Filter
|
||||||
#define MPU60X0_REG_INT_STATUS 0X3A
|
enum Adxl345GyroRanges gyro_range; ///< deg/s Range
|
||||||
|
enum Adxl345AccelRanges accel_range; ///< g Range
|
||||||
|
bool_t i2c_bypass; ///< bypass mpu i2c
|
||||||
|
bool_t drdy_int_enable; ///< Enable Data Ready Interrupt
|
||||||
|
uint8_t clk_sel; ///< Clock select
|
||||||
|
};
|
||||||
|
|
||||||
// Accelero
|
static inline void mpu60X0_set_default_config(struct Mpu60x0Config *c)
|
||||||
#define MPU60X0_REG_ACCEL_XOUT_H 0X3B
|
{
|
||||||
#define MPU60X0_REG_ACCEL_XOUT_L 0X3C
|
c->smplrt_div = MPU60X0_DEFAULT_SMPLRT_DIV;
|
||||||
#define MPU60X0_REG_ACCEL_YOUT_H 0X3D
|
c->dlpf_cfg = MPU60X0_DEFAULT_DLPF_CFG;
|
||||||
#define MPU60X0_REG_ACCEL_YOUT_L 0X3E
|
c->gyro_range = MPU60X0_DEFAULT_FS_SEL;
|
||||||
#define MPU60X0_REG_ACCEL_ZOUT_H 0X3F
|
c->accel_range = MPU60X0_DEFAULT_AFS_SEL;
|
||||||
#define MPU60X0_REG_ACCEL_ZOUT_L 0X40
|
c->i2c_bypass = TRUE;
|
||||||
|
c->drdy_int_enable = FALSE;
|
||||||
|
c->clk_sel = MPU60X0_DEFAULT_CLK_SEL;
|
||||||
|
}
|
||||||
|
|
||||||
// Temperature
|
#endif // MPU60X0_H
|
||||||
#define MPU60X0_REG_TEMP_OUT_H 0X41
|
|
||||||
#define MPU60X0_REG_TEMP_OUT_L 0X42
|
|
||||||
|
|
||||||
// Gyro
|
|
||||||
#define MPU60X0_REG_GYRO_XOUT_H 0X43
|
|
||||||
#define MPU60X0_REG_GYRO_XOUT_L 0X44
|
|
||||||
#define MPU60X0_REG_GYRO_YOUT_H 0X45
|
|
||||||
#define MPU60X0_REG_GYRO_YOUT_L 0X46
|
|
||||||
#define MPU60X0_REG_GYRO_ZOUT_H 0X47
|
|
||||||
#define MPU60X0_REG_GYRO_ZOUT_L 0X48
|
|
||||||
|
|
||||||
// External Sensor Data
|
|
||||||
#define MPU60X0_EXT_SENS_DATA 0X49
|
|
||||||
#define MPU60X0_EXT_SENS_DATA_SIZE 24
|
|
||||||
|
|
||||||
|
|
||||||
/////////////////////////////////////////////////
|
|
||||||
// MPU60X0 Definitions
|
|
||||||
|
|
||||||
#define MPU60X0_REG_WHO_AM_I 0X75
|
|
||||||
#define MPU60X0_WHOAMI_REPLY 0x68
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#endif /* MPU60X0 */
|
|
||||||
|
|||||||
@@ -0,0 +1,156 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2010-2013 The Paparazzi Team
|
||||||
|
*
|
||||||
|
* This file is part of paparazzi.
|
||||||
|
*
|
||||||
|
* paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, write to
|
||||||
|
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||||
|
* Boston, MA 02111-1307, USA.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file peripherals/mpu60x0_regs.h
|
||||||
|
*
|
||||||
|
* Register and address definitions for MPU-6000 and MPU-6050.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef MPU60X0_REGS_H
|
||||||
|
#define MPU60X0_REGS_H
|
||||||
|
|
||||||
|
/* default I2C address */
|
||||||
|
#define MPU60X0_ADDR 0xD0
|
||||||
|
#define MPU60X0_ADDR_ALT 0xD2
|
||||||
|
|
||||||
|
#define MPU60X0_SPI_READ 0x80
|
||||||
|
|
||||||
|
// Power and Interface
|
||||||
|
#define MPU60X0_REG_AUX_VDDIO 0x01 // Must be set to 0 on MPU6000
|
||||||
|
#define MPU60X0_REG_USER_CTRL 0x6A
|
||||||
|
#define MPU60X0_REG_PWR_MGMT_1 0x6B
|
||||||
|
#define MPU60X0_REG_PWR_MGMT_2 0x6C
|
||||||
|
|
||||||
|
// FIFO
|
||||||
|
#define MPU60X0_REG_FIFO_EN 0X23
|
||||||
|
#define MPU60X0_REG_FIFO_COUNT_H 0x72
|
||||||
|
#define MPU60X0_REG_FIFO_COUNT_L 0x73
|
||||||
|
#define MPU60X0_REG_FIFO_R_W 0x74
|
||||||
|
|
||||||
|
// Measurement Settings
|
||||||
|
#define MPU60X0_REG_SMPLRT_DIV 0X19
|
||||||
|
#define MPU60X0_REG_CONFIG 0X1A
|
||||||
|
#define MPU60X0_REG_GYRO_CONFIG 0X1B
|
||||||
|
#define MPU60X0_REG_ACCEL_CONFIG 0X1C
|
||||||
|
|
||||||
|
// I2C Slave settings
|
||||||
|
#define MPU60X0_REG_I2C_MST_CTRL 0X24
|
||||||
|
#define MPU60X0_REG_I2C_MST_STATUS 0X36
|
||||||
|
#define MPU60X0_REG_I2C_MST_DELAY 0X67
|
||||||
|
// Slave 0
|
||||||
|
#define MPU60X0_REG_I2C_SLV0_ADDR 0X25 // i2c addr
|
||||||
|
#define MPU60X0_REG_I2C_SLV0_REG 0X26 // slave reg
|
||||||
|
#define MPU60X0_REG_I2C_SLV0_CTRL 0X27 // set-bits
|
||||||
|
#define MPU60X0_REG_I2C_SLV0_DO 0X63 // DO
|
||||||
|
// Slave 1
|
||||||
|
#define MPU60X0_REG_I2C_SLV1_ADDR 0X28 // i2c addr
|
||||||
|
#define MPU60X0_REG_I2C_SLV1_REG 0X29 // slave reg
|
||||||
|
#define MPU60X0_REG_I2C_SLV1_CTRL 0X2A // set-bits
|
||||||
|
#define MPU60X0_REG_I2C_SLV1_DO 0X64 // DO
|
||||||
|
// Slave 2
|
||||||
|
#define MPU60X0_REG_I2C_SLV2_ADDR 0X2B // i2c addr
|
||||||
|
#define MPU60X0_REG_I2C_SLV2_REG 0X2C // slave reg
|
||||||
|
#define MPU60X0_REG_I2C_SLV2_CTRL 0X2D // set-bits
|
||||||
|
#define MPU60X0_REG_I2C_SLV2_DO 0X65 // DO
|
||||||
|
// Slave 3
|
||||||
|
#define MPU60X0_REG_I2C_SLV3_ADDR 0X2E // i2c addr
|
||||||
|
#define MPU60X0_REG_I2C_SLV3_REG 0X2F // slave reg
|
||||||
|
#define MPU60X0_REG_I2C_SLV3_CTRL 0X30 // set-bits
|
||||||
|
#define MPU60X0_REG_I2C_SLV3_DO 0X66 // DO
|
||||||
|
// Slave 4 - special
|
||||||
|
#define MPU60X0_REG_I2C_SLV4_ADDR 0X31 // i2c addr
|
||||||
|
#define MPU60X0_REG_I2C_SLV4_REG 0X32 // slave reg
|
||||||
|
#define MPU60X0_REG_I2C_SLV4_DO 0X33 // DO
|
||||||
|
#define MPU60X0_REG_I2C_SLV4_CTRL 0X34 // set-bits
|
||||||
|
#define MPU60X0_REG_I2C_SLV4_DI 0X35 // DI
|
||||||
|
|
||||||
|
// Interrupt
|
||||||
|
#define MPU60X0_REG_INT_PIN_CFG 0X37
|
||||||
|
#define MPU60X0_REG_INT_ENABLE 0X38
|
||||||
|
#define MPU60X0_REG_INT_STATUS 0X3A
|
||||||
|
|
||||||
|
// Accelero
|
||||||
|
#define MPU60X0_REG_ACCEL_XOUT_H 0X3B
|
||||||
|
#define MPU60X0_REG_ACCEL_XOUT_L 0X3C
|
||||||
|
#define MPU60X0_REG_ACCEL_YOUT_H 0X3D
|
||||||
|
#define MPU60X0_REG_ACCEL_YOUT_L 0X3E
|
||||||
|
#define MPU60X0_REG_ACCEL_ZOUT_H 0X3F
|
||||||
|
#define MPU60X0_REG_ACCEL_ZOUT_L 0X40
|
||||||
|
|
||||||
|
// Temperature
|
||||||
|
#define MPU60X0_REG_TEMP_OUT_H 0X41
|
||||||
|
#define MPU60X0_REG_TEMP_OUT_L 0X42
|
||||||
|
|
||||||
|
// Gyro
|
||||||
|
#define MPU60X0_REG_GYRO_XOUT_H 0X43
|
||||||
|
#define MPU60X0_REG_GYRO_XOUT_L 0X44
|
||||||
|
#define MPU60X0_REG_GYRO_YOUT_H 0X45
|
||||||
|
#define MPU60X0_REG_GYRO_YOUT_L 0X46
|
||||||
|
#define MPU60X0_REG_GYRO_ZOUT_H 0X47
|
||||||
|
#define MPU60X0_REG_GYRO_ZOUT_L 0X48
|
||||||
|
|
||||||
|
// External Sensor Data
|
||||||
|
#define MPU60X0_EXT_SENS_DATA 0X49
|
||||||
|
#define MPU60X0_EXT_SENS_DATA_SIZE 24
|
||||||
|
|
||||||
|
|
||||||
|
/////////////////////////////////////////////////
|
||||||
|
// MPU60X0 Definitions
|
||||||
|
|
||||||
|
#define MPU60X0_REG_WHO_AM_I 0X75
|
||||||
|
#define MPU60X0_WHOAMI_REPLY 0x68
|
||||||
|
|
||||||
|
/** Digital Low Pass Filter Options
|
||||||
|
* DLFP is affecting both gyro and accels,
|
||||||
|
* with slightly different bandwidth
|
||||||
|
*/
|
||||||
|
enum Mpu60x0DLPF {
|
||||||
|
MPU60X0_DLPF_256HZ = 0x0, // internal sampling rate 8kHz
|
||||||
|
MPU60X0_DLPF_188HZ = 0x1, // internal sampling rate 1kHz
|
||||||
|
MPU60X0_DLPF_98HZ = 0x2,
|
||||||
|
MPU60X0_DLPF_42HZ = 0x3,
|
||||||
|
MPU60X0_DLPF_20HZ = 0x4,
|
||||||
|
MPU60X0_DLPF_10HZ = 0x5,
|
||||||
|
MPU60X0_DLPF_05HZ = 0x6
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Selectable gyro range
|
||||||
|
*/
|
||||||
|
enum Mpu60x0GyroRanges {
|
||||||
|
MPU60X0_GYRO_RANGE_250 = 0x00,
|
||||||
|
MPU60X0_GYRO_RANGE_500 = 0x01,
|
||||||
|
MPU60X0_GYRO_RANGE_1000 = 0x02,
|
||||||
|
MPU60X0_GYRO_RANGE_2000 = 0x03
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Selectable accel range
|
||||||
|
*/
|
||||||
|
enum Mpu60x0AccelRanges {
|
||||||
|
MPU60X0_ACCEL_RANGE_2G = 0x00,
|
||||||
|
MPU60X0_ACCEL_RANGE_4G = 0x01,
|
||||||
|
MPU60X0_ACCEL_RANGE_8G = 0x02,
|
||||||
|
MPU60X0_ACCEL_RANGE_16G = 0x03
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif /* MPU60X0_REGS_H */
|
||||||
@@ -0,0 +1,172 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2013 Gautier Hattenberger
|
||||||
|
*
|
||||||
|
* This file is part of paparazzi.
|
||||||
|
*
|
||||||
|
* paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, write to
|
||||||
|
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||||
|
* Boston, MA 02111-1307, USA.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file peripherals/mpu60x0_spi.c
|
||||||
|
*
|
||||||
|
* Driver for the MPU-60X0 using SPI.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "peripherals/mpu60x0_spi.h"
|
||||||
|
|
||||||
|
static void trans_cb( struct spi_transaction *trans );
|
||||||
|
|
||||||
|
void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t slave_idx)
|
||||||
|
{
|
||||||
|
/* set spi_peripheral */
|
||||||
|
mpu->spi_p = spi_p;
|
||||||
|
|
||||||
|
/* configure spi transaction */
|
||||||
|
mpu->spi_trans.cpol = SPICpolIdleHigh;
|
||||||
|
mpu->spi_trans.cpha = SPICphaEdge2;
|
||||||
|
mpu->spi_trans.dss = SPIDss8bit;
|
||||||
|
mpu->spi_trans.bitorder = SPIMSBFirst;
|
||||||
|
mpu->spi_trans.cdiv = SPIDiv64;
|
||||||
|
|
||||||
|
mpu->spi_trans.select = SPISelectUnselect;
|
||||||
|
mpu->spi_trans.slave_idx = slave_idx;
|
||||||
|
mpu->spi_trans.output_length = MPU60X0_BUFFER_LEN; //FIXME
|
||||||
|
mpu->spi_trans.input_length = MPU60X0_BUFFER_LEN;
|
||||||
|
mpu->spi_trans.before_cb = NULL;
|
||||||
|
mpu->spi_trans.after_cb = trans_cb;
|
||||||
|
mpu->spi_trans.input_buf = &(mpu->rx_buf[0]);
|
||||||
|
mpu->spi_trans.output_buf = &(mpu->tx_buf[0]);
|
||||||
|
|
||||||
|
/* set inital status: Success or Done */
|
||||||
|
mpu->spi_trans.status = SPITransDone;
|
||||||
|
|
||||||
|
/* set default MPU60X0 config options */
|
||||||
|
mpu60x0_set_default_config(&(mpu->config));
|
||||||
|
|
||||||
|
mpu->initialized = FALSE;
|
||||||
|
mpu->data_available = FALSE;
|
||||||
|
mpu->init_status = MPU60X0_CONF_UNINIT;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
static void mpu60x0_spi_write_to_reg(struct Mpu60x0_Spi *mpu, uint8_t _reg, uint8_t _val) {
|
||||||
|
mpu->spi_trans.output_length = 2;
|
||||||
|
mpu->spi_trans.input_length = 0;
|
||||||
|
mpu->tx_buf[0] = _reg;
|
||||||
|
mpu->tx_buf[1] = _val;
|
||||||
|
spi_submit(mpu->spi_p, &(mpu->spi_trans));
|
||||||
|
}
|
||||||
|
|
||||||
|
// Configuration function called once before normal use
|
||||||
|
static void mpu60x0_spi_send_config(struct Mpu60x0_Spi *mpu)
|
||||||
|
{
|
||||||
|
switch (mpu->init_status) {
|
||||||
|
case MPU60X0_CONF_SD:
|
||||||
|
mpu60x0_spi_write_to_reg(mpu, MPU60X0_REG_SMPLRT_DIV, mpu->config.smplrt_div);
|
||||||
|
mpu->init_status++;
|
||||||
|
break;
|
||||||
|
case MPU60X0_CONF_CONFIG:
|
||||||
|
mpu60x0_spi_write_to_reg(mpu, MPU60X0_REG_CONFIG, mpu->config.dlpf_cfg);
|
||||||
|
mpu->init_status++;
|
||||||
|
break;
|
||||||
|
case MPU60X0_CONF_GYRO:
|
||||||
|
mpu60x0_spi_write_to_reg(mpu, MPU60X0_REG_GYRO_CONFIG, (mpu->config.gyro_range<<3));
|
||||||
|
mpu->init_status++;
|
||||||
|
break;
|
||||||
|
case MPU60X0_CONF_ACCEL:
|
||||||
|
mpu60x0_spi_write_to_reg(mpu, MPU60X0_REG_ACCEL_CONFIG, (mpu->config.accel_range<<3));
|
||||||
|
mpu->init_status++;
|
||||||
|
break;
|
||||||
|
case MPU60X0_CONF_INT_PIN:
|
||||||
|
mpu60x0_spi_write_to_reg(mpu, MPU60X0_REG_INT_PIN_CFG, (mpu->config.i2c_bypass<<1));
|
||||||
|
mpu->init_status++;
|
||||||
|
break;
|
||||||
|
case MPU60X0_CONF_INT_ENABLE:
|
||||||
|
mpu60x0_spi_write_to_reg(mpu, MPU60X0_REG_INT_ENABLE, (mpu->config.drdy_int_enable<<0));
|
||||||
|
mpu->init_status++;
|
||||||
|
break;
|
||||||
|
case MPU60X0_CONF_PWR:
|
||||||
|
mpu60x0_spi_write_to_reg(mpu, MPU60X0_REG_PWR_MGMT_1, ((mpu->config.clk_sel)|(0<<6));
|
||||||
|
mpu->init_status++;
|
||||||
|
break;
|
||||||
|
case MPU60X0_CONF_DONE:
|
||||||
|
mpu->initialized = TRUE;
|
||||||
|
mpu->spi_trans.status = SPITransDone;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void mpu60x0_spi_start_configure(struct Mpu60x0_Spi *mpu)
|
||||||
|
{
|
||||||
|
if (mpu->init_status == MPU60X0_CONF_UNINIT) {
|
||||||
|
mpu->init_status++;
|
||||||
|
if (mpu->spi_trans.status == SPITransSuccess || mpu->spi_trans.status == SPITransDone) {
|
||||||
|
mpu60x0_spi_send_config(mpu);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void mpu60x0_spi_read(struct Mpu60x0_Spi *mpu)
|
||||||
|
{
|
||||||
|
if (mpu->initialized && mpu->spi_trans.status == SPITransDone) {
|
||||||
|
mpu->spi_trans.output_length = 1;
|
||||||
|
mpu->spi_trans.input_length = 15; // FIXME external data
|
||||||
|
/* set read bit and multiple byte bit, then address */
|
||||||
|
mpu->tx_buf[0] = MPU60X0_REG_INT_STATUS + MPU60X0_SPI_READ;
|
||||||
|
spi_submit(mpu->spi_p, &(mpu->spi_trans));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
|
||||||
|
|
||||||
|
void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu)
|
||||||
|
{
|
||||||
|
if (mpu->initialized) {
|
||||||
|
if (mpu->spi_trans.status == SPITransFailed) {
|
||||||
|
mpu->spi_trans.status = SPITransDone;
|
||||||
|
}
|
||||||
|
else if (mpu->spi_trans.status == SPITransSuccess) {
|
||||||
|
// Successfull reading
|
||||||
|
if (bit_is_set(mpu->rx_buf,0)) {
|
||||||
|
// new data
|
||||||
|
mpu->data_accel.vect.x = Int16FromBuf(mpu->rx_buf,1);
|
||||||
|
mpu->data_accel.vect.y = Int16FromBuf(mpu->rx_buf,3);
|
||||||
|
mpu->data_accel.vect.z = Int16FromBuf(mpu->rx_buf,5);
|
||||||
|
mpu->data_rates.rates.p = Int16FromBuf(mpu->rx_buf,9);
|
||||||
|
mpu->data_rates.rates.q = Int16FromBuf(mpu->rx_buf,11);
|
||||||
|
mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf,13);
|
||||||
|
mpu->data_available = TRUE;
|
||||||
|
}
|
||||||
|
mpu->spi_trans.status = SPITransDone;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (mpu->init_status != MPU60X0_CONF_UNINIT) { // Configuring but not yet initialized
|
||||||
|
switch (mpu->spi_trans.status) {
|
||||||
|
case SPITransFailed:
|
||||||
|
mpu->init_status--; // Retry config (TODO max retry)
|
||||||
|
case SPITransSuccess:
|
||||||
|
case SPITransDone:
|
||||||
|
mpu->spi_trans.status = SPITransDone;
|
||||||
|
mpu60x0_spi_send_config(mpu);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,74 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) 2013 Gautier Hattenberger
|
||||||
|
*
|
||||||
|
* This file is part of paparazzi.
|
||||||
|
*
|
||||||
|
* paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, write to
|
||||||
|
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||||
|
* Boston, MA 02111-1307, USA.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file peripherals/mpu60x0_spi.h
|
||||||
|
*
|
||||||
|
* Driver for the MPU-60X0 using SPI.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef MPU60X0_SPI_H
|
||||||
|
#define MPU60X0_SPI_H
|
||||||
|
|
||||||
|
#include "std.h"
|
||||||
|
#include "math/pprz_algebra_int.h"
|
||||||
|
#include "mcu_periph/spi.h"
|
||||||
|
|
||||||
|
/* Include common MPU60X0 options and definitions */
|
||||||
|
#include "peripherals/mpu60x0.h"
|
||||||
|
|
||||||
|
|
||||||
|
#define MPU60X0_BUFFER_LEN 32
|
||||||
|
|
||||||
|
struct Mpu60x0_Spi {
|
||||||
|
struct spi_periph *spi_p;
|
||||||
|
struct spi_transaction spi_trans;
|
||||||
|
volatile uint8_t tx_buf[MPU60X0_BUFFER_LEN]; // FIXME correct length
|
||||||
|
volatile uint8_t rx_buf[MPU60X0_BUFFER_LEN]; // FIXME idem
|
||||||
|
enum Mpu60x0ConfStatus init_status; ///< init status
|
||||||
|
bool_t initialized; ///< config done flag
|
||||||
|
volatile bool_t data_available; ///< data ready flag
|
||||||
|
union {
|
||||||
|
struct Int16Vect3 vect; ///< accel data vector in accel coordinate system
|
||||||
|
int16_t value[3]; ///< accel data values accessible by channel index
|
||||||
|
} data_accel;
|
||||||
|
union {
|
||||||
|
struct Int16Rates rates; ///< rates data as angular rates in gyro coordinate system
|
||||||
|
int16_t value[3]; ///< rates data values accessible by channel index
|
||||||
|
} data_rates;
|
||||||
|
struct Mpu60x0Config config;
|
||||||
|
};
|
||||||
|
|
||||||
|
// Functions
|
||||||
|
extern void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t addr);
|
||||||
|
extern void mpu60x0_spi_start_configure(struct Mpu60x0_Spi *mpu);
|
||||||
|
extern void mpu60x0_spi_read(struct Mpu60x0_Spi *mpu);
|
||||||
|
extern void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu);
|
||||||
|
|
||||||
|
/// convenience function: read or start configuration if not already initialized
|
||||||
|
static inline void mpu60x0_spi_periodic(struct Mpu60x0_Spi *mpu) {
|
||||||
|
if (mpu->initialized)
|
||||||
|
mpu60x0_spi_read(mpu);
|
||||||
|
else
|
||||||
|
mpu60x0_spi_start_configure(mpu);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // MPU60X0_SPI_H
|
||||||
Reference in New Issue
Block a user