fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 18:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged master into driver_framework

Browse Source
This commit is contained in:
Lorenz Meier
2015-12-01 12:34:02 +01:00
parent 1e99b04d9c 93bb1ab80f
commit f0a4979da6
109 changed files with 5346 additions and 359 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/mpu6000/mpu6000.cpp
+76 -15
View File
@@ -149,6 +149,21 @@
#define BIT_INT_STATUS_DATA 0x01
#define MPU_WHOAMI_6000 0x68
#define ICM_WHOAMI_20608 0xaf
// ICM2608 specific registers
/* this is an undocumented register which
if set incorrectly results in getting a 2.7m/s/s offset
on the Y axis of the accelerometer
*/
#define MPUREG_ICM_UNDOC1 0x11
#define MPUREG_ICM_UNDOC1_VALUE 0xc9
// 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 +223,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 +258,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 */
@@ -292,7 +309,7 @@ private:
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define MPU6000_NUM_CHECKED_REGISTERS 9
#define MPU6000_NUM_CHECKED_REGISTERS 10
static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS];
uint8_t _checked_next;
@@ -325,6 +342,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.
@@ -468,7 +494,8 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU
MPUREG_GYRO_CONFIG,
MPUREG_ACCEL_CONFIG,
MPUREG_INT_ENABLE,
MPUREG_INT_PIN_CFG
MPUREG_INT_PIN_CFG,
MPUREG_ICM_UNDOC1
};
@@ -506,8 +533,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{},
@@ -768,6 +797,10 @@ int MPU6000::reset()
write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
usleep(1000);
if (is_icm_device()) {
write_checked_reg(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE);
}
// Oscillator set
// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
usleep(1000);
@@ -778,16 +811,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
@@ -804,6 +836,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;
@@ -1483,6 +1516,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);
@@ -1641,6 +1681,11 @@ MPU6000::check_registers(void)
*/
uint8_t v;
// the MPUREG_ICM_UNDOC1 is specific to the ICM20608 (and undocumented)
if (_checked_registers[_checked_next] == MPUREG_ICM_UNDOC1 && !is_icm_device()) {
_checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS;
}
if ((v = read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) !=
_checked_values[_checked_next]) {
/*
@@ -2051,7 +2096,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);
@@ -2068,7 +2113,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;
@@ -2084,13 +2129,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) {
@@ -2334,6 +2389,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)");
}
@@ -2344,17 +2400,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;
@@ -2376,7 +2437,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")) {