Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 16:30:07 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[peripheral] update mpu9250 driver, imu and module as well

Browse Source
This commit is contained in:
Gautier Hattenberger
2014-11-17 21:43:05 +01:00
parent 0d3e40a8a2
commit 18cbec7f15
10 changed files with 106 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/shared/imu_mpu9250.makefile → conf/firmwares/subsystems/shared/imu_mpu9250_i2c.makefile
+3 -3
View File
@@ -8,14 +8,14 @@ ifeq ($(TARGET), ap)
IMU_MPU9250_CFLAGS = -DUSE_IMU
endif
IMU_MPU9250_CFLAGS += -DIMU_TYPE_H=\"imu/imu_mpu9250.h\"
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.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
IMU_MPU9250_SRCS += peripherals/ak8963.c
# set default i2c bus
conf/modules/imu_mpu9250.xml
+1
View File
@@ -20,6 +20,7 @@
<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>
sw/airborne/modules/sensors/imu_mpu9250.c
+7
View File
@@ -67,5 +67,12 @@ void imu_mpu9250_report(void)
(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/peripherals/mpu9250.c
+1 -1
View File
@@ -81,7 +81,7 @@ void mpu9250_send_config(Mpu9250ConfigSet mpu_set, void* mpu, struct Mpu9250Conf
break;
case MPU9250_CONF_DLPF_ACCEL:
/* configure digital low pass filter fir accelerometer */
mpu_set(mpu, MPU9250_REG_CONFIG, config->dlpf_accel_cfg);
mpu_set(mpu, MPU9250_REG_ACCEL_CONFIG_2, config->dlpf_accel_cfg);
config->init_status++;
break;
case MPU9250_CONF_GYRO:
sw/airborne/peripherals/mpu9250.h
+2 -2
View File
@@ -35,9 +35,9 @@
/// 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_2000
#define MPU9250_DEFAULT_FS_SEL MPU9250_GYRO_RANGE_1000
/// Default accel full scale range +- 16g
#define MPU9250_DEFAULT_AFS_SEL MPU9250_ACCEL_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)
sw/airborne/peripherals/mpu9250_i2c.c
+26 -11
View File
@@ -27,6 +27,8 @@
#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 */
@@ -44,13 +46,16 @@ void mpu9250_i2c_init(struct Mpu9250_I2c *mpu, struct i2c_periph *i2c_p, uint8_t
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;
mpu->config.nb_slaves = 1;
/* set callback function to configure mag */
//mpu->config.slaves[0].configure = &imu_mpu9250_configure_mag_slave;
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;
}
@@ -80,6 +85,12 @@ void mpu9250_i2c_read(struct Mpu9250_I2c *mpu)
/* 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
}
}
@@ -125,19 +136,23 @@ void mpu9250_i2c_event(struct Mpu9250_I2c *mpu)
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(Mpu60x0ConfigSet mpu_set __attribute__ ((unused)), void* mpu __attribute__ ((unused)))
//{
// ak8963_start_configure(&imu_mpu9250.akm);
// if (imu_mpu9250.akm.initialized)
// return TRUE;
// else
// return FALSE;
//}
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)
sw/airborne/peripherals/mpu9250_i2c.h
+2
View File
@@ -34,6 +34,7 @@
/* Include common MPU9250 options and definitions */
#include "peripherals/mpu9250.h"
#include "peripherals/ak8963.h"
#define MPU9250_BUFFER_EXT_LEN 16
@@ -66,6 +67,7 @@ struct Mpu9250_I2c {
uint8_t data_ext[MPU9250_BUFFER_EXT_LEN];
struct Mpu9250Config config;
enum Mpu9250I2cSlaveInitStatus slave_init_status;
struct Ak8963 akm; ///< "internal" magnetometer
};
// Functions
sw/airborne/peripherals/mpu9250_regs.h
+34 -32
View File
@@ -32,6 +32,8 @@
#define MPU9250_ADDR 0xD0
#define MPU9250_ADDR_ALT 0xD2
#define MPU9250_MAG_ADDR 0x18
#define MPU9250_SPI_READ 0x80
// Power and Interface
@@ -43,15 +45,15 @@
// 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
#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
#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
@@ -59,31 +61,31 @@
#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
#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
#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
#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
#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
#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
@@ -116,7 +118,7 @@
#define MPU9250_REG_WHO_AM_I 0x75
#define MPU9250_WHOAMI_REPLY 0x68
#define MPU9250_WHOAMI_REPLY 0x68
// Bit positions
#define MPU9250_I2C_BYPASS_EN 1
@@ -126,7 +128,7 @@
#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_I2C_MST_EN 5
#define MPU9250_FIFO_EN 6
// in MPU9250_REG_I2C_MST_STATUS
@@ -135,7 +137,7 @@
/** Digital Low Pass Filter Options
*/
enum Mpu9250DLPFGyro {
MPU9250_DLPF__GYRO_250HZ = 0x0, // internal sampling rate 8kHz
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,
@@ -144,7 +146,6 @@ enum Mpu9250DLPFGyro {
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
@@ -154,6 +155,7 @@ enum Mpu9250DLPFAccel {
MPU9250_DLPF_ACCEL_10HZ = 0x5,
MPU9250_DLPF_ACCEL_05HZ = 0x6
};
/**
* Selectable gyro range
*/
sw/airborne/subsystems/imu/imu_mpu9250.c → sw/airborne/subsystems/imu/imu_mpu9250_i2c.c
+26 -14
View File
@@ -19,7 +19,7 @@
*/
/**
* @file subsystems/imu/imu_mpu9250.c
* @file subsystems/imu/imu_mpu9250_i2c.c
*
* IMU driver for the MPU9250 using I2C
*
@@ -31,21 +31,29 @@
#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 44Hz, Delay 4.9ms
* Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz
/* 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 260Hz, Delay 0ms
* Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz
/* 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_41HZ
#define IMU_MPU9250_ACCEL_LOWPASS_FILTER MPU9250_DLPF_ACCEL_41HZ
#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)
@@ -122,20 +130,24 @@ void imu_mpu9250_event(void)
(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]),
(int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z])
};
//struct Int32Vect3 mag = {
// (int32_t)(imu_mpu9250.mpu.data_mag.value[IMU_MPU9250_CHAN_X]),
// (int32_t)(imu_mpu9250.mpu.data_mag.value[IMU_MPU9250_CHAN_Y]),
// (int32_t)(imu_mpu9250.mpu.data_mag.value[IMU_MPU9250_CHAN_Z])
//};
// unscaled vector
VECT3_COPY(imu.accel_unscaled, accel);
RATES_COPY(imu.gyro_unscaled, rates);
//VECT3_COPY(imu.mag_unscaled, mag);
imu_mpu9250.mpu.data_available = FALSE;
imu_mpu9250.gyro_valid = TRUE;
imu_mpu9250.accel_valid = TRUE;
//imu_mpu9250.mag_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.h → sw/airborne/subsystems/imu/imu_mpu9250_i2c.h
+4 -4
View File
@@ -19,14 +19,14 @@
*/
/**
* @file subsystems/imu/imu_mpu9250.h
* @file subsystems/imu/imu_mpu9250_i2c.h
*
* IMU driver for the MPU9250 using I2C
*
*/
#ifndef IMU_MPU9250_H
#define IMU_MPU9250_H
#ifndef IMU_MPU9250_I2C_H
#define IMU_MPU9250_I2C_H
#include "std.h"
#include "generated/airframe.h"
@@ -98,4 +98,4 @@ static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler
}
}
#endif /* IMU_MPU9250_H */
#endif /* IMU_MPU9250_I2C_H */