yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

MPU9250 I2C mode

Browse Source 
Fixes for other boards

Functioning sensors
This commit is contained in:
Dennis Shtatnov
2016-08-20 21:04:42 -04:00
committed by Lorenz Meier
parent 9c8e56401b
commit 5100785f51
15 changed files with 784 additions and 322 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
ROMFS/px4fmu_common/init.d/4900_crazyflie
View File

@@ -10,5 +10,12 @@
sh /etc/init.d/4001_quad_x
if [ $AUTOCNF == yes ]
then
param set COM_RC_IN_MODE 2
param set BAT_N_CELLS 1
param set BAT_CAPACITY 240
param set BAT_SOURCE 1
fi
set OUTPUT_MODE crazyflie
set USE_IO no

5
ROMFS/px4fmu_common/init.d/rc.interface
View File

@@ -109,6 +109,11 @@ then
set MIXER_AUX none
fi
if ver hwcmp CRAZYFLIE
then
set MIXER_AUX none
fi
if [ $MIXER_AUX != none -a $AUX_MODE != none ]
then
#

21
ROMFS/px4fmu_common/init.d/rc.sensors
View File

@@ -8,12 +8,20 @@ then
if ms5611 start
then
fi
else
if ver hwcmp CRAZYFLIE
then
# Crazyflie uses separate driver
else
# Configure all I2C buses to 100 KHz as they
# are all external or slow
fmu i2c 1 100000
fmu i2c 2 100000
fi
if ver hwcmp PX4FMU_V4
then
# We know there are sketchy boards out there
@@ -198,6 +206,19 @@ then
fi
fi
if ver hwcmp CRAZYFLIE
then
# Onboard I2C
if mpu9250 -R 12 start
then
fi
# I2C bypass of mpu
if lps25h start
then
fi
fi
if meas_airspeed start
then
else

15
ROMFS/px4fmu_common/init.d/rcS
View File

@@ -164,10 +164,16 @@ then
then
set USE_IO no
fi
if ver hwcmp CRAZYFLIE
then
set USE_IO no
fi
else
set USE_IO no
fi
# should set to 0.8 for mindpx-v2 borad.
if param compare INAV_LIDAR_ERR 0.5
then
@@ -675,6 +681,15 @@ then
ardrone_interface start -d /dev/ttyS1
fi
#
# Start Crazyflie driver
#
if ver hwcmp CRAZYFLIE
then
crazyflie start
fi
#
# Fixed wing setup
#

2
cmake/configs/nuttx_crazyflie_default.cmake
View File

@@ -11,7 +11,7 @@ set(config_module_list
drivers/led
drivers/boards/crazyflie
drivers/crazyflie
#drivers/mpu9250
drivers/mpu9250
#drivers/ak8963
drivers/lps25h
drivers/gps

1
src/drivers/mpu9250/CMakeLists.txt
View File

@@ -44,6 +44,7 @@ px4_add_module(
main.cpp
gyro.cpp
mag.cpp
mag_i2c.cpp
DEPENDS
platforms__common
)

2
src/drivers/mpu9250/gyro.h
View File

@@ -31,6 +31,8 @@
*
****************************************************************************/
#pragma once
class MPU9250;
/**

85
src/drivers/mpu9250/mag.cpp
View File

@@ -149,8 +149,11 @@
#define AK8963_RESET 0x01
MPU9250_mag::MPU9250_mag(MPU9250 *parent, const char *path) :
// If interface is non-null, then it will used for interacting with the device.
// Otherwise, it will passthrough the parent MPU9250
MPU9250_mag::MPU9250_mag(MPU9250 *parent, device::Device *interface, const char *path) :
CDev("MPU9250_mag", path),
_interface(interface),
_parent(parent),
_mag_topic(nullptr),
_mag_orb_class_instance(-1),
@@ -249,7 +252,16 @@ bool MPU9250_mag::check_duplicate(uint8_t *mag_data)
}
void
MPU9250_mag::measure(struct ak8963_regs data)
MPU9250_mag::measure()
{
struct ak8963_regs data;
if(OK == _interface->read(AK8963REG_ST1, &data, sizeof(struct ak8963_regs))){
_measure(data);
}
}
void
MPU9250_mag::_measure(struct ak8963_regs data)
{
bool mag_notify = true;
@@ -508,12 +520,7 @@ MPU9250_mag::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out)
void
MPU9250_mag::read_block(uint8_t reg, uint8_t *val, uint8_t count)
{
uint8_t addr = reg | 0x80;
uint8_t tx[32] = { addr, };
uint8_t rx[32];
_parent->transfer(tx, rx, count + 1);
memcpy(val, rx + 1, count);
_parent->_interface->read(reg, val, count);
}
void
@@ -525,12 +532,26 @@ MPU9250_mag::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size)
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new reads
}
uint8_t
MPU9250_mag::read_reg(unsigned int reg)
{
uint8_t buf;
if (_interface == nullptr) {
passthrough_read(reg, &buf, 0x01);
}
else {
_interface->read(reg, &buf, 1);
}
return buf;
}
bool
MPU9250_mag::ak8963_check_id(void)
{
for (int i = 0; i < 5; i++) {
uint8_t deviceid = 0;
passthrough_read(AK8963REG_WIA, &deviceid, 0x01);
uint8_t deviceid = read_reg(AK8963REG_WIA);
if (AK8963_DEVICE_ID == deviceid) {
return true;
@@ -551,10 +572,27 @@ MPU9250_mag::passthrough_write(uint8_t reg, uint8_t val)
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new writes
}
void
MPU9250_mag::write_reg(unsigned reg, uint8_t value)
{
// general register transfer at low clock speed
if (_interface == nullptr) {
passthrough_write(reg, value);
}
else {
_interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
}
}
void
MPU9250_mag::ak8963_reset(void)
{
passthrough_write(AK8963REG_CNTL2, AK8963_RESET);
write_reg(AK8963REG_CNTL2, AK8963_RESET);
}
bool
@@ -563,10 +601,15 @@ MPU9250_mag::ak8963_read_adjustments(void)
uint8_t response[3];
float ak8963_ASA[3];
passthrough_write(AK8963REG_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC);
write_reg(AK8963REG_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC);
usleep(50);
if (_interface != nullptr) {
_interface->read(AK8963REG_ASAX, response, 3);
}
else {
passthrough_read(AK8963REG_ASAX, response, 3);
passthrough_write(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE);
}
write_reg(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE);
for (int i = 0; i < 3; i++) {
if (0 != response[i] && 0xff != response[i]) {
@@ -589,10 +632,18 @@ MPU9250_mag::ak8963_setup(void)
{
int retries = 10;
// enable the I2C master to slaves on the aux bus
/* Configures the parent to act in master mode */
if(_interface == nullptr) {
uint8_t user_ctrl = _parent->read_reg(MPUREG_USER_CTRL);
_parent->write_checked_reg(MPUREG_USER_CTRL, user_ctrl | BIT_I2C_MST_EN);
_parent->write_reg(MPUREG_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | BIT_I2C_MST_WAIT_FOR_ES | BITS_I2C_MST_CLOCK_400HZ);
}
else {
// uint8_t user_ctrl = _parent->read_reg(MPUREG_USER_CTRL);
// // Passthrough mode
// _parent->write_checked_reg(MPUREG_USER_CTRL, user_ctrl & (~BIT_I2C_MST_EN));
}
if (!ak8963_check_id()) {
::printf("AK8963: bad id\n");
@@ -605,9 +656,13 @@ MPU9250_mag::ak8963_setup(void)
}
}
passthrough_write(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC);
write_reg(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC);
if(_interface == NULL) {
/* Configure mpu to internally read ak8963 data */
set_passthrough(AK8963REG_ST1, sizeof(struct ak8963_regs));
}
return true;
}

