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

Added ICM20602

Browse Source
This commit is contained in:
David Sidrane
2016-12-21 08:34:22 +01:00
committed by Lorenz Meier
parent 05701a2830
commit 6ebd24a678
4 changed files with 127 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rc.sensors
+1 -2
View File
@@ -325,8 +325,7 @@ fi
if ver hwcmp PX4FMU_V5 if ver hwcmp PX4FMU_V5
then then
# Internal SPI bus ICM-20602 # Internal SPI bus ICM-20602
# Place holder Need 20602 Support added if mpu6000 -R 2 -T 20602 start
if mpu6000 -R 2 -T 20608 start
then then
fi fi
src/drivers/mpu6000/mpu6000.cpp
+43 -20
View File
@@ -108,7 +108,6 @@
*/ */
#define MPU6000_TIMER_REDUCTION 200 #define MPU6000_TIMER_REDUCTION 200
enum MPU6000_BUS { enum MPU6000_BUS {
MPU6000_BUS_ALL = 0, MPU6000_BUS_ALL = 0,
MPU6000_BUS_I2C_INTERNAL, MPU6000_BUS_I2C_INTERNAL,
@@ -255,7 +254,7 @@ private:
/** /**
* is_icm_device * is_icm_device
*/ */
bool is_icm_device() { return _device_type == 20608;} bool is_icm_device() { return !is_mpu_device();}
/** /**
* is_mpu_device * is_mpu_device
*/ */
@@ -778,7 +777,23 @@ int
MPU6000::probe() MPU6000::probe()
{ {
uint8_t whoami = read_reg(MPUREG_WHOAMI); uint8_t whoami = read_reg(MPUREG_WHOAMI);
uint8_t expected = is_mpu_device() ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608; uint8_t expected = 0;
switch (_device_type) {
default:
case 6000:
expected = MPU_WHOAMI_6000;
break;
case 20602:
expected = ICM_WHOAMI_20602;
break;
case 20608:
expected = ICM_WHOAMI_20608;
break;
}
if (whoami != expected) { if (whoami != expected) {
DEVICE_DEBUG("unexpected WHOAMI 0x%02x", whoami); DEVICE_DEBUG("unexpected WHOAMI 0x%02x", whoami);
@@ -803,6 +818,7 @@ MPU6000::probe()
case MPU6000_REV_D9: case MPU6000_REV_D9:
case MPU6000_REV_D10: case MPU6000_REV_D10:
case ICM20608_REV_00: case ICM20608_REV_00:
case ICM20602_REV_02:
case MPU6050_REV_D8: case MPU6050_REV_D8:
DEVICE_DEBUG("ID 0x%02x", _product); DEVICE_DEBUG("ID 0x%02x", _product);
_checked_values[0] = _product; _checked_values[0] = _product;
@@ -2182,29 +2198,36 @@ struct mpu6000_bus_option {
const char *gyropath; const char *gyropath;
MPU6000_constructor interface_constructor; MPU6000_constructor interface_constructor;
uint8_t busnum; uint8_t busnum;
bool external;
MPU6000 *dev; MPU6000 *dev;
} bus_options[] = { } bus_options[] = {
#if defined (USE_I2C) #if defined (USE_I2C)
# if defined(PX4_I2C_BUS_ONBOARD) # if defined(PX4_I2C_BUS_ONBOARD)
{ MPU6000_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, &MPU6000_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, { MPU6000_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, &MPU6000_I2C_interface, PX4_I2C_BUS_ONBOARD, false, NULL },
# endif # endif
# if defined(PX4_I2C_BUS_EXPANSION) # if defined(PX4_I2C_BUS_EXPANSION)
{ MPU6000_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, &MPU6000_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, { MPU6000_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, &MPU6000_I2C_interface, PX4_I2C_BUS_EXPANSION, true, NULL },
# endif # endif
#endif #endif
#ifdef PX4_SPIDEV_MPU #ifdef PX4_SPIDEV_MPU
{ MPU6000_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, NULL }, { MPU6000_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL },
#endif #endif
#if defined(PX4_SPI_BUS_EXT) #if defined(PX4_SPI_BUS_EXT)
{ MPU6000_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, &MPU6000_SPI_interface, PX4_SPI_BUS_EXT, NULL }, { MPU6000_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, &MPU6000_SPI_interface, PX4_SPI_BUS_EXT, true, NULL },
#endif
#ifdef PX4_SPIDEV_ICM_20602
{ MPU6000_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL },
#endif
#if defined(PX4_SPI_BUS_EXTERNAL)
{ MPU6000_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, &MPU6000_SPI_interface, PX4_SPI_BUS_EXTERNAL, true, NULL },
#endif #endif
}; };
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
void start(enum MPU6000_BUS busid, enum Rotation rotation, int range, int device_type, bool external); void start(enum MPU6000_BUS busid, enum Rotation rotation, int range, int device_type);
bool start_bus(struct mpu6000_bus_option &bus, enum Rotation rotation, int range, int device_type, bool external); bool start_bus(struct mpu6000_bus_option &bus, enum Rotation rotation, int range, int device_type);
void stop(enum MPU6000_BUS busid); void stop(enum MPU6000_BUS busid);
void test(enum MPU6000_BUS busid); void test(enum MPU6000_BUS busid);
static struct mpu6000_bus_option &find_bus(enum MPU6000_BUS busid); static struct mpu6000_bus_option &find_bus(enum MPU6000_BUS busid);
@@ -2234,16 +2257,16 @@ struct mpu6000_bus_option &find_bus(enum MPU6000_BUS busid)
* start driver for a specific bus option * start driver for a specific bus option
*/ */
bool bool
start_bus(struct mpu6000_bus_option &bus, enum Rotation rotation, int range, int device_type, bool external) start_bus(struct mpu6000_bus_option &bus, enum Rotation rotation, int range, int device_type)
{ {
int fd = -1; int fd = -1;
if (bus.dev != nullptr) { if (bus.dev != nullptr) {
warnx("%s SPI not available", external ? "External" : "Internal"); warnx("%s SPI not available", bus.external ? "External" : "Internal");
return false; return false;
} }
device::Device *interface = bus.interface_constructor(bus.busnum, device_type, external); device::Device *interface = bus.interface_constructor(bus.busnum, device_type, bus.external);
if (interface == nullptr) { if (interface == nullptr) {
warnx("no device on bus %u", (unsigned)bus.busid); warnx("no device on bus %u", (unsigned)bus.busid);
@@ -2308,7 +2331,7 @@ fail:
* or failed to detect the sensor. * or failed to detect the sensor.
*/ */
void void
start(enum MPU6000_BUS busid, enum Rotation rotation, int range, int device_type, bool external) start(enum MPU6000_BUS busid, enum Rotation rotation, int range, int device_type)
{ {
bool started = false; bool started = false;
@@ -2324,7 +2347,7 @@ start(enum MPU6000_BUS busid, enum Rotation rotation, int range, int device_type
continue; continue;
} }
started |= start_bus(bus_options[i], rotation, range, device_type, external); started |= start_bus(bus_options[i], rotation, range, device_type);
} }
exit(started ? 0 : 1); exit(started ? 0 : 1);
@@ -2539,8 +2562,11 @@ 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 I2C bus");
warnx(" -T 6000|20608 (default 6000)"); warnx(" -I internal I2C bus");
warnx(" -S external SPI bus");
warnx(" -s internal SPI bus");
warnx(" -T 6000|20608|20602 (default 6000)");
warnx(" -R rotation"); warnx(" -R rotation");
warnx(" -a accel range (in g)"); warnx(" -a accel range (in g)");
} }
@@ -2553,7 +2579,6 @@ mpu6000_main(int argc, char *argv[])
enum MPU6000_BUS busid = MPU6000_BUS_ALL; enum MPU6000_BUS busid = MPU6000_BUS_ALL;
int device_type = 6000; int device_type = 6000;
int ch; int ch;
bool external = false;
enum Rotation rotation = ROTATION_NONE; enum Rotation rotation = ROTATION_NONE;
int accel_range = 8; int accel_range = 8;
@@ -2594,8 +2619,6 @@ mpu6000_main(int argc, char *argv[])
} }
} }
external = (busid == MPU6000_BUS_I2C_EXTERNAL || busid == MPU6000_BUS_SPI_EXTERNAL);
const char *verb = argv[optind]; const char *verb = argv[optind];
/* /*
@@ -2603,7 +2626,7 @@ mpu6000_main(int argc, char *argv[])
*/ */
if (!strcmp(verb, "start")) { if (!strcmp(verb, "start")) {
mpu6000::start(busid, rotation, accel_range, device_type, external); mpu6000::start(busid, rotation, accel_range, device_type);
} }
if (!strcmp(verb, "stop")) { if (!strcmp(verb, "stop")) {
src/drivers/mpu6000/mpu6000.h
+6
View File
@@ -121,6 +121,7 @@
#define MPU_WHOAMI_6000 0x68 #define MPU_WHOAMI_6000 0x68
#define ICM_WHOAMI_20608 0xaf #define ICM_WHOAMI_20608 0xaf
#define ICM_WHOAMI_20602 0x12
// ICM2608 specific registers // ICM2608 specific registers
@@ -140,6 +141,11 @@
#define MPUREG_ICM_UNDOC1 0x11 #define MPUREG_ICM_UNDOC1 0x11
#define MPUREG_ICM_UNDOC1_VALUE 0xc9 #define MPUREG_ICM_UNDOC1_VALUE 0xc9
// Product ID Description for ICM2602
// Read From device
#define ICM20602_REV_02 2
// Product ID Description for ICM2608 // Product ID Description for ICM2608
// There is none // There is none
src/drivers/mpu6000/mpu6000_spi.cpp
+77 -19
View File
@@ -64,12 +64,12 @@
#define DIR_READ 0x80 #define DIR_READ 0x80
#define DIR_WRITE 0x00 #define DIR_WRITE 0x00
#if PX4_SPIDEV_MPU #if defined(PX4_SPIDEV_MPU) || defined(PX4_SPIDEV_ICM_20602)
#ifdef PX4_SPI_BUS_EXT # ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT # define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else # else
#define EXTERNAL_BUS 0 # define EXTERNAL_BUS 0
#endif # endif
/* /*
The MPU6000 can only handle high SPI bus speeds on the sensor and The MPU6000 can only handle high SPI bus speeds on the sensor and
@@ -109,30 +109,73 @@ private:
device::Device * device::Device *
MPU6000_SPI_interface(int bus, int device_type, bool external_bus) MPU6000_SPI_interface(int bus, int device_type, bool external_bus)
{ {
spi_dev_e cs = SPIDEV_NONE; int cs = SPIDEV_NONE;
device::Device *interface = nullptr; device::Device *interface = nullptr;
if (external_bus) { if (external_bus) {
#ifdef PX4_SPI_BUS_EXT
# if defined(PX4_SPIDEV_EXT_ICM) #if defined(PX4_SPI_BUS_EXT) || defined(PX4_SPI_BUS_EXTERNAL)
cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_EXT_MPU : PX4_SPIDEV_EXT_ICM);
# else switch (device_type) {
cs = (spi_dev_e) PX4_SPIDEV_EXT_MPU;
case 6000:
# if defined(PX4_SPIDEV_EXT_MPU)
cs = PX4_SPIDEV_EXT_MPU;
# endif
break;
case 20602:
# if defined(PX4_SPIDEV_ICM_20602_EXT)
cs = PX4_SPIDEV_ICM_20602_EXT;
# endif # endif
break;
case 20608:
# if defined(PX4_SPIDEV_EXT_ICM)
cs = PX4_SPIDEV_EXT_ICM;
# elif defined(PX4_SPIDEV_ICM_20608_EXT)
cs = PX4_SPIDEV_ICM_20608_EXT;
# endif
break;
default:
break;
}
#endif #endif
} else { } else {
#if defined(PX4_SPIDEV_ICM) switch (device_type) {
cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_MPU : PX4_SPIDEV_ICM);
#else case 6000:
cs = (spi_dev_e) PX4_SPIDEV_MPU; #if defined(PX4_SPIDEV_MPU)
cs = PX4_SPIDEV_MPU;
#endif #endif
break;
case 20602:
#if defined(PX4_SPIDEV_ICM_20602)
cs = PX4_SPIDEV_ICM_20602;
#endif
break;
case 20608:
#if defined(PX4_SPIDEV_ICM)
cs = PX4_SPIDEV_ICM;
#elif defined(PX4_SPIDEV_ICM_20608)
cs = PX4_SPIDEV_ICM_20608;
#endif
break;
default:
break;
}
} }
if (cs != SPIDEV_NONE) { if (cs != SPIDEV_NONE) {
interface = new MPU6000_SPI(bus, cs, device_type); interface = new MPU6000_SPI(bus, (spi_dev_e) cs, device_type);
} }
return interface; return interface;
@@ -273,9 +316,24 @@ int
MPU6000_SPI::probe() MPU6000_SPI::probe()
{ {
uint8_t whoami = 0; uint8_t whoami = 0;
uint8_t expected = _device_type == 6000 ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608; uint8_t expected = MPU_WHOAMI_6000;
return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
switch (_device_type) {
default:
case 6000:
break;
case 20602:
expected = ICM_WHOAMI_20602;
break;
case 20608:
expected = ICM_WHOAMI_20608;
break;
}
return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
} }
#endif // PX4_SPIDEV_MPU #endif // PX4_SPIDEV_MPU