Browse Source

Added support for ICM-20608-G to MPU6000 driver

sbg
David Sidrane 9 years ago committed by Lorenz Meier
parent
commit
46c63da8be
  1. 68
      src/drivers/mpu6000/mpu6000.cpp

68
src/drivers/mpu6000/mpu6000.cpp

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

Loading…
Cancel
Save