26
src/drivers/mpu9250/mag.h
View File

@@ -31,6 +31,8 @@
*
****************************************************************************/
#pragma once
class MPU9250;
#pragma pack(push, 1)
@@ -43,13 +45,18 @@ struct ak8963_regs {
};
#pragma pack(pop)
extern device::Device *AK8963_I2C_interface(int bus, bool external_bus);
typedef device::Device *(*MPU9250_mag_constructor)(int, bool);
/**
* Helper class implementing the magnetometer driver node.
*/
class MPU9250_mag : public device::CDev
{
public:
MPU9250_mag(MPU9250 *parent, const char *path);
MPU9250_mag(MPU9250 *parent, device::Device *interface, const char *path);
~MPU9250_mag();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
@@ -67,9 +74,24 @@ public:
bool ak8963_read_adjustments(void);
protected:
Device *_interface;
friend class MPU9250;
void measure(struct ak8963_regs data);
/* Directly measure from the _interface if possible */
void measure();
/* Update the state with prefetched data (internally called by the regular measure() )*/
void _measure(struct ak8963_regs data);
uint8_t read_reg(unsigned reg);
void write_reg(unsigned reg, uint8_t value);
bool is_passthrough() { return _interface == nullptr; }
int self_test(void);
private:

179
src/drivers/mpu9250/mag_i2c.cpp Normal file
View File

@@ -0,0 +1,179 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mag_i2c.cpp
*
* I2C interface for AK8963
*/
/* XXX trim includes */
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <assert.h>
#include <debug.h>
#include <errno.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
#include "MPU9250.h"
#include "mag.h"
#include "board_config.h"
// TODO: This is redundant with mag.cpp
#define AK8963_I2C_ADDR 0x0C
#define ADDR_ID 0x00
#define ID_WHO_AM_I 0x48
#ifdef USE_I2C
device::Device *AK8963_I2C_interface(int bus, bool external_bus);
class AK8963_I2C : public device::I2C
{
public:
AK8963_I2C(int bus);
virtual ~AK8963_I2C();
virtual int init();
virtual int read(unsigned address, void *data, unsigned count);
virtual int write(unsigned address, void *data, unsigned count);
virtual int ioctl(unsigned operation, unsigned &arg);
protected:
virtual int probe();
};
device::Device *
AK8963_I2C_interface(int bus, bool external_bus)
{
return new AK8963_I2C(bus);
}
AK8963_I2C::AK8963_I2C(int bus) :
I2C("AK8963_I2C", nullptr, bus, AK8963_I2C_ADDR, 400000)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250;
}
AK8963_I2C::~AK8963_I2C()
{
}
int
AK8963_I2C::init()
{
/* this will call probe() */
return I2C::init();
}
int
AK8963_I2C::ioctl(unsigned operation, unsigned &arg)
{
int ret;
switch (operation) {
case ACCELIOCGEXTERNAL:
return _bus == PX4_I2C_BUS_EXPANSION ? 1 : 0;
case DEVIOCGDEVICEID:
return CDev::ioctl(nullptr, operation, arg);
case MPUIOCGIS_I2C:
return 1;
default:
ret = -EINVAL;
}
return ret;
}
int
AK8963_I2C::write(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE];
if (sizeof(cmd) < (count + 1)) {
return -EIO;
}
cmd[0] = MPU9250_REG(reg_speed);
cmd[1] = *(uint8_t *)data;
return transfer(&cmd[0], count + 1, nullptr, 0);
}
int
AK8963_I2C::read(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd = MPU9250_REG(reg_speed);
return transfer(&cmd, 1, (uint8_t *)data, count);
}
int
AK8963_I2C::probe()
{
uint8_t whoami = 0;
uint8_t expected = ID_WHO_AM_I;
if (read(ADDR_ID, &whoami, 1)) {
return -EIO;
}
if (whoami != expected) {
return -EIO;
}
return OK;
}
#endif

67
src/drivers/mpu9250/main.cpp
View File

