fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added support for ICM-20608-G to MPU6000 driver

Browse Source
This commit is contained in:
David Sidrane 2015-11-18 12:13:24 -10:00 committed by Lorenz Meier
parent cb6327ebab
commit 46c63da8be
1 changed files with 55 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
src/drivers/mpu6000/mpu6000.cpp
View File

@ -149,6 +149,12 @@
#define BIT_INT_STATUS_DATA 0x01
#define MPU_WHOAMI_6000 0x68
#define ICM_WHOAMI_20608 0xaf
// Product ID Description for ICM2608
// There is none
#define ICM20608_REV_00 0
// Product ID Description for MPU6000
// high 4 bits low 4 bits
@ -208,7 +214,8 @@ class MPU6000_gyro;
class MPU6000 : public device::SPI
{
public:
MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation);
MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation,
int device_type);
virtual ~MPU6000();
virtual int init();
@ -242,6 +249,7 @@ protected:
virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg);
private:
int _device_type;
MPU6000_gyro *_gyro;
uint8_t _product; /** product code */
@ -326,6 +334,15 @@ private:
*/
int reset();
/**
* is_icm_device
*/
bool is_icm_device() { return _device_type == 20608;}
/**
* is_mpu_device
*/
bool is_mpu_device() { return _device_type == 6000;}
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
@ -507,8 +524,10 @@ private:
/** driver 'main' command */
extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) :
MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation,
int device_type) :
SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
_device_type(device_type),
_gyro(new MPU6000_gyro(this, path_gyro)),
_product(0),
_call{},
@ -780,16 +799,15 @@ int MPU6000::reset()
int
MPU6000::probe()
{
uint8_t whoami;
whoami = read_reg(MPUREG_WHOAMI);
uint8_t whoami = read_reg(MPUREG_WHOAMI);
uint8_t expected = is_mpu_device() ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608;
if (whoami != MPU_WHOAMI_6000) {
if (whoami != expected) {
DEVICE_DEBUG("unexpected WHOAMI 0x%02x", whoami);
return -EIO;
}
/* look for a product ID we recognise */
/* look for a product ID we recognize */
_product = read_reg(MPUREG_PRODUCT_ID);
// verify product revision
@ -806,6 +824,7 @@ MPU6000::probe()
case MPU6000_REV_D8:
case MPU6000_REV_D9:
case MPU6000_REV_D10:
case ICM20608_REV_00:
DEVICE_DEBUG("ID 0x%02x", _product);
_checked_values[0] = _product;
return OK;
@ -1485,6 +1504,13 @@ MPU6000::read_reg(unsigned reg, uint32_t speed)
{
uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
// There is no MPUREG_PRODUCT_ID on the icm device
// so lets make dummy it up and allow the rest of the
// code to run as is
if (reg == MPUREG_PRODUCT_ID && is_icm_device()) {
return ICM20608_REV_00;
}
// general register transfer at low clock speed
set_frequency(speed);
@ -2054,7 +2080,7 @@ namespace mpu6000
MPU6000 *g_dev_int; // on internal bus
MPU6000 *g_dev_ext; // on external bus
void start(bool, enum Rotation, int range);
void start(bool, enum Rotation, int range, int device_type);
void stop(bool);
void test(bool);
void reset(bool);
@ -2071,7 +2097,7 @@ void usage();
* or failed to detect the sensor.
*/
void
start(bool external_bus, enum Rotation rotation, int range)
start(bool external_bus, enum Rotation rotation, int range, int device_type)
{
int fd;
MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
@ -2087,13 +2113,23 @@ start(bool external_bus, enum Rotation rotation, int range)
/* create the driver */
if (external_bus) {
#ifdef PX4_SPI_BUS_EXT
*g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation);
# if defined(PX4_SPIDEV_EXT_ICM)
spi_dev_e cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_EXT_MPU : PX4_SPIDEV_EXT_ICM);
# else
spi_dev_e cs = (spi_dev_e) PX4_SPIDEV_EXT_MPU;
# endif
*g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, cs, rotation, device_type);
#else
errx(0, "External SPI not available");
#endif
} else {
*g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation);
#if defined(PX4_SPIDEV_ICM)
spi_dev_e cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_MPU : PX4_SPIDEV_ICM);
#else
spi_dev_e cs = (spi_dev_e) PX4_SPIDEV_MPU;
#endif
*g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, cs, rotation, device_type);
}
if (*g_dev_ptr == nullptr) {
@ -2337,6 +2373,7 @@ usage()
warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -M 6000|20608 (default 6000)");
warnx(" -R rotation");
warnx(" -a accel range (in g)");
}
@ -2347,17 +2384,22 @@ int
mpu6000_main(int argc, char *argv[])
{
bool external_bus = false;
int device_type = 6000;
int ch;
enum Rotation rotation = ROTATION_NONE;
int accel_range = 8;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc, argv, "XR:a:")) != EOF) {
while ((ch = getopt(argc, argv, "T:XR:a:")) != EOF) {
switch (ch) {
case 'X':
external_bus = true;
break;
case 'T':
device_type = atoi(optarg);
break;
case 'R':
rotation = (enum Rotation)atoi(optarg);
break;
@ -2379,7 +2421,7 @@ mpu6000_main(int argc, char *argv[])
*/
if (!strcmp(verb, "start")) {
mpu6000::start(external_bus, rotation, accel_range);
mpu6000::start(external_bus, rotation, accel_range, device_type);
}
if (!strcmp(verb, "stop")) {