diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index a985191a4e..d5a7941a6a 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -109,4 +109,8 @@ struct accel_calibration_s { /** get the hardware low-pass filter cut-off in Hz*/ #define ACCELIOCGHWLOWPASS _ACCELIOC(11) +/** determine if hardware is external or onboard */ +#define ACCELIOCGEXTERNAL _ACCELIOC(12) + + #endif /* _DRV_ACCEL_H */ diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 431c2fac67..04a6f14081 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -106,4 +106,8 @@ struct gyro_calibration_s { /** get the hardware low-pass filter cut-off in Hz*/ #define GYROIOCGHWLOWPASS _GYROIOC(10) +/** determine if hardware is external or onboard */ +#define GYROIOCGEXTERNAL _GYROIOC(12) + + #endif /* _DRV_GYRO_H */ diff --git a/src/drivers/mpu6000/CMakeLists.txt b/src/drivers/mpu6000/CMakeLists.txt index 13009f31b0..290ff79917 100644 --- a/src/drivers/mpu6000/CMakeLists.txt +++ b/src/drivers/mpu6000/CMakeLists.txt @@ -33,12 +33,14 @@ px4_add_module( MODULE drivers__mpu6000 MAIN mpu6000 - STACK_MAIN 1300 + STACK_MAIN 1400 COMPILE_FLAGS -Weffc++ -Os SRCS mpu6000.cpp + mpu6000_i2c.cpp + mpu6000_spi.cpp DEPENDS platforms__common ) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index fce303315e..efa59a3e2b 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -34,10 +34,25 @@ /** * @file mpu6000.cpp * - * Driver for the Invensense MPU6000 connected via SPI. + * Driver for the Invensense MPU6000, MPU6050 and the ICM2608 connected via + * SPI or I2C. + * + * When the device is on the SPI bus the hrt is used to provide thread of + * execution to the driver. + * + * When the device is on the I2C bus a work queue is used provide thread of + * execution to the driver. + * + * The I2C code is only included in the build if USE_I2C is defined by the + * existance of any of PX4_I2C_MPU6050_ADDR, PX4_I2C_MPU6000_ADDR or + * PX4_I2C_ICM_20608_G_ADDR in the board_config.h file. + * + * The command line option -T 6000|20608 (default 6000) selects between + * MPU60x0 or the ICM20608G; * * @author Andrew Tridgell * @author Pat Hickey + * @author David Sidrane */ #include @@ -60,14 +75,17 @@ #include #include #include +#include #include +#include #include #include #include #include +#include #include #include #include @@ -75,155 +93,36 @@ #include #include -#define DIR_READ 0x80 -#define DIR_WRITE 0x00 - -#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel" -#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro" -#define MPU_DEVICE_PATH_ACCEL_EXT "/dev/mpu6000_accel_ext" -#define MPU_DEVICE_PATH_GYRO_EXT "/dev/mpu6000_gyro_ext" - -// MPU 6000 registers -#define MPUREG_WHOAMI 0x75 -#define MPUREG_SMPLRT_DIV 0x19 -#define MPUREG_CONFIG 0x1A -#define MPUREG_GYRO_CONFIG 0x1B -#define MPUREG_ACCEL_CONFIG 0x1C -#define MPUREG_FIFO_EN 0x23 -#define MPUREG_INT_PIN_CFG 0x37 -#define MPUREG_INT_ENABLE 0x38 -#define MPUREG_INT_STATUS 0x3A -#define MPUREG_ACCEL_XOUT_H 0x3B -#define MPUREG_ACCEL_XOUT_L 0x3C -#define MPUREG_ACCEL_YOUT_H 0x3D -#define MPUREG_ACCEL_YOUT_L 0x3E -#define MPUREG_ACCEL_ZOUT_H 0x3F -#define MPUREG_ACCEL_ZOUT_L 0x40 -#define MPUREG_TEMP_OUT_H 0x41 -#define MPUREG_TEMP_OUT_L 0x42 -#define MPUREG_GYRO_XOUT_H 0x43 -#define MPUREG_GYRO_XOUT_L 0x44 -#define MPUREG_GYRO_YOUT_H 0x45 -#define MPUREG_GYRO_YOUT_L 0x46 -#define MPUREG_GYRO_ZOUT_H 0x47 -#define MPUREG_GYRO_ZOUT_L 0x48 -#define MPUREG_USER_CTRL 0x6A -#define MPUREG_PWR_MGMT_1 0x6B -#define MPUREG_PWR_MGMT_2 0x6C -#define MPUREG_FIFO_COUNTH 0x72 -#define MPUREG_FIFO_COUNTL 0x73 -#define MPUREG_FIFO_R_W 0x74 -#define MPUREG_PRODUCT_ID 0x0C -#define MPUREG_TRIM1 0x0D -#define MPUREG_TRIM2 0x0E -#define MPUREG_TRIM3 0x0F -#define MPUREG_TRIM4 0x10 - -// Configuration bits MPU 3000 and MPU 6000 (not revised)? -#define BIT_SLEEP 0x40 -#define BIT_H_RESET 0x80 -#define BITS_CLKSEL 0x07 -#define MPU_CLK_SEL_PLLGYROX 0x01 -#define MPU_CLK_SEL_PLLGYROZ 0x03 -#define MPU_EXT_SYNC_GYROX 0x02 -#define BITS_GYRO_ST_X 0x80 -#define BITS_GYRO_ST_Y 0x40 -#define BITS_GYRO_ST_Z 0x20 -#define BITS_FS_250DPS 0x00 -#define BITS_FS_500DPS 0x08 -#define BITS_FS_1000DPS 0x10 -#define BITS_FS_2000DPS 0x18 -#define BITS_FS_MASK 0x18 -#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 -#define BITS_DLPF_CFG_188HZ 0x01 -#define BITS_DLPF_CFG_98HZ 0x02 -#define BITS_DLPF_CFG_42HZ 0x03 -#define BITS_DLPF_CFG_20HZ 0x04 -#define BITS_DLPF_CFG_10HZ 0x05 -#define BITS_DLPF_CFG_5HZ 0x06 -#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 -#define BITS_DLPF_CFG_MASK 0x07 -#define BIT_INT_ANYRD_2CLEAR 0x10 -#define BIT_RAW_RDY_EN 0x01 -#define BIT_I2C_IF_DIS 0x10 -#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 -// Product Name Product Revision -#define MPU6000ES_REV_C4 0x14 -#define MPU6000ES_REV_C5 0x15 -#define MPU6000ES_REV_D6 0x16 -#define MPU6000ES_REV_D7 0x17 -#define MPU6000ES_REV_D8 0x18 -#define MPU6000_REV_C4 0x54 -#define MPU6000_REV_C5 0x55 -#define MPU6000_REV_D6 0x56 -#define MPU6000_REV_D7 0x57 -#define MPU6000_REV_D8 0x58 -#define MPU6000_REV_D9 0x59 -#define MPU6000_REV_D10 0x5A - -#define MPU6000_ACCEL_DEFAULT_RANGE_G 8 -#define MPU6000_ACCEL_DEFAULT_RATE 1000 -#define MPU6000_ACCEL_MAX_OUTPUT_RATE 280 -#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 - -#define MPU6000_GYRO_DEFAULT_RANGE_G 8 -#define MPU6000_GYRO_DEFAULT_RATE 1000 -/* rates need to be the same between accel and gyro */ -#define MPU6000_GYRO_MAX_OUTPUT_RATE MPU6000_ACCEL_MAX_OUTPUT_RATE -#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 - -#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42 - -#define MPU6000_ONE_G 9.80665f - -#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 - interrupt status registers. All other registers have a maximum 1MHz - SPI speed - */ -#define MPU6000_LOW_BUS_SPEED 1000*1000 -#define MPU6000_HIGH_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */ +#include "mpu6000.h" /* we set the timer interrupt to run a bit faster than the desired sample rate and then throw away duplicates by comparing accelerometer values. This time reduction is enough to cope with worst case timing jitter due to other timers + + I2C bus is running at 100 kHz Transaction time is 2.163Ms + I2C bus is running at 400 kHz (304 kHz acutal) Transaction time + is 583 uS + */ #define MPU6000_TIMER_REDUCTION 200 + +enum MPU6000_BUS { + MPU6000_BUS_ALL = 0, + MPU6000_BUS_I2C_INTERNAL, + MPU6000_BUS_I2C_EXTERNAL, + MPU6000_BUS_SPI_INTERNAL, + MPU6000_BUS_SPI_EXTERNAL +}; + class MPU6000_gyro; -class MPU6000 : public device::SPI +class MPU6000 : public device::CDev { public: - MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation, + MPU6000(device::Device *interface, const char *path_accel, const char *path_gyro, enum Rotation rotation, int device_type); virtual ~MPU6000(); @@ -250,6 +149,8 @@ public: void test_error(); protected: + Device *_interface; + virtual int probe(); friend class MPU6000_gyro; @@ -262,6 +163,15 @@ private: MPU6000_gyro *_gyro; uint8_t _product; /** product code */ +#if defined(USE_I2C) + /* + * SPI bus based device use hrt + * I2C bus needs to use work queue + */ + work_s _work; +#endif + bool _use_hrt; + struct hrt_call _call; unsigned _call_interval; @@ -351,6 +261,39 @@ private: */ bool is_mpu_device() { return _device_type == 6000;} + +#if defined(USE_I2C) + /** + * When the I2C interfase is on + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + * + * This is the heart of the measurement state machine. This function + * alternately starts a measurement, or collects the data from the + * previous measurement. + * + * When the interval between measurements is greater than the minimum + * measurement interval, a gap is inserted between collection + * and measurement to provide the most recent measurement possible + * at the next interval. + */ + void cycle(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + void use_i2c(bool on_true) { _use_hrt = !on_true; } + +#endif + + bool is_i2c(void) { return !_use_hrt; } + + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -365,7 +308,7 @@ private: /** * Fetch measurements from the sensor and update the report buffers. */ - void measure(); + int measure(); /** * Read a register from the MPU6000 @@ -376,13 +319,14 @@ private: uint8_t read_reg(unsigned reg, uint32_t speed = MPU6000_LOW_BUS_SPEED); uint16_t read_reg16(unsigned reg); + /** * Write a register in the MPU6000 * * @param reg The register to write. * @param value The new value to write. */ - void write_reg(unsigned reg, uint8_t value); + int write_reg(unsigned reg, uint8_t value); /** * Modify a register in the MPU6000 @@ -409,7 +353,7 @@ private: * @param max_g The maximum G value the range must support. * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_accel_range(unsigned max_g); + int set_accel_range(unsigned max_g); /** * Swap a 16-bit value read from the MPU6000 to native byte order. @@ -421,7 +365,11 @@ private: * * @return true if the sensor is not on the main MCU board */ - bool is_external() { return (_bus == EXTERNAL_BUS); } + bool is_external() + { + unsigned dummy; + return !_interface->ioctl(ACCELIOCGEXTERNAL, dummy); + } /** * Measurement self test @@ -435,51 +383,35 @@ private: * * @return 0 on success, 1 on failure */ - int accel_self_test(); + int accel_self_test(); /** * Gyro self test * * @return 0 on success, 1 on failure */ - int gyro_self_test(); + int gyro_self_test(); /* set low pass filter frequency */ - void _set_dlpf_filter(uint16_t frequency_hz); + void _set_dlpf_filter(uint16_t frequency_hz); + void _set_icm_acc_dlpf_filter(uint16_t frequency_hz); /* set sample rate (approximate) - 1kHz to 5Hz */ - void _set_sample_rate(unsigned desired_sample_rate_hz); + void _set_sample_rate(unsigned desired_sample_rate_hz); /* check that key registers still have the right value */ - void check_registers(void); + void check_registers(void); /* do not allow to copy this class due to pointer data members */ MPU6000(const MPU6000 &); MPU6000 operator=(const MPU6000 &); -#pragma pack(push, 1) - /** - * Report conversation within the MPU6000, including command byte and - * interrupt status. - */ - struct MPUReport { - uint8_t cmd; - uint8_t status; - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; - }; -#pragma pack(pop) }; /* @@ -533,13 +465,20 @@ 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(device::Device *interface, const char *path_accel, const char *path_gyro, enum Rotation rotation, int device_type) : - SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), + CDev("MPU6000", path_accel), + _interface(interface), _device_type(device_type), _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), - _call{}, +#if defined(USE_I2C) + _work {}, + _use_hrt(false), +#else + _use_hrt(true), +#endif + _call {}, _call_interval(0), _accel_reports(nullptr), _accel_scale{}, @@ -642,17 +581,36 @@ MPU6000::~MPU6000() int MPU6000::init() { - int ret; - /* do SPI init (and probe) first */ - ret = SPI::init(); +#if defined(USE_I2C) + unsigned dummy; + use_i2c(_interface->ioctl(MPUIOCGIS_I2C, dummy)); +#endif + + + /* probe again to get our settings that are based on the device type */ + + int ret = probe(); + + /* if probe failed, bail now */ - /* if probe/setup failed, bail now */ if (ret != OK) { - DEVICE_DEBUG("SPI setup failed"); + + DEVICE_DEBUG("CDev init failed"); return ret; } + /* do init */ + + ret = CDev::init(); + + /* if init failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("CDev init failed"); + return ret; + } + + ret = -ENOMEM; /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); @@ -666,6 +624,8 @@ MPU6000::init() goto out; } + ret = -EIO; + if (reset() != OK) { goto out; } @@ -748,8 +708,15 @@ int MPU6000::reset() write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); up_udelay(1000); - // Disable I2C bus (recommended on datasheet) - write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); + if (is_i2c()) { + // Enable I2C bus (recommended on datasheet) + write_checked_reg(MPUREG_USER_CTRL, 0); + + } else { + // Disable I2C bus (recommended on datasheet) + write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); + } + px4_leave_critical_section(state); if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) { @@ -774,6 +741,11 @@ int MPU6000::reset() // was 90 Hz, but this ruins quality and does not improve the // system response _set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ); + + if (is_icm_device()) { + _set_icm_acc_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ); + } + usleep(1000); // Gyro scale 2000 deg/s () write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); @@ -837,6 +809,7 @@ MPU6000::probe() case MPU6000_REV_D9: case MPU6000_REV_D10: case ICM20608_REV_00: + case MPU6050_REV_D8: DEVICE_DEBUG("ID 0x%02x", _product); _checked_values[0] = _product; return OK; @@ -880,36 +853,75 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) choose next highest filter frequency available */ if (frequency_hz == 0) { - filter = BITS_DLPF_CFG_2100HZ_NOLPF; + filter = MPU_GYRO_DLPF_CFG_2100HZ_NOLPF; } else if (frequency_hz <= 5) { - filter = BITS_DLPF_CFG_5HZ; + filter = MPU_GYRO_DLPF_CFG_5HZ; } else if (frequency_hz <= 10) { - filter = BITS_DLPF_CFG_10HZ; + filter = MPU_GYRO_DLPF_CFG_10HZ; } else if (frequency_hz <= 20) { - filter = BITS_DLPF_CFG_20HZ; + filter = MPU_GYRO_DLPF_CFG_20HZ; } else if (frequency_hz <= 42) { - filter = BITS_DLPF_CFG_42HZ; + filter = MPU_GYRO_DLPF_CFG_42HZ; } else if (frequency_hz <= 98) { - filter = BITS_DLPF_CFG_98HZ; + filter = MPU_GYRO_DLPF_CFG_98HZ; } else if (frequency_hz <= 188) { - filter = BITS_DLPF_CFG_188HZ; + filter = MPU_GYRO_DLPF_CFG_188HZ; } else if (frequency_hz <= 256) { - filter = BITS_DLPF_CFG_256HZ_NOLPF2; + filter = MPU_GYRO_DLPF_CFG_256HZ_NOLPF2; } else { - filter = BITS_DLPF_CFG_2100HZ_NOLPF; + filter = MPU_GYRO_DLPF_CFG_2100HZ_NOLPF; } write_checked_reg(MPUREG_CONFIG, filter); } +void +MPU6000::_set_icm_acc_dlpf_filter(uint16_t frequency_hz) +{ + uint8_t filter; + + /* + choose next highest filter frequency available + */ + if (frequency_hz == 0) { + filter = ICM_ACC_DLPF_CFG_1046HZ_NOLPF; + + } else if (frequency_hz <= 5) { + filter = ICM_ACC_DLPF_CFG_5HZ; + + } else if (frequency_hz <= 10) { + filter = ICM_ACC_DLPF_CFG_10HZ; + + } else if (frequency_hz <= 21) { + filter = ICM_ACC_DLPF_CFG_21HZ; + + } else if (frequency_hz <= 44) { + filter = ICM_ACC_DLPF_CFG_44HZ; + + } else if (frequency_hz <= 99) { + filter = ICM_ACC_DLPF_CFG_99HZ; + + } else if (frequency_hz <= 218) { + filter = ICM_ACC_DLPF_CFG_218HZ; + + } else if (frequency_hz <= 420) { + filter = ICM_ACC_DLPF_CFG_420HZ; + + } else { + filter = ICM_ACC_DLPF_CFG_1046HZ_NOLPF; + } + + write_checked_reg(ICMREG_ACCEL_CONFIG2, filter); +} + ssize_t MPU6000::read(struct file *filp, char *buffer, size_t buflen) { @@ -1085,7 +1097,6 @@ MPU6000::factory_self_test() float gyro_ftrim[3]; // get baseline values without self-test enabled - set_frequency(MPU6000_HIGH_BUS_SPEED); memset(accel_baseline, 0, sizeof(accel_baseline)); memset(gyro_baseline, 0, sizeof(gyro_baseline)); @@ -1094,8 +1105,8 @@ MPU6000::factory_self_test() for (uint8_t i = 0; i < repeats; i++) { up_udelay(1000); - mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)); + _interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_HIGH_BUS_SPEED), (uint8_t *)&mpu_report, + sizeof(mpu_report)); accel_baseline[0] += int16_t_from_bytes(mpu_report.accel_x); accel_baseline[1] += int16_t_from_bytes(mpu_report.accel_y); @@ -1119,13 +1130,12 @@ MPU6000::factory_self_test() up_udelay(20000); // get values with self-test enabled - set_frequency(MPU6000_HIGH_BUS_SPEED); - for (uint8_t i = 0; i < repeats; i++) { up_udelay(1000); - mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)); + _interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_HIGH_BUS_SPEED), (uint8_t *)&mpu_report, + sizeof(mpu_report)); + accel[0] += int16_t_from_bytes(mpu_report.accel_x); accel[1] += int16_t_from_bytes(mpu_report.accel_y); accel[2] += int16_t_from_bytes(mpu_report.accel_z); @@ -1222,9 +1232,9 @@ MPU6000::test_error() _in_factory_test = true; // deliberately trigger an error. This was noticed during // development as a handy way to test the reset logic - uint8_t data[16]; + uint8_t data[sizeof(MPUReport)]; memset(data, 0, sizeof(data)); - transfer(data, data, sizeof(data)); + _interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_LOW_BUS_SPEED), data, sizeof(data)); ::printf("error triggered\n"); print_registers(); _in_factory_test = false; @@ -1273,6 +1283,8 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) int MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) { + unsigned dummy = arg; + switch (cmd) { case SENSORIOCRESET: @@ -1318,6 +1330,11 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); float sample_rate = 1.0e6f / ticks; _set_dlpf_filter(cutoff_freq_hz); + + if (is_icm_device()) { + _set_icm_acc_dlpf_filter(cutoff_freq_hz); + } + _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); @@ -1339,7 +1356,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) them. This prevents aliasing due to a beat between the stm32 clock and the mpu6000 clock */ - _call.period = _call_interval - MPU6000_TIMER_REDUCTION; + + if (!is_i2c()) { + _call.period = _call_interval - MPU6000_TIMER_REDUCTION; + } /* if we need to start the poll state machine, do it */ if (want_start) { @@ -1392,6 +1412,11 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSLOWPASS: // set hardware filtering _set_dlpf_filter(arg); + + if (is_icm_device()) { + _set_icm_acc_dlpf_filter(arg); + } + // set software filtering _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); @@ -1426,15 +1451,23 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSELFTEST: return accel_self_test(); + case ACCELIOCGEXTERNAL: + return _interface->ioctl(cmd, dummy); + + case DEVIOCGDEVICEID: + return _interface->ioctl(cmd, dummy); + default: /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); + return CDev::ioctl(filp, cmd, arg); } } int MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) { + unsigned dummy = arg; + switch (cmd) { /* these are shared with the accel side */ @@ -1505,57 +1538,53 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSELFTEST: return gyro_self_test(); + case GYROIOCGEXTERNAL: + return _interface->ioctl(cmd, dummy); + + case DEVIOCGDEVICEID: + return _interface->ioctl(cmd, dummy); + default: /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); + return CDev::ioctl(filp, cmd, arg); } } uint8_t 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 + */ - // 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); - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[1]; + uint8_t buf; + _interface->read(MPU6000_SET_SPEED(reg, speed), &buf, 1); + return buf; } uint16_t MPU6000::read_reg16(unsigned reg) { - uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; + uint8_t buf[2]; // general register transfer at low clock speed - set_frequency(MPU6000_LOW_BUS_SPEED); - transfer(cmd, cmd, sizeof(cmd)); - - return (uint16_t)(cmd[1] << 8) | cmd[2]; + _interface->read(MPU6000_LOW_SPEED_OP(reg), &buf, arraySize(buf)); + return (uint16_t)(buf[0] << 8) | buf[1]; } -void +int MPU6000::write_reg(unsigned reg, uint8_t value) { - uint8_t cmd[2]; - - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; - // general register transfer at low clock speed - set_frequency(MPU6000_LOW_BUS_SPEED); - transfer(cmd, nullptr, sizeof(cmd)); + return _interface->write(MPU6000_LOW_SPEED_OP(reg), &value, 1); } void @@ -1638,17 +1667,34 @@ MPU6000::start() _accel_reports->flush(); _gyro_reports->flush(); - /* start polling at the specified rate */ - hrt_call_every(&_call, - 1000, - _call_interval - MPU6000_TIMER_REDUCTION, - (hrt_callout)&MPU6000::measure_trampoline, this); + if (_use_hrt) { + /* start polling at the specified rate */ + hrt_call_every(&_call, + 1000, + _call_interval - MPU6000_TIMER_REDUCTION, + (hrt_callout)&MPU6000::measure_trampoline, this); + + } else { +#ifdef USE_I2C + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MPU6000::cycle_trampoline, this, 1); +#endif + } } void MPU6000::stop() { - hrt_cancel(&_call); + + if (_use_hrt) { + hrt_cancel(&_call); + + } else { +#ifdef USE_I2C + _call_interval = 0; + work_cancel(HPWORK, &_work); +#endif + } /* reset internal states */ memset(_last_accel, 0, sizeof(_last_accel)); @@ -1658,6 +1704,38 @@ MPU6000::stop() _gyro_reports->flush(); } +#if defined(USE_I2C) +void +MPU6000::cycle_trampoline(void *arg) +{ + MPU6000 *dev = (MPU6000 *)arg; + + dev->cycle(); +} + +void +MPU6000::cycle() +{ + + int ret = measure(); + + if (ret != OK) { + /* issue a reset command to the sensor */ + reset(); + start(); + return; + } + + if (_call_interval != 0) { + work_queue(HPWORK, + &_work, + (worker_t)&MPU6000::cycle_trampoline, + this, + USEC2TICK(_call_interval - MPU6000_TIMER_REDUCTION)); + } +} +#endif + void MPU6000::measure_trampoline(void *arg) { @@ -1666,7 +1744,6 @@ MPU6000::measure_trampoline(void *arg) /* make another measurement */ dev->measure(); } - void MPU6000::check_registers(void) { @@ -1728,17 +1805,17 @@ MPU6000::check_registers(void) _checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS; } -void +int MPU6000::measure() { if (_in_factory_test) { // don't publish any data while in factory test mode - return; + return OK; } if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete - return; + return OK; } struct MPUReport mpu_report; @@ -1759,13 +1836,13 @@ MPU6000::measure() /* * Fetch the full set of measurements from the MPU6000 in one pass. */ - mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; // sensor transfer at high clock speed - set_frequency(MPU6000_HIGH_BUS_SPEED); - if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) { - return; + if (sizeof(mpu_report) != _interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_HIGH_BUS_SPEED), + (uint8_t *)&mpu_report, + sizeof(mpu_report))) { + return -EIO; } check_registers(); @@ -1783,7 +1860,7 @@ MPU6000::measure() perf_end(_sample_perf); perf_count(_duplicates); _got_duplicate = true; - return; + return OK; } memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6); @@ -1817,7 +1894,7 @@ MPU6000::measure() // costs 20ms with interrupts disabled. That means if // the mpu6k does go bad it would cause a FMU failure, // regardless of whether another sensor is available, - return; + return -EIO; } perf_count(_good_transfers); @@ -1827,7 +1904,7 @@ MPU6000::measure() // the sensor again. We still increment // _good_transfers, but don't return any data yet _register_wait--; - return; + return OK; } @@ -1851,7 +1928,7 @@ MPU6000::measure() /* * Report buffers. */ - accel_report arb; + accel_report arb; gyro_report grb; /* @@ -1982,6 +2059,7 @@ MPU6000::measure() /* stop measuring */ perf_end(_sample_perf); + return OK; } void @@ -2098,71 +2176,104 @@ MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) namespace mpu6000 { -MPU6000 *g_dev_int; // on internal bus -MPU6000 *g_dev_ext; // on external bus +/* + list of supported bus configurations + */ -void start(bool, enum Rotation, int range, int device_type); -void stop(bool); -void test(bool); -void reset(bool); -void info(bool); -void regdump(bool); -void testerror(bool); -void factorytest(bool); +struct mpu6000_bus_option { + enum MPU6000_BUS busid; + const char *accelpath; + const char *gyropath; + MPU6000_constructor interface_constructor; + uint8_t busnum; + 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 }, +# 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 }, +# 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 }, +#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 }, +#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 stop(enum MPU6000_BUS busid); +void test(enum MPU6000_BUS busid); +static struct mpu6000_bus_option &find_bus(enum MPU6000_BUS busid); +void reset(enum MPU6000_BUS busid); +void info(enum MPU6000_BUS busid); +void regdump(enum MPU6000_BUS busid); +void testerror(enum MPU6000_BUS busid); +void factorytest(enum MPU6000_BUS busid); void usage(); /** - * Start the driver. - * - * This function only returns if the driver is up and running - * or failed to detect the sensor. + * find a bus structure for a busid */ -void -start(bool external_bus, enum Rotation rotation, int range, int device_type) +struct mpu6000_bus_option &find_bus(enum MPU6000_BUS busid) { - int fd; - MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; - const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; - const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO; - - if (*g_dev_ptr != nullptr) - /* if already started, the still command succeeded */ - { - errx(0, "already started"); + for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { + if ((busid == MPU6000_BUS_ALL || + busid == bus_options[i].busid) && bus_options[i].dev != NULL) { + return bus_options[i]; + } } - /* create the driver */ - if (external_bus) { -#ifdef PX4_SPI_BUS_EXT -# 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 + errx(1, "bus %u not started", (unsigned)busid); +} - } else { -#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); +/** + * 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) +{ + int fd = -1; + + if (bus.dev != nullptr) { + warnx("%s SPI not available", external ? "External" : "Internal"); + return false; } - if (*g_dev_ptr == nullptr) { - goto fail; + device::Device *interface = bus.interface_constructor(bus.busnum, device_type, external); + + if (interface == nullptr) { + warnx("no device on bus %u", (unsigned)bus.busid); + return false; } - if (OK != (*g_dev_ptr)->init()) { + if (interface->init() != OK) { + delete interface; + warnx("no device on bus %u", (unsigned)bus.busid); + return false; + } + + bus.dev = new MPU6000(interface, bus.accelpath, bus.gyropath, rotation, device_type); + + if (bus.dev == nullptr) { + delete interface; + return false; + } + + if (OK != bus.dev->init()) { goto fail; } /* set the poll rate to default, starts automatic data collection */ - fd = open(path_accel, O_RDONLY); + + fd = open(bus.accelpath, O_RDONLY); if (fd < 0) { goto fail; @@ -2178,25 +2289,61 @@ start(bool external_bus, enum Rotation rotation, int range, int device_type) close(fd); - exit(0); + return true; + fail: - if (*g_dev_ptr != nullptr) { - delete(*g_dev_ptr); - *g_dev_ptr = nullptr; + if (fd >= 0) { + close(fd); } - errx(1, "no device on this bus"); + if (bus.dev != nullptr) { + delete(bus.dev); + bus.dev = nullptr; + } + + return false; +} + +/** + * Start the driver. + * + * This function only returns if the driver is up and running + * or failed to detect the sensor. + */ +void +start(enum MPU6000_BUS busid, enum Rotation rotation, int range, int device_type, bool external) +{ + + bool started = false; + + for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { + if (busid == MPU6000_BUS_ALL && bus_options[i].dev != NULL) { + // this device is already started + continue; + } + + if (busid != MPU6000_BUS_ALL && bus_options[i].busid != busid) { + // not the one that is asked for + continue; + } + + started |= start_bus(bus_options[i], rotation, range, device_type, external); + } + + exit(started ? 0 : 1); + } void -stop(bool external_bus) +stop(enum MPU6000_BUS busid) { - MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + struct mpu6000_bus_option &bus = find_bus(busid); - if (*g_dev_ptr != nullptr) { - delete *g_dev_ptr; - *g_dev_ptr = nullptr; + + if (bus.dev != nullptr) { + delete bus.dev; + bus.dev = nullptr; } else { /* warn, but not an error */ @@ -2212,26 +2359,25 @@ stop(bool external_bus) * and automatic modes. */ void -test(bool external_bus) +test(enum MPU6000_BUS busid) { - const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; - const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO; + struct mpu6000_bus_option &bus = find_bus(busid); accel_report a_report; gyro_report g_report; ssize_t sz; /* get the driver */ - int fd = open(path_accel, O_RDONLY); + int fd = open(bus.accelpath, O_RDONLY); - if (fd < 0) - err(1, "%s open failed (try 'mpu6000 start')", - path_accel); + if (fd < 0) { + err(1, "%s open failed (try 'mpu6000 start')", bus.accelpath); + } /* get the driver */ - int fd_gyro = open(path_gyro, O_RDONLY); + int fd_gyro = open(bus.gyropath, O_RDONLY); if (fd_gyro < 0) { - err(1, "%s open failed", path_gyro); + err(1, "%s open failed", bus.gyropath); } /* reset to manual polling */ @@ -2288,7 +2434,7 @@ test(bool external_bus) /* XXX add poll-rate tests here too */ - reset(external_bus); + reset(busid); errx(0, "PASS"); } @@ -2296,10 +2442,10 @@ test(bool external_bus) * Reset the driver. */ void -reset(bool external_bus) +reset(enum MPU6000_BUS busid) { - const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; - int fd = open(path_accel, O_RDONLY); + struct mpu6000_bus_option &bus = find_bus(busid); + int fd = open(bus.accelpath, O_RDONLY); if (fd < 0) { err(1, "failed "); @@ -2322,16 +2468,17 @@ reset(bool external_bus) * Print a little info about the driver. */ void -info(bool external_bus) +info(enum MPU6000_BUS busid) { - MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + struct mpu6000_bus_option &bus = find_bus(busid); - if (*g_dev_ptr == nullptr) { + + if (bus.dev == nullptr) { errx(1, "driver not running"); } - printf("state @ %p\n", *g_dev_ptr); - (*g_dev_ptr)->print_info(); + printf("state @ %p\n", bus.dev); + bus.dev->print_info(); exit(0); } @@ -2340,16 +2487,17 @@ info(bool external_bus) * Dump the register information */ void -regdump(bool external_bus) +regdump(enum MPU6000_BUS busid) { - MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + struct mpu6000_bus_option &bus = find_bus(busid); - if (*g_dev_ptr == nullptr) { + + if (bus.dev == nullptr) { errx(1, "driver not running"); } - printf("regdump @ %p\n", *g_dev_ptr); - (*g_dev_ptr)->print_registers(); + printf("regdump @ %p\n", bus.dev); + bus.dev->print_registers(); exit(0); } @@ -2358,15 +2506,16 @@ regdump(bool external_bus) * deliberately produce an error to test recovery */ void -testerror(bool external_bus) +testerror(enum MPU6000_BUS busid) { - MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + struct mpu6000_bus_option &bus = find_bus(busid); - if (*g_dev_ptr == nullptr) { + + if (bus.dev == nullptr) { errx(1, "driver not running"); } - (*g_dev_ptr)->test_error(); + bus.dev->test_error(); exit(0); } @@ -2375,15 +2524,16 @@ testerror(bool external_bus) * Dump the register information */ void -factorytest(bool external_bus) +factorytest(enum MPU6000_BUS busid) { - MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + struct mpu6000_bus_option &bus = find_bus(busid); - if (*g_dev_ptr == nullptr) { + + if (bus.dev == nullptr) { errx(1, "driver not running"); } - (*g_dev_ptr)->factory_self_test(); + bus.dev->factory_self_test(); exit(0); } @@ -2404,17 +2554,30 @@ usage() int mpu6000_main(int argc, char *argv[]) { - bool external_bus = false; + 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; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "T:XR:a:")) != EOF) { + while ((ch = getopt(argc, argv, "T:XISsR:a:")) != EOF) { switch (ch) { case 'X': - external_bus = true; + busid = MPU6000_BUS_I2C_EXTERNAL; + break; + + case 'I': + busid = MPU6000_BUS_I2C_INTERNAL; + break; + + case 'S': + busid = MPU6000_BUS_SPI_EXTERNAL; + break; + + case 's': + busid = MPU6000_BUS_SPI_INTERNAL; break; case 'T': @@ -2435,6 +2598,8 @@ mpu6000_main(int argc, char *argv[]) } } + external = (busid == MPU6000_BUS_I2C_EXTERNAL || busid == MPU6000_BUS_SPI_EXTERNAL); + const char *verb = argv[optind]; /* @@ -2442,47 +2607,47 @@ mpu6000_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) { - mpu6000::start(external_bus, rotation, accel_range, device_type); + mpu6000::start(busid, rotation, accel_range, device_type, external); } if (!strcmp(verb, "stop")) { - mpu6000::stop(external_bus); + mpu6000::stop(busid); } /* * Test the driver/device. */ if (!strcmp(verb, "test")) { - mpu6000::test(external_bus); + mpu6000::test(busid); } /* * Reset the driver. */ if (!strcmp(verb, "reset")) { - mpu6000::reset(external_bus); + mpu6000::reset(busid); } /* * Print driver information. */ - if (!strcmp(verb, "info")) { - mpu6000::info(external_bus); + if (!strcmp(verb, "info") || !strcmp(verb, "status")) { + mpu6000::info(busid); } /* * Print register information. */ if (!strcmp(verb, "regdump")) { - mpu6000::regdump(external_bus); + mpu6000::regdump(busid); } if (!strcmp(verb, "factorytest")) { - mpu6000::factorytest(external_bus); + mpu6000::factorytest(busid); } if (!strcmp(verb, "testerror")) { - mpu6000::testerror(external_bus); + mpu6000::testerror(busid); } mpu6000::usage(); diff --git a/src/drivers/mpu6000/mpu6000.h b/src/drivers/mpu6000/mpu6000.h new file mode 100644 index 0000000000..469e75023b --- /dev/null +++ b/src/drivers/mpu6000/mpu6000.h @@ -0,0 +1,228 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file .h + * + * Shared defines for the mpu6000 driver. + */ + +#pragma once + +#if defined(PX4_I2C_MPU6050_ADDR) || \ + defined(PX4_I2C_MPU6000_ADDR) || \ + defined(PX4_I2C_ICM_20608_G_ADDR) +# define USE_I2C +#endif + + +#define DIR_READ 0x80 +#define DIR_WRITE 0x00 + +#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel" +#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro" +#define MPU_DEVICE_PATH_ACCEL_EXT "/dev/mpu6000_accel_ext" +#define MPU_DEVICE_PATH_GYRO_EXT "/dev/mpu6000_gyro_ext" + +// MPU 6000 registers +#define MPUREG_WHOAMI 0x75 +#define MPUREG_SMPLRT_DIV 0x19 +#define MPUREG_CONFIG 0x1A +#define MPUREG_GYRO_CONFIG 0x1B +#define MPUREG_ACCEL_CONFIG 0x1C +#define MPUREG_FIFO_EN 0x23 +#define MPUREG_INT_PIN_CFG 0x37 +#define MPUREG_INT_ENABLE 0x38 +#define MPUREG_INT_STATUS 0x3A +#define MPUREG_ACCEL_XOUT_H 0x3B +#define MPUREG_ACCEL_XOUT_L 0x3C +#define MPUREG_ACCEL_YOUT_H 0x3D +#define MPUREG_ACCEL_YOUT_L 0x3E +#define MPUREG_ACCEL_ZOUT_H 0x3F +#define MPUREG_ACCEL_ZOUT_L 0x40 +#define MPUREG_TEMP_OUT_H 0x41 +#define MPUREG_TEMP_OUT_L 0x42 +#define MPUREG_GYRO_XOUT_H 0x43 +#define MPUREG_GYRO_XOUT_L 0x44 +#define MPUREG_GYRO_YOUT_H 0x45 +#define MPUREG_GYRO_YOUT_L 0x46 +#define MPUREG_GYRO_ZOUT_H 0x47 +#define MPUREG_GYRO_ZOUT_L 0x48 +#define MPUREG_USER_CTRL 0x6A +#define MPUREG_PWR_MGMT_1 0x6B +#define MPUREG_PWR_MGMT_2 0x6C +#define MPUREG_FIFO_COUNTH 0x72 +#define MPUREG_FIFO_COUNTL 0x73 +#define MPUREG_FIFO_R_W 0x74 +#define MPUREG_PRODUCT_ID 0x0C +#define MPUREG_TRIM1 0x0D +#define MPUREG_TRIM2 0x0E +#define MPUREG_TRIM3 0x0F +#define MPUREG_TRIM4 0x10 +#define MPU_GYRO_DLPF_CFG_256HZ_NOLPF2 0x00 +#define MPU_GYRO_DLPF_CFG_188HZ 0x01 +#define MPU_GYRO_DLPF_CFG_98HZ 0x02 +#define MPU_GYRO_DLPF_CFG_42HZ 0x03 +#define MPU_GYRO_DLPF_CFG_20HZ 0x04 +#define MPU_GYRO_DLPF_CFG_10HZ 0x05 +#define MPU_GYRO_DLPF_CFG_5HZ 0x06 +#define MPU_GYRO_DLPF_CFG_2100HZ_NOLPF 0x07 +#define MPU_DLPF_CFG_MASK 0x07 + +// Configuration bits MPU 3000 and MPU 6000 (not revised)? +#define BIT_SLEEP 0x40 +#define BIT_H_RESET 0x80 +#define BITS_CLKSEL 0x07 +#define MPU_CLK_SEL_PLLGYROX 0x01 +#define MPU_CLK_SEL_PLLGYROZ 0x03 +#define MPU_EXT_SYNC_GYROX 0x02 +#define BITS_GYRO_ST_X 0x80 +#define BITS_GYRO_ST_Y 0x40 +#define BITS_GYRO_ST_Z 0x20 +#define BITS_FS_250DPS 0x00 +#define BITS_FS_500DPS 0x08 +#define BITS_FS_1000DPS 0x10 +#define BITS_FS_2000DPS 0x18 +#define BITS_FS_MASK 0x18 +#define BIT_INT_ANYRD_2CLEAR 0x10 +#define BIT_RAW_RDY_EN 0x01 +#define BIT_I2C_IF_DIS 0x10 +#define BIT_INT_STATUS_DATA 0x01 + +#define MPU_WHOAMI_6000 0x68 +#define ICM_WHOAMI_20608 0xaf + +// ICM2608 specific registers + +#define ICMREG_ACCEL_CONFIG2 0x1D +#define ICM_ACC_DLPF_CFG_1046HZ_NOLPF 0x00 +#define ICM_ACC_DLPF_CFG_218HZ 0x01 +#define ICM_ACC_DLPF_CFG_99HZ 0x02 +#define ICM_ACC_DLPF_CFG_44HZ 0x03 +#define ICM_ACC_DLPF_CFG_21HZ 0x04 +#define ICM_ACC_DLPF_CFG_10HZ 0x05 +#define ICM_ACC_DLPF_CFG_5HZ 0x06 +#define ICM_ACC_DLPF_CFG_420HZ 0x07 +/* 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 +// Product Name Product Revision +#define MPU6000ES_REV_C4 0x14 +#define MPU6000ES_REV_C5 0x15 +#define MPU6000ES_REV_D6 0x16 +#define MPU6000ES_REV_D7 0x17 +#define MPU6000ES_REV_D8 0x18 +#define MPU6000_REV_C4 0x54 +#define MPU6000_REV_C5 0x55 +#define MPU6000_REV_D6 0x56 +#define MPU6000_REV_D7 0x57 +#define MPU6000_REV_D8 0x58 +#define MPU6000_REV_D9 0x59 +#define MPU6000_REV_D10 0x5A +#define MPU6050_REV_D8 0x28 // TODO:Need verification + +#define MPU6000_ACCEL_DEFAULT_RANGE_G 8 +#define MPU6000_ACCEL_DEFAULT_RATE 1000 +#define MPU6000_ACCEL_MAX_OUTPUT_RATE 280 +#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MPU6000_GYRO_DEFAULT_RANGE_G 8 +#define MPU6000_GYRO_DEFAULT_RATE 1000 +/* rates need to be the same between accel and gyro */ +#define MPU6000_GYRO_MAX_OUTPUT_RATE MPU6000_ACCEL_MAX_OUTPUT_RATE +#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42 + +#define MPU6000_ONE_G 9.80665f + +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + +#define MPUIOCGIS_I2C (unsigned)(DEVIOCGDEVICEID+100) + +#pragma pack(push, 1) +/** + * Report conversation within the MPU6000, including command byte and + * interrupt status. + */ +struct MPUReport { + uint8_t cmd; + uint8_t status; + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t temp[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; +}; +#pragma pack(pop) + +#define MPU_MAX_READ_BUFFER_SIZE (sizeof(MPUReport) + 1) +#define MPU_MAX_WRITE_BUFFER_SIZE (2) +/* + The MPU6000 can only handle high bus speeds on the sensor and + interrupt status registers. All other registers have a maximum 1MHz + Communication with all registers of the device is performed using either + I2C at 400kHz or SPI at 1MHz. For applications requiring faster communications, + the sensor and interrupt registers may be read using SPI at 20MHz + */ +#define MPU6000_LOW_BUS_SPEED 0 +#define MPU6000_HIGH_BUS_SPEED 0x8000 +# define MPU6000_IS_HIGH_SPEED(r) ((r) & MPU6000_HIGH_BUS_SPEED) +# define MPU6000_REG(r) ((r) &~MPU6000_HIGH_BUS_SPEED) +# define MPU6000_SET_SPEED(r, s) ((r)|(s)) +# define MPU6000_HIGH_SPEED_OP(r) MPU6000_SET_SPEED((r), MPU6000_HIGH_BUS_SPEED) +# define MPU6000_LOW_SPEED_OP(r) MPU6000_REG((r)) + +/* interface factories */ +extern device::Device *MPU6000_SPI_interface(int bus, int device_type, bool external_bus); +extern device::Device *MPU6000_I2C_interface(int bus, int device_type, bool external_bus); +extern int MPU6000_probe(device::Device *dev, int device_type); + +typedef device::Device *(*MPU6000_constructor)(int, int, bool); diff --git a/src/drivers/mpu6000/mpu6000_i2c.cpp b/src/drivers/mpu6000/mpu6000_i2c.cpp new file mode 100644 index 0000000000..884e521296 --- /dev/null +++ b/src/drivers/mpu6000/mpu6000_i2c.cpp @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mpu6000_i2c.cpp + * + * I2C interface for MPU6000 /MPU6050 + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "mpu6000.h" + +#include "board_config.h" + +#ifdef USE_I2C + +device::Device *MPU6000_I2C_interface(int bus, int device_type, bool external_bus); + +class MPU6000_I2C : public device::I2C +{ +public: + MPU6000_I2C(int bus, int device_type); + virtual ~MPU6000_I2C(); + + virtual int init(); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + + virtual int ioctl(unsigned operation, unsigned &arg); + +protected: + virtual int probe(); + +private: + int _device_type; + +}; + + +device::Device * +MPU6000_I2C_interface(int bus, int device_type, bool external_bus) +{ + return new MPU6000_I2C(bus, device_type); +} + +MPU6000_I2C::MPU6000_I2C(int bus, int device_type) : + I2C("MPU6000_I2C", nullptr, bus, PX4_I2C_MPU6050_ADDR, 400000), + _device_type(device_type) +{ + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000; +} + +MPU6000_I2C::~MPU6000_I2C() +{ +} + +int +MPU6000_I2C::init() +{ + /* this will call probe() */ + return I2C::init(); +} + +int +MPU6000_I2C::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + + case ACCELIOCGEXTERNAL: + return _bus == PX4_I2C_BUS_EXPANSION ? 1 : 0; + + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + + case MPUIOCGIS_I2C: + return 1; + + default: + ret = -EINVAL; + } + + return ret; +} + +int +MPU6000_I2C::write(unsigned reg_speed, void *data, unsigned count) +{ + uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE]; + + if (sizeof(cmd) < (count + 1)) { + return -EIO; + } + + cmd[0] = MPU6000_REG(reg_speed); + cmd[1] = *(uint8_t *)data; + return transfer(&cmd[0], count + 1, nullptr, 0); +} + +int +MPU6000_I2C::read(unsigned reg_speed, void *data, unsigned count) +{ + /* We want to avoid copying the data of MPUReport: So if the caller + * supplies a buffer not MPUReport in size, it is assume to be a reg or + * reg 16 read + * Since MPUReport has a cmd at front, we must return the data + * after that. Foe anthing else we must return it + */ + uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, status); + uint8_t cmd = MPU6000_REG(reg_speed); + int ret = transfer(&cmd, 1, &((uint8_t *)data)[offset], count); + return ret == OK ? count : ret; +} + + +int +MPU6000_I2C::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; + +} +#endif /* PX4_I2C_OBDEV_HMC5883 */ diff --git a/src/drivers/mpu6000/mpu6000_spi.cpp b/src/drivers/mpu6000/mpu6000_spi.cpp new file mode 100644 index 0000000000..428e232d4c --- /dev/null +++ b/src/drivers/mpu6000/mpu6000_spi.cpp @@ -0,0 +1,281 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mpu6000_spi.cpp + * + * Driver for the Invensense MPU6000 connected via SPI. + * + * @author Andrew Tridgell + * @author Pat Hickey + * @author David sidrane + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "mpu6000.h" +#include + +#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 + +/* + The MPU6000 can only handle high SPI bus speeds on the sensor and + interrupt status registers. All other registers have a maximum 1MHz + SPI speed + + */ +#define MPU6000_LOW_SPI_BUS_SPEED 1000*1000 +#define MPU6000_HIGH_SPI_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */ + + +device::Device *MPU6000_SPI_interface(int bus, int device_type, bool external_bus); + + +class MPU6000_SPI : public device::SPI +{ +public: + MPU6000_SPI(int bus, spi_dev_e device, int device_type); + virtual ~MPU6000_SPI(); + + virtual int init(); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + + virtual int ioctl(unsigned operation, unsigned &arg); +protected: + virtual int probe(); + +private: + + int _device_type; + /* Helper to set the desired speed and isolate the register on return */ + + void set_bus_frequency(unsigned ®_speed_reg_out); +}; + +device::Device * +MPU6000_SPI_interface(int bus, int device_type, bool external_bus) +{ + spi_dev_e 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; +# endif +#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; +#endif + } + + if (cs != SPIDEV_NONE) { + + interface = new MPU6000_SPI(bus, cs, device_type); + } + + return interface; +} + +MPU6000_SPI::MPU6000_SPI(int bus, spi_dev_e device, int device_type) : + SPI("MPU6000", nullptr, bus, device, SPIDEV_MODE3, MPU6000_LOW_SPI_BUS_SPEED), + _device_type(device_type) +{ + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000; +} + +MPU6000_SPI::~MPU6000_SPI() +{ +} + +int +MPU6000_SPI::init() +{ + int ret; + + ret = SPI::init(); + + if (ret != OK) { + DEVICE_DEBUG("SPI init failed"); + return -EIO; + } + + return OK; +} + +int +MPU6000_SPI::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + + case ACCELIOCGEXTERNAL: +#if defined(PX4_SPI_BUS_EXT) + return _bus == PX4_SPI_BUS_EXT ? 1 : 0; +#else + return 0; +#endif + + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + + case MPUIOCGIS_I2C: + return 0; + + default: { + ret = -EINVAL; + } + } + + return ret; +} + +void +MPU6000_SPI::set_bus_frequency(unsigned ®_speed) +{ + /* Set the desired speed */ + + set_frequency(MPU6000_IS_HIGH_SPEED(reg_speed) ? MPU6000_HIGH_SPI_BUS_SPEED : MPU6000_LOW_SPI_BUS_SPEED); + + /* Isoolate the register on return */ + + reg_speed = MPU6000_REG(reg_speed); +} + + +int +MPU6000_SPI::write(unsigned reg_speed, void *data, unsigned count) +{ + uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE]; + + if (sizeof(cmd) < (count + 1)) { + return -EIO; + } + + /* Set the desired speed and isolate the register */ + + set_bus_frequency(reg_speed); + + cmd[0] = reg_speed | DIR_WRITE; + cmd[1] = *(uint8_t *)data; + + return transfer(&cmd[0], &cmd[0], count + 1); +} + +int +MPU6000_SPI::read(unsigned reg_speed, void *data, unsigned count) +{ + /* We want to avoid copying the data of MPUReport: So if the caller + * supplies a buffer not MPUReport in size, it is assume to be a reg or reg 16 read + * and we need to provied the buffer large enough for the callers data + * and our command. + */ + uint8_t cmd[3] = {0, 0, 0}; + + uint8_t *pbuff = count < sizeof(MPUReport) ? cmd : (uint8_t *) data ; + + + if (count < sizeof(MPUReport)) { + + /* add command */ + + count++; + } + + set_bus_frequency(reg_speed); + + /* Set command */ + + pbuff[0] = reg_speed | DIR_READ ; + + /* Transfer the command and get the data */ + + int ret = transfer(pbuff, pbuff, count); + + if (ret == OK && pbuff == &cmd[0]) { + + /* Adjust the count back */ + + count--; + + /* Return the data */ + + memcpy(data, &cmd[1], count); + + } + + return ret == OK ? count : ret; +} + +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; + +} + +#endif // PX4_SPIDEV_MPU