@@ -34,7 +34,7 @@
/**
* @file main.cpp
*
* Driver for the Invensense mpu9250 connected via SPI.
* Driver for the Invensense mpu9250 connected via I2C or SPI.
*
* @authors Andrew Tridgell
* Robert Dickenson
@@ -83,11 +83,6 @@
/** driver 'main' command */
extern "C" { __EXPORT int mpu9250_main(int argc, char *argv[]); }
/**
* Local functions in support of the shell command.
*/
namespace mpu9250
{
enum MPU9250_BUS {
MPU9250_BUS_ALL = 0,
@@ -97,6 +92,13 @@ enum MPU9250_BUS {
MPU9250_BUS_SPI_EXTERNAL
};
/**
* Local functions in support of the shell command.
*/
namespace mpu9250
{
/*
list of supported bus configurations
*/
@@ -107,30 +109,31 @@ struct mpu9250_bus_option {
const char *gyropath;
const char *magpath;
MPU9250_constructor interface_constructor;
bool magpassthrough;
uint8_t busnum;
MPU6000 *dev;
MPU9250 *dev;
} bus_options[] = {
#if defined (USE_I2C)
# if defined(PX4_I2C_BUS_ONBOARD)
{ MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
{ MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_I2C_interface, false, PX4_I2C_BUS_ONBOARD, NULL },
# endif
# if defined(PX4_I2C_BUS_EXPANSION)
{ MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
{ MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, NULL },
# endif
#endif
#ifdef PX4_SPIDEV_MPU
{ MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
{ MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, NULL },
#endif
#if defined(PX4_SPI_BUS_EXT)
{ MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT &MPU9250_SPI_interface, PX4_SPI_BUS_EXT, NULL },
{ MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT &MPU9250_SPI_interface, true, PX4_SPI_BUS_EXT, NULL },
#endif
};
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
void start(enum MPU9250_BUS busid, enum Rotation rotation);
bool start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation);
void start(enum MPU9250_BUS busid, enum Rotation rotation, bool external_bus);
bool start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external_bus);
struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid);
void stop(enum MPU9250_BUS busid);
void test(enum MPU9250_BUS busid);
@@ -159,7 +162,7 @@ struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid)
* start driver for a specific bus option
*/
bool
start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, int range, int device_type, bool external)
start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external)
{
int fd = -1;
@@ -168,7 +171,7 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, int range, int
return false;
}
device::Device *interface = bus.interface_constructor(bus.busnum, device_type, external);
device::Device *interface = bus.interface_constructor(bus.busnum, external);
if (interface == nullptr) {
warnx("no device on bus %u", (unsigned)bus.busid);
@@ -181,7 +184,17 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, int range, int
return false;
}
bus.dev = new MPU9250(interface, bus.accelpath, bus.gyropath, bus.magpath, rotation, device_type);
device::Device *mag_interface = nullptr;
#ifdef USE_I2C
/* For i2c interfaces, connect to the magnetomer directly */
bool is_i2c = bus.busid == MPU9250_BUS_I2C_INTERNAL || bus.busid == MPU9250_BUS_I2C_EXTERNAL;
if (is_i2c) {
mag_interface = AK8963_I2C_interface(bus.busnum, external);
}
#endif
bus.dev = new MPU9250(interface, mag_interface, bus.accelpath, bus.gyropath, bus.magpath, rotation);
if (bus.dev == nullptr) {
delete interface;
@@ -203,9 +216,6 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, int range, int
goto fail;
}
if (ioctl(fd, ACCELIOCSRANGE, range) < 0) {
goto fail;
}
close(fd);
@@ -232,7 +242,7 @@ fail:
* or failed to detect the sensor.
*/
void
start(enum MPU9250_BUS busid, enum Rotation rotation, int range, int device_type, bool external)
start(enum MPU9250_BUS busid, enum Rotation rotation, bool external)
{
bool started = false;
@@ -248,7 +258,7 @@ start(enum MPU9250_BUS busid, enum Rotation rotation, int range, int device_type
continue;
}
started |= start_bus(bus_options[i], rotation, range, device_type, external);
started |= start_bus(bus_options[i], rotation, external);
}
exit(started ? 0 : 1);
@@ -473,7 +483,6 @@ usage()
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
warnx(" -a accel range (in g)");
}
} // namespace
@@ -485,13 +494,13 @@ mpu9250_main(int argc, char *argv[])
int ch;
bool external = false;
enum Rotation rotation = ROTATION_NONE;
int accel_range = 8;
//int accel_range = 8;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc, argv, "XISsR:a:")) != EOF) {
while ((ch = getopt(argc, argv, "XISsR:")) != EOF) {
switch (ch) {
case 'X':
busid = MPU9260_BUS_I2C_EXTERNAL;
busid = MPU9250_BUS_I2C_EXTERNAL;
break;
case 'I':
@@ -503,17 +512,13 @@ mpu9250_main(int argc, char *argv[])
break;
case 's':
busid = MPU9260_BUS_SPI_INTERNAL;
busid = MPU9250_BUS_SPI_INTERNAL;
break;
case 'R':
rotation = (enum Rotation)atoi(optarg);
break;
case 'a':
accel_range = atoi(optarg);
break;
default:
mpu9250::usage();
exit(0);
@@ -529,7 +534,7 @@ mpu9250_main(int argc, char *argv[])
*/
if (!strcmp(verb, "start")) {
mpu9250::start(busid, rotation, accel_range, device_type, external);
mpu9250::start(busid, rotation, external);
}
if (!strcmp(verb, "stop")) {

273
src/drivers/mpu9250/mpu9250.cpp
View File

@@ -61,6 +61,7 @@
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/conversions.h>
#include <systemlib/px4_macros.h>
#include <nuttx/arch.h>
#include <nuttx/clock.h>
@@ -81,113 +82,6 @@
#include "gyro.h"
#include "mpu9250.h"
#define DIR_READ 0x80
#define DIR_WRITE 0x00
// MPU 9250 registers
#define MPUREG_WHOAMI 0x75
#define MPUREG_SMPLRT_DIV 0x19
#define MPUREG_CONFIG 0x1A
#define MPUREG_GYRO_CONFIG 0x1B
#define MPUREG_ACCEL_CONFIG 0x1C
#define MPUREG_ACCEL_CONFIG2 0x1D
#define MPUREG_LPACCEL_ODR 0x1E
#define MPUREG_WOM_THRESH 0x1F
#define MPUREG_FIFO_EN 0x23
#define MPUREG_I2C_MST_CTRL 0x24
#define MPUREG_I2C_SLV0_ADDR 0x25
#define MPUREG_I2C_SLV0_REG 0x26
#define MPUREG_I2C_SLV0_CTRL 0x27
#define MPUREG_I2C_SLV1_ADDR 0x28
#define MPUREG_I2C_SLV1_REG 0x29
#define MPUREG_I2C_SLV1_CTRL 0x2A
#define MPUREG_I2C_SLV2_ADDR 0x2B
#define MPUREG_I2C_SLV2_REG 0x2C
#define MPUREG_I2C_SLV2_CTRL 0x2D
#define MPUREG_I2C_SLV3_ADDR 0x2E
#define MPUREG_I2C_SLV3_REG 0x2F
#define MPUREG_I2C_SLV3_CTRL 0x30
#define MPUREG_I2C_SLV4_ADDR 0x31
#define MPUREG_I2C_SLV4_REG 0x32
#define MPUREG_I2C_SLV4_DO 0x33
#define MPUREG_I2C_SLV4_CTRL 0x34
#define MPUREG_I2C_SLV4_DI 0x35
#define MPUREG_I2C_MST_STATUS 0x36
#define MPUREG_INT_PIN_CFG 0x37
#define MPUREG_INT_ENABLE 0x38
#define MPUREG_INT_STATUS 0x3A
#define MPUREG_ACCEL_XOUT_H 0x3B
#define MPUREG_ACCEL_XOUT_L 0x3C
#define MPUREG_ACCEL_YOUT_H 0x3D
#define MPUREG_ACCEL_YOUT_L 0x3E
#define MPUREG_ACCEL_ZOUT_H 0x3F
#define MPUREG_ACCEL_ZOUT_L 0x40
#define MPUREG_TEMP_OUT_H 0x41
#define MPUREG_TEMP_OUT_L 0x42
#define MPUREG_GYRO_XOUT_H 0x43
#define MPUREG_GYRO_XOUT_L 0x44
#define MPUREG_GYRO_YOUT_H 0x45
#define MPUREG_GYRO_YOUT_L 0x46
#define MPUREG_GYRO_ZOUT_H 0x47
#define MPUREG_GYRO_ZOUT_L 0x48
#define MPUREG_EXT_SENS_DATA_00 0x49
#define MPUREG_I2C_SLV0_D0 0x63
#define MPUREG_I2C_SLV1_D0 0x64
#define MPUREG_I2C_SLV2_D0 0x65
#define MPUREG_I2C_SLV3_D0 0x66
#define MPUREG_I2C_MST_DELAY_CTRL 0x67
#define MPUREG_SIGNAL_PATH_RESET 0x68
#define MPUREG_MOT_DETECT_CTRL 0x69
#define MPUREG_USER_CTRL 0x6A
#define MPUREG_PWR_MGMT_1 0x6B
#define MPUREG_PWR_MGMT_2 0x6C
#define MPUREG_FIFO_COUNTH 0x72
#define MPUREG_FIFO_COUNTL 0x73
#define MPUREG_FIFO_R_W 0x74
// Configuration bits MPU 9250
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define MPU_CLK_SEL_AUTO 0x01
#define BITS_GYRO_ST_X 0x80
#define BITS_GYRO_ST_Y 0x40
#define BITS_GYRO_ST_Z 0x20
#define BITS_FS_250DPS 0x00
#define BITS_FS_500DPS 0x08
#define BITS_FS_1000DPS 0x10
#define BITS_FS_2000DPS 0x18
#define BITS_FS_MASK 0x18
#define BITS_DLPF_CFG_250HZ 0x00
#define BITS_DLPF_CFG_184HZ 0x01
#define BITS_DLPF_CFG_92HZ 0x02
#define BITS_DLPF_CFG_41HZ 0x03
#define BITS_DLPF_CFG_20HZ 0x04
#define BITS_DLPF_CFG_10HZ 0x05
#define BITS_DLPF_CFG_5HZ 0x06
#define BITS_DLPF_CFG_3600HZ 0x07
#define BITS_DLPF_CFG_MASK 0x07
#define BITS_ACCEL_CONFIG2_41HZ 0x03
#define BIT_RAW_RDY_EN 0x01
#define BIT_INT_ANYRD_2CLEAR 0x10
#define MPU_WHOAMI_9250 0x71
#define MPU9250_ACCEL_DEFAULT_RATE 1000
#define MPU9250_ACCEL_MAX_OUTPUT_RATE 280
#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
#define MPU9250_GYRO_DEFAULT_RATE 1000
/* rates need to be the same between accel and gyro */
#define MPU9250_GYRO_MAX_OUTPUT_RATE MPU9250_ACCEL_MAX_OUTPUT_RATE
#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 41
#define MPU9250_ONE_G 9.80665f
/*
we set the timer interrupt to run a bit faster than the desired
sample rate and then throw away duplicates by comparing
@@ -215,15 +109,19 @@ const uint8_t MPU9250::_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = { MPU
};
MPU9250::MPU9250(device::Device *interface, const char *path_accel, const char *path_gyro, const char *path_mag,
MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, const char *path_mag,
enum Rotation rotation) :
CDev("MPU9250", path_accel),
_interface(interface),
_interface_bus(interface_bus),
//SPI("MPU9250", path_accel, bus, device, SPIDEV_MODE3, MPU9250_LOW_BUS_SPEED),
_gyro(new MPU9250_gyro(this, path_gyro)),
_mag(new MPU9250_mag(this, path_mag)),
_mag(new MPU9250_mag(this, mag_interface, path_mag)),
_whoami(0),
#if defined(USE_I2C)
_work {},
_use_hrt(false),
#else
_use_hrt(true),
#endif
_call{},
_call_interval(0),
_accel_reports(nullptr),
@@ -277,6 +175,9 @@ MPU9250::MPU9250(device::Device *interface, const char *path_accel, const char *
_mag->_device_id.devid = _device_id.devid;
_mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250;
/* For an independent mag, ensure that it is connected to the i2c bus */
// default accel scale factors
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
@@ -294,6 +195,7 @@ MPU9250::MPU9250(device::Device *interface, const char *path_accel, const char *
_gyro_scale.z_scale = 1.0f;
memset(&_call, 0, sizeof(_call));
memset(&_work, 0, sizeof(_work));
}
MPU9250::~MPU9250()
@@ -334,8 +236,19 @@ MPU9250::~MPU9250()
int
MPU9250::init()
{
int ret;
#if defined(USE_I2C)
unsigned dummy;
use_i2c(_interface->ioctl(MPUIOCGIS_I2C, dummy));
#endif
int ret = probe();
if (ret != OK) {
DEVICE_DEBUG("MPU9250 probe failed");
return ret;
}
/* do init */
@@ -349,21 +262,7 @@ MPU9250::init()
}
// /* do SPI init (and probe) first */
// ret = SPI::init();
// /* if probe/setup failed, bail now */
// if (ret != OK) {
// DEVICE_DEBUG("SPI setup failed");
// return ret;
// }
ret = probe();
if (ret != OK) {
DEVICE_DEBUG("MPU9250 probe failed");
return ret;
}
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
@@ -406,6 +305,11 @@ MPU9250::init()
return ret;
}
if (!_mag->is_passthrough() && _mag->_interface->init() != OK) {
warnx("failed to setup ak8963 interface");
}
/* do CDev init for the mag device node, keep it optional */
ret = _mag->init();
@@ -486,7 +390,7 @@ int MPU9250::reset()
// INT CFG => Interrupt on Data Ready
write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready
usleep(1000);
write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (_mag->is_passthrough()? 0 : BIT_INT_BYPASS_EN)); // INT: Clear on any read, also use i2c bypass is master mode isn't needed
usleep(1000);
write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ);
@@ -758,8 +662,7 @@ MPU9250::test_error()
// development as a handy way to test the reset logic
uint8_t data[16];
memset(data, 0, sizeof(data));
_interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_LOW_BUS_SPEED), data, sizeof(data));
//transfer(data, data, sizeof(data));
_interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_LOW_BUS_SPEED), data, sizeof(data));
::printf("error triggered\n");
print_registers();
}
@@ -973,7 +876,7 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg)
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
return CDev::ioctl(filp, cmd, arg);
}
}
@@ -1071,23 +974,9 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
uint8_t
MPU9250::read_reg(unsigned reg, uint32_t speed)
{
// From MPU6000 implementation
uint8_t buf;
_interface->read(MPU6000_SET_SPEED(reg, speed), &buf, 1);
_interface->read(MPU9250_SET_SPEED(reg, speed), &buf, 1);
return buf;
uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
// general register transfer at low clock speed
set_frequency(speed);
transfer(cmd, cmd, sizeof(cmd));
return cmd[1];
}
uint16_t
@@ -1099,18 +988,6 @@ MPU9250::read_reg16(unsigned reg)
_interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf));
return (uint16_t)(buf[0] << 8) | buf[1];
uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
// general register transfer at low clock speed
set_frequency(MPU9250_LOW_BUS_SPEED);
transfer(cmd, cmd, sizeof(cmd));
return (uint16_t)(cmd[1] << 8) | cmd[2];
}
void
@@ -1118,19 +995,7 @@ MPU9250::write_reg(unsigned reg, uint8_t value)
{
// general register transfer at low clock speed
return _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
uint8_t cmd[2];
cmd[0] = reg | DIR_WRITE;
cmd[1] = value;
// general register transfer at low clock speed
set_frequency(MPU9250_LOW_BUS_SPEED);
transfer(cmd, nullptr, sizeof(cmd));
_interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
}
void
@@ -1153,6 +1018,7 @@ MPU9250::write_checked_reg(unsigned reg, uint8_t value)
if (reg == _checked_registers[i]) {
_checked_values[i] = value;
_checked_bad[i] = value;
break; // TODO: Assumes no duplicates
}
}
}
@@ -1203,18 +1069,69 @@ MPU9250::start()
_gyro_reports->flush();
_mag->_mag_reports->flush();
if (_use_hrt) {
/* start polling at the specified rate */
hrt_call_every(&_call,
1000,
_call_interval - MPU9250_TIMER_REDUCTION,
(hrt_callout)&MPU9250::measure_trampoline, this);
(hrt_callout)&MPU9250::measure_trampoline, this);;
} else {
#ifdef USE_I2C
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&MPU9250::cycle_trampoline, this, 1);
#endif
}
}
void
MPU9250::stop()
{
if (_use_hrt) {
hrt_cancel(&_call);
} else {
#ifdef USE_I2C
work_cancel(HPWORK, &_work);
#endif
}
}
#if defined(USE_I2C)
void
MPU9250::cycle_trampoline(void *arg)
{
MPU9250 *dev = (MPU9250 *)arg;
dev->cycle();
}
void
MPU9250::cycle()
{
// int ret = measure();
measure();
// if (ret != OK) {
// /* issue a reset command to the sensor */
// reset();
// start();
// return;
// }
if (_call_interval != 0) {
work_queue(HPWORK,
&_work,
(worker_t)&MPU9250::cycle_trampoline,
this,
USEC2TICK(_call_interval - MPU9250_TIMER_REDUCTION));
}
}
#endif
void
MPU9250::measure_trampoline(void *arg)
@@ -1350,12 +1267,13 @@ MPU9250::measure()
/* start measuring */
perf_begin(_sample_perf);
// TODO: Don't fetch magnetometer data if it isn't being managed
// In that case, trigger a magnetometer read
/*
* Fetch the full set of measurements from the MPU9250 in one pass.
*/
mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
if (sizeof(mpu_report) != _interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED),
if (OK != _interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED),
(uint8_t *)&mpu_report,
sizeof(mpu_report))) {
return;
@@ -1367,7 +1285,12 @@ MPU9250::measure()
return;
}
_mag->measure(mpu_report.mag);
if (_mag->is_passthrough()) {
_mag->_measure(mpu_report.mag);
}
else {
_mag->measure();
}
/*
* Convert from big to little endian

334
src/drivers/mpu9250/mpu9250.h
View File

@@ -1,13 +1,250 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
// TODO: All these includes are from main.cpp
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <fcntl.h>
#include <errno.h>
#include <stdio.h>
#include <getopt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/conversions.h>
// TODO: These three are new
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <board_config.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
#include <drivers/device/i2c.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/device/integrator.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>
#include "mag.h"
#include "gyro.h"
#if defined(PX4_I2C_OBDEV_MPU9250)
# define USE_I2C
#endif
#define DIR_READ 0x80
#define DIR_WRITE 0x00
// MPU 9250 registers
#define MPUREG_WHOAMI 0x75
#define MPUREG_SMPLRT_DIV 0x19
#define MPUREG_CONFIG 0x1A
#define MPUREG_GYRO_CONFIG 0x1B
#define MPUREG_ACCEL_CONFIG 0x1C
#define MPUREG_ACCEL_CONFIG2 0x1D
#define MPUREG_LPACCEL_ODR 0x1E
#define MPUREG_WOM_THRESH 0x1F
#define MPUREG_FIFO_EN 0x23
#define MPUREG_I2C_MST_CTRL 0x24
#define MPUREG_I2C_SLV0_ADDR 0x25
#define MPUREG_I2C_SLV0_REG 0x26
#define MPUREG_I2C_SLV0_CTRL 0x27
#define MPUREG_I2C_SLV1_ADDR 0x28
#define MPUREG_I2C_SLV1_REG 0x29
#define MPUREG_I2C_SLV1_CTRL 0x2A
#define MPUREG_I2C_SLV2_ADDR 0x2B
#define MPUREG_I2C_SLV2_REG 0x2C
#define MPUREG_I2C_SLV2_CTRL 0x2D
#define MPUREG_I2C_SLV3_ADDR 0x2E
#define MPUREG_I2C_SLV3_REG 0x2F
#define MPUREG_I2C_SLV3_CTRL 0x30
#define MPUREG_I2C_SLV4_ADDR 0x31
#define MPUREG_I2C_SLV4_REG 0x32
#define MPUREG_I2C_SLV4_DO 0x33
#define MPUREG_I2C_SLV4_CTRL 0x34
#define MPUREG_I2C_SLV4_DI 0x35
#define MPUREG_I2C_MST_STATUS 0x36
#define MPUREG_INT_PIN_CFG 0x37
#define MPUREG_INT_ENABLE 0x38
#define MPUREG_INT_STATUS 0x3A
#define MPUREG_ACCEL_XOUT_H 0x3B
#define MPUREG_ACCEL_XOUT_L 0x3C
#define MPUREG_ACCEL_YOUT_H 0x3D
#define MPUREG_ACCEL_YOUT_L 0x3E
#define MPUREG_ACCEL_ZOUT_H 0x3F
#define MPUREG_ACCEL_ZOUT_L 0x40
#define MPUREG_TEMP_OUT_H 0x41
#define MPUREG_TEMP_OUT_L 0x42
#define MPUREG_GYRO_XOUT_H 0x43
#define MPUREG_GYRO_XOUT_L 0x44
#define MPUREG_GYRO_YOUT_H 0x45
#define MPUREG_GYRO_YOUT_L 0x46
#define MPUREG_GYRO_ZOUT_H 0x47
#define MPUREG_GYRO_ZOUT_L 0x48
#define MPUREG_EXT_SENS_DATA_00 0x49
#define MPUREG_I2C_SLV0_D0 0x63
#define MPUREG_I2C_SLV1_D0 0x64
#define MPUREG_I2C_SLV2_D0 0x65
#define MPUREG_I2C_SLV3_D0 0x66
#define MPUREG_I2C_MST_DELAY_CTRL 0x67
#define MPUREG_SIGNAL_PATH_RESET 0x68
#define MPUREG_MOT_DETECT_CTRL 0x69
#define MPUREG_USER_CTRL 0x6A
#define MPUREG_PWR_MGMT_1 0x6B
#define MPUREG_PWR_MGMT_2 0x6C
#define MPUREG_FIFO_COUNTH 0x72
#define MPUREG_FIFO_COUNTL 0x73
#define MPUREG_FIFO_R_W 0x74
// Configuration bits MPU 9250
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define MPU_CLK_SEL_AUTO 0x01
#define BITS_GYRO_ST_X 0x80
#define BITS_GYRO_ST_Y 0x40
#define BITS_GYRO_ST_Z 0x20
#define BITS_FS_250DPS 0x00
#define BITS_FS_500DPS 0x08
#define BITS_FS_1000DPS 0x10
#define BITS_FS_2000DPS 0x18
#define BITS_FS_MASK 0x18
#define BITS_DLPF_CFG_250HZ 0x00
#define BITS_DLPF_CFG_184HZ 0x01
#define BITS_DLPF_CFG_92HZ 0x02
#define BITS_DLPF_CFG_41HZ 0x03
#define BITS_DLPF_CFG_20HZ 0x04
#define BITS_DLPF_CFG_10HZ 0x05
#define BITS_DLPF_CFG_5HZ 0x06
#define BITS_DLPF_CFG_3600HZ 0x07
#define BITS_DLPF_CFG_MASK 0x07
#define BITS_ACCEL_CONFIG2_41HZ 0x03
#define BIT_RAW_RDY_EN 0x01
#define BIT_INT_ANYRD_2CLEAR 0x10
#define BIT_INT_BYPASS_EN 0x02
#define MPU_WHOAMI_9250 0x71
#define MPU9250_ACCEL_DEFAULT_RATE 1000
#define MPU9250_ACCEL_MAX_OUTPUT_RATE 280
#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
#define MPU9250_GYRO_DEFAULT_RATE 1000
/* rates need to be the same between accel and gyro */
#define MPU9250_GYRO_MAX_OUTPUT_RATE MPU9250_ACCEL_MAX_OUTPUT_RATE
#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 41
#define MPU9250_ONE_G 9.80665f
#define MPUIOCGIS_I2C (unsigned)(DEVIOCGDEVICEID+100)
#pragma pack(push, 1)
/**
* Report conversation within the mpu, including command byte and
* interrupt status.
*/
struct MPUReport {
uint8_t cmd;
uint8_t status;
uint8_t accel_x[2];
uint8_t accel_y[2];
uint8_t accel_z[2];
uint8_t temp[2];
uint8_t gyro_x[2];
uint8_t gyro_y[2];
uint8_t gyro_z[2];
struct ak8963_regs mag;
};
#pragma pack(pop)
// TODO: Big enough?
#define MPU_MAX_WRITE_BUFFER_SIZE (2)
/*
The MPU9250 can only handle high bus speeds on the sensor and
interrupt status registers. All other registers have a maximum 1MHz
Communication with all registers of the device is performed using either
I2C at 400kHz or SPI at 1MHz. For applications requiring faster communications,
the sensor and interrupt registers may be read using SPI at 20MHz
*/
#define MPU9250_LOW_BUS_SPEED 0
#define MPU9250_HIGH_BUS_SPEED 0x8000
# define MPU9250_IS_HIGH_SPEED(r) ((r) & MPU9250_HIGH_BUS_SPEED)
# define MPU9250_REG(r) ((r) &~MPU9250_HIGH_BUS_SPEED)
# define MPU9250_SET_SPEED(r, s) ((r)|(s))
# define MPU9250_HIGH_SPEED_OP(r) MPU9250_SET_SPEED((r), MPU9250_HIGH_BUS_SPEED)
# define MPU9250_LOW_SPEED_OP(r) MPU9250_REG((r))
/* interface factories */
extern device::Device *MPU9250_SPI_interface(int bus, bool external_bus);
extern device::Device *MPU9250_I2C_interface(int bus, bool external_bus);
extern int MPU9250_probe(device::Device *dev, int device_type);
typedef device::Device *(*MPU9250_constructor)(int, bool);
class MPU9250_mag;
class MPU9250_gyro;
class MPU9250 : public device::CDev
{
public:
MPU9250(device::Device *interface, const char *path_accel, const char *path_gyro, const char *path_mag,
MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, const char *path_mag,
enum Rotation rotation);
virtual ~MPU9250();
@@ -42,6 +279,15 @@ private:
MPU9250_mag *_mag;
uint8_t _whoami; /** whoami result */
#if defined(USE_I2C)
/*
* SPI bus based device use hrt
* I2C bus needs to use work queue
*/
work_s _work;
#endif
bool _use_hrt;
struct hrt_call _call;
unsigned _call_interval;
@@ -123,6 +369,42 @@ private:
*/
int reset();
#if defined(USE_I2C)
/**
* When the I2C interfase is on
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*
* This is the heart of the measurement state machine. This function
* alternately starts a measurement, or collects the data from the
* previous measurement.
*
* When the interval between measurements is greater than the minimum
* measurement interval, a gap is inserted between collection
* and measurement to provide the most recent measurement possible
* at the next interval.
*/
void cycle();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
void use_i2c(bool on_true) { _use_hrt = !on_true; }
#endif
bool is_i2c(void) { return !_use_hrt; }
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
@@ -193,7 +475,11 @@ private:
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external() { return (_bus == EXTERNAL_BUS); }
bool is_external()
{
unsigned dummy;
return !_interface->ioctl(ACCELIOCGEXTERNAL, dummy);
}
/**
* Measurement self test
@@ -234,48 +520,4 @@ private:
/* do not allow to copy this class due to pointer data members */
MPU9250(const MPU9250 &);
MPU9250 operator=(const MPU9250 &);
#pragma pack(push, 1)
/**
* Report conversation within the mpu, including command byte and
* interrupt status.
*/
struct MPUReport {
uint8_t cmd;
uint8_t status;
uint8_t accel_x[2];
uint8_t accel_y[2];
uint8_t accel_z[2];
uint8_t temp[2];
uint8_t gyro_x[2];
uint8_t gyro_y[2];
uint8_t gyro_z[2];
struct ak8963_regs mag;
};
#pragma pack(pop)
};
/*
The MPU9250 can only handle high bus speeds on the sensor and
interrupt status registers. All other registers have a maximum 1MHz
Communication with all registers of the device is performed using either
I2C at 400kHz or SPI at 1MHz. For applications requiring faster communications,
the sensor and interrupt registers may be read using SPI at 20MHz
*/
#define MPU9250_LOW_BUS_SPEED 0
#define MPU9250_HIGH_BUS_SPEED 0x8000
# define MPU9250_IS_HIGH_SPEED(r) ((r) & MPU9250_HIGH_BUS_SPEED)
# define MPU9250_REG(r) ((r) &~MPU9250_HIGH_BUS_SPEED)
# define MPU9250_SET_SPEED(r, s) ((r)|(s))
# define MPU9250_HIGH_SPEED_OP(r) MPU9250_SET_SPEED((r), MPU9250_HIGH_BUS_SPEED)
# define MPU9250_LOW_SPEED_OP(r) MPU9250_REG((r))
/* interface factories */
extern device::Device *MPU9250_SPI_interface(int bus, int device_type, bool external_bus);
extern device::Device *MPU9250_I2C_interface(int bus, int device_type, bool external_bus);
extern int MPU9250_probe(device::Device *dev, int device_type);
typedef device::Device *(*MPU9250_constructor)(int, int, bool);

29
src/drivers/mpu9250/mpu9250_i2c.cpp
View File

@@ -55,18 +55,18 @@
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
#include "MPU9250.h"
#include "mpu9250.h"
#include "board_config.h"
#ifdef USE_I2C
device::Device *MPU9250_I2C_interface(int bus, int device_type, bool external_bus);
device::Device *MPU9250_I2C_interface(int bus, bool external_bus);
class MPU9250_I2C : public device::I2C
{
public:
MPU9250_I2C(int bus, int device_type);
MPU9250_I2C(int bus);
virtual ~MPU9250_I2C();
virtual int init();
@@ -78,21 +78,17 @@ public:
protected:
virtual int probe();
private:
int _device_type;
};
device::Device *
MPU9250_I2C_interface(int bus, int device_type, bool external_bus)
MPU9250_I2C_interface(int bus, bool external_bus)
{
return new MPU9250_I2C(bus, device_type);
return new MPU9250_I2C(bus);
}
MPU9250_I2C::MPU9250_I2C(int bus, int device_type) :
I2C("MPU9250_I2C", nullptr, bus, PX4_I2C_OBDEV_MPU9250, 400000),
_device_type(device_type)
MPU9250_I2C::MPU9250_I2C(int bus) :
I2C("MPU9250_I2C", nullptr, bus, PX4_I2C_OBDEV_MPU9250, 400000)
{
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250;
}
@@ -156,8 +152,7 @@ MPU9250_I2C::read(unsigned reg_speed, void *data, unsigned count)
*/
uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, status);
uint8_t cmd = MPU9250_REG(reg_speed);
int ret = transfer(&cmd, 1, &((uint8_t *)data)[offset], count);
return ret == OK ? count : ret;
return transfer(&cmd, 1, &((uint8_t *)data)[offset], count);
}
@@ -165,8 +160,8 @@ int
MPU9250_I2C::probe()
{
uint8_t whoami = 0;
uint8_t expected = _device_type == 6000 ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608;
return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
uint8_t expected = MPU_WHOAMI_9250;
return (read(MPUREG_WHOAMI, &whoami, 1) == OK && (whoami == expected)) ? 0 : -EIO;
}
#endif /* PX4_I2C_OBDEV_HMC5883 */
#endif /* USE_I2C */

38
src/drivers/mpu9250/mpu9250_spi.cpp
View File

@@ -64,6 +64,7 @@
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#if PX4_SPIDEV_MPU
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
@@ -78,16 +79,16 @@
SPI speed
*/
#define MPU9250_LOW_SPI_BUS_SPEED 1000*1000
#define MPU9250_HIGH_SPI_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */
#define MPU9250_HIGH_SPI_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU9250 */
device::Device *MPU9250_SPI_interface(int bus, int device_type, bool external_bus);
device::Device *MPU9250_SPI_interface(int bus, bool external_bus);
class MPU9250_SPI : public device::SPI
{
public:
MPU9250_SPI(int bus, spi_dev_e device, int device_type);
MPU9250_SPI(int bus, spi_dev_e device);
virtual ~MPU9250_SPI();
virtual int init();
@@ -100,47 +101,37 @@ protected:
private:
int _device_type;
/* Helper to set the desired speed and isolate the register on return */
void set_bus_frequency(unsigned &reg_speed_reg_out);
};
device::Device *
MPU9250_SPI_interface(int bus, int device_type, bool external_bus)
MPU9250_SPI_interface(int bus, bool external_bus)
{
spi_dev_e cs = SPIDEV_NONE;
device::Device *interface = nullptr;
if (external_bus) {
#ifdef PX4_SPI_BUS_EXT
# if defined(PX4_SPIDEV_EXT_ICM)
cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_EXT_MPU : PX4_SPIDEV_EXT_ICM);
# else
cs = (spi_dev_e) PX4_SPIDEV_EXT_MPU;
# endif
#endif
} else {
#if defined(PX4_SPIDEV_ICM)
cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_MPU : PX4_SPIDEV_ICM);
#else
cs = (spi_dev_e) PX4_SPIDEV_MPU;
errx(0, "External SPI not available");
#endif
} else {
cs = (spi_dev_e) PX4_SPIDEV_MPU;
}
if (cs != SPIDEV_NONE) {
interface = new MPU9250_SPI(bus, cs, device_type);
interface = new MPU9250_SPI(bus, cs);
}
return interface;
}
MPU9250_SPI::MPU9250_SPI(int bus, spi_dev_e device, int device_type) :
SPI("MPU9250", nullptr, bus, device, SPIDEV_MODE3, MPU9250_LOW_SPI_BUS_SPEED),
_device_type(device_type)
MPU9250_SPI::MPU9250_SPI(int bus, spi_dev_e device) :
SPI("MPU9250", nullptr, bus, device, SPIDEV_MODE3, MPU9250_LOW_SPI_BUS_SPEED)
{
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250;
}
@@ -266,16 +257,15 @@ MPU9250_SPI::read(unsigned reg_speed, void *data, unsigned count)
}
return ret == OK ? count : ret;
return ret;
}
int
MPU9250_SPI::probe()
{
uint8_t whoami = 0;
uint8_t expected = _device_type == 6000 ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608;
return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
uint8_t expected = MPU_WHOAMI_9250;
return (read(MPUREG_WHOAMI, &whoami, 1) == OK && (whoami == expected)) ? 0 : -EIO;
}
#endif // PX4_SPIDEV_MPU