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

Added ICM20602

Browse Source
This commit is contained in:
David Sidrane
2016-12-16 12:45:54 -10: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

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

@@ -325,8 +325,7 @@ fi
if ver hwcmp PX4FMU_V5
then
# Internal SPI bus ICM-20602
# Place holder Need 20602 Support added
if mpu6000 -R 2 -T 20608 start
if mpu6000 -R 2 -T 20602 start
then
fi

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

@@ -108,7 +108,6 @@
*/
#define MPU6000_TIMER_REDUCTION 200
enum MPU6000_BUS {
MPU6000_BUS_ALL = 0,
MPU6000_BUS_I2C_INTERNAL,
@@ -255,7 +254,7 @@ private:
/**
* is_icm_device
*/
bool is_icm_device() { return _device_type == 20608;}
bool is_icm_device() { return !is_mpu_device();}
/**
* is_mpu_device
*/
@@ -778,7 +777,23 @@ int
MPU6000::probe()
{
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) {
DEVICE_DEBUG("unexpected WHOAMI 0x%02x", whoami);
@@ -803,6 +818,7 @@ MPU6000::probe()
case MPU6000_REV_D9:
case MPU6000_REV_D10:
case ICM20608_REV_00:
case ICM20602_REV_02:
case MPU6050_REV_D8:
DEVICE_DEBUG("ID 0x%02x", _product);
_checked_values[0] = _product;
@@ -2182,29 +2198,36 @@ struct mpu6000_bus_option {
const char *gyropath;
MPU6000_constructor interface_constructor;
uint8_t busnum;
bool external;
MPU6000 *dev;
} bus_options[] = {
#if defined (USE_I2C)
# 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
# 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
#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
#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
};
#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);
bool start_bus(struct mpu6000_bus_option &bus, 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);
void stop(enum MPU6000_BUS busid);
void test(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
*/
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;
if (bus.dev != nullptr) {
warnx("%s SPI not available", external ? "External" : "Internal");
warnx("%s SPI not available", bus.external ? "External" : "Internal");
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) {
warnx("no device on bus %u", (unsigned)bus.busid);
@@ -2308,7 +2331,7 @@ fail:
* or failed to detect the sensor.
*/
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;
@@ -2324,7 +2347,7 @@ start(enum MPU6000_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, range, device_type);
}
exit(started ? 0 : 1);
@@ -2539,8 +2562,11 @@ usage()
{
warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -T 6000|20608 (default 6000)");
warnx(" -X external I2C bus");
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(" -a accel range (in g)");
}
@@ -2553,7 +2579,6 @@ mpu6000_main(int argc, char *argv[])
enum MPU6000_BUS busid = MPU6000_BUS_ALL;
int device_type = 6000;
int ch;
bool external = false;
enum Rotation rotation = ROTATION_NONE;
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];
/*
@@ -2603,7 +2626,7 @@ mpu6000_main(int argc, char *argv[])
*/
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")) {

6
src/drivers/mpu6000/mpu6000.h
View File

@@ -121,6 +121,7 @@
#define MPU_WHOAMI_6000 0x68
#define ICM_WHOAMI_20608 0xaf
#define ICM_WHOAMI_20602 0x12
// ICM2608 specific registers
@@ -140,6 +141,11 @@
#define MPUREG_ICM_UNDOC1 0x11
#define MPUREG_ICM_UNDOC1_VALUE 0xc9
// Product ID Description for ICM2602
// Read From device
#define ICM20602_REV_02 2
// Product ID Description for ICM2608
// There is none

96
src/drivers/mpu6000/mpu6000_spi.cpp
View File

@@ -64,12 +64,12 @@
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#if PX4_SPIDEV_MPU
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
#define EXTERNAL_BUS 0
#endif
#if defined(PX4_SPIDEV_MPU) || defined(PX4_SPIDEV_ICM_20602)
# ifdef PX4_SPI_BUS_EXT
# define EXTERNAL_BUS PX4_SPI_BUS_EXT
# else
# define EXTERNAL_BUS 0
# endif
/*
The MPU6000 can only handle high SPI bus speeds on the sensor and
@@ -109,30 +109,73 @@ private:
device::Device *
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;
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;
#if defined(PX4_SPI_BUS_EXT) || defined(PX4_SPI_BUS_EXTERNAL)
switch (device_type) {
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
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
} 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;
switch (device_type) {
case 6000:
#if defined(PX4_SPIDEV_MPU)
cs = PX4_SPIDEV_MPU;
#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) {
interface = new MPU6000_SPI(bus, cs, device_type);
interface = new MPU6000_SPI(bus, (spi_dev_e) cs, device_type);
}
return interface;
@@ -273,9 +316,24 @@ int
MPU6000_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_6000;
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