From e8ae0fe13ca84896f9adaed91d4a121ac2d113ad Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 18 Jul 2016 17:49:10 -1000 Subject: [PATCH] Made the MPU6000 driver a highbread using both hrt for SPI or workqueue for I2C --- ROMFS/tap_common/init.d/rc.sensors | 8 +- cmake/configs/nuttx_tap-v1_default.cmake | 2 +- src/drivers/boards/tap-v1/board_config.h | 6 +- src/drivers/mpu6000/CMakeLists.txt | 2 +- src/drivers/mpu6000/mpu6000.cpp | 201 ++++++++++++++++++++--- src/drivers/mpu6000/mpu6000.h | 2 + src/drivers/mpu6000/mpu6000_i2c.cpp | 3 + src/drivers/mpu6000/mpu6000_spi.cpp | 3 + 8 files changed, 194 insertions(+), 33 deletions(-) diff --git a/ROMFS/tap_common/init.d/rc.sensors b/ROMFS/tap_common/init.d/rc.sensors index 4e767bdc6c..9f85bf3c2f 100644 --- a/ROMFS/tap_common/init.d/rc.sensors +++ b/ROMFS/tap_common/init.d/rc.sensors @@ -7,6 +7,10 @@ if adc start then fi +if ms5611 -T 5607 -I start +then +fi + # External I2C bus if hmc5883 -C -T -X start then @@ -21,8 +25,8 @@ if hmc5883 -C -T -S -R 2 start then fi -# Internal SPI bus ICM-20608-G is rotated 90 deg yaw -if mpu6000 -R 2 -T 20608 start +# Internal I2C bus Rotation TBD +if mpu6000 -I -T 6000 start then fi diff --git a/cmake/configs/nuttx_tap-v1_default.cmake b/cmake/configs/nuttx_tap-v1_default.cmake index 2dad824901..ab88c20870 100644 --- a/cmake/configs/nuttx_tap-v1_default.cmake +++ b/cmake/configs/nuttx_tap-v1_default.cmake @@ -15,7 +15,7 @@ set(config_module_list drivers/boards/tap-v1 drivers/rgbled_pwm drivers/tap_esc - #drivers/mpu6500 + drivers/mpu6000 drivers/ms5611 drivers/hmc5883 drivers/gps diff --git a/src/drivers/boards/tap-v1/board_config.h b/src/drivers/boards/tap-v1/board_config.h index f1f5ac31d8..de8ac79c8f 100644 --- a/src/drivers/boards/tap-v1/board_config.h +++ b/src/drivers/boards/tap-v1/board_config.h @@ -114,8 +114,7 @@ __BEGIN_DECLS * Note that these are unshifted addresses (not includinf R/W). */ -/* todo: - * Cannot tell from the schematic if there is one or 2 MPU6050 +/* * The slave address of the MPU-60X0 is b110100X which is 7 bits long. * The LSB bit of the 7 bit address is determined by the logic level * on pin AD0. This allows two MPU-60X0s to be connected to the same I2C bus. @@ -123,8 +122,7 @@ __BEGIN_DECLS * should be b1101000 (pin AD0 is logic low) and the address of the other * should be b1101001 (pin AD0 is logic high). */ -#define PX4_I2C_ON_BOARD_MPU6050_ADDRS {0x68,0x69} - +#define PX4_I2C_MPU6050_ADDR 0x68 /* * ADC channels diff --git a/src/drivers/mpu6000/CMakeLists.txt b/src/drivers/mpu6000/CMakeLists.txt index 0a8295b168..290ff79917 100644 --- a/src/drivers/mpu6000/CMakeLists.txt +++ b/src/drivers/mpu6000/CMakeLists.txt @@ -33,7 +33,7 @@ px4_add_module( MODULE drivers__mpu6000 MAIN mpu6000 - STACK_MAIN 1300 + STACK_MAIN 1400 COMPILE_FLAGS -Weffc++ -Os diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 50fc1e22e9..5a42fac6c5 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 @@ -63,6 +78,7 @@ #include #include +#include #include #include @@ -85,6 +101,9 @@ worst case timing jitter due to other timers */ #define MPU6000_TIMER_REDUCTION 200 +#define MPU6000_CONVERSION_INTERVAL 10000 //todo:This is set long for the moment +// As the bus is running at 100KHX and the +// Transaction time is 2.163Ms enum MPU6000_BUS { MPU6000_BUS_ALL = 0, @@ -140,6 +159,16 @@ 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 + */ + bool _use_hrt; + work_s _work; +#endif + + struct hrt_call _call; unsigned _call_interval; @@ -229,6 +258,39 @@ private: */ bool is_mpu_device() { return _device_type == 6000;} + +#if defined(USE_I2C) + /** + * When the I2C interfase is cho + * 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); + + bool is_i2c(void) { return !_use_hrt; } + + void use_i2c(bool on_true) { _use_hrt = !on_true; } + +#endif + + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -243,7 +305,7 @@ private: /** * Fetch measurements from the sensor and update the report buffers. */ - void measure(); + int measure(); /** * Read a register from the MPU6000 @@ -406,7 +468,11 @@ MPU6000::MPU6000(device::Device *interface, const char *path_accel, const char * _device_type(device_type), _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), - _call{}, +#if defined(USE_I2C) + _use_hrt(false), + _work{}, +#endif + _call {}, _call_interval(0), _accel_reports(nullptr), _accel_scale{}, @@ -446,6 +512,10 @@ MPU6000::MPU6000(device::Device *interface, const char *path_accel, const char * _last_accel{}, _got_duplicate(false) { +#if defined(USE_I2C) + // work_cancel in stop_cycle called from the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +#endif // disable debug() calls _debug_enabled = false; @@ -509,7 +579,14 @@ MPU6000::~MPU6000() int MPU6000::init() { - /* probe again to get our settings */ + +#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(); @@ -531,6 +608,7 @@ MPU6000::init() return ret; } + ret = -ENOMEM; /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); @@ -544,6 +622,8 @@ MPU6000::init() goto out; } + ret = -EIO; + if (reset() != OK) { goto out; } @@ -626,8 +706,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 defined(USE_I2C) + + if (!is_i2c()) +#endif + { + // 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) { @@ -1218,7 +1305,13 @@ 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 defined(USE_I2C) + + if (!is_i2c()) +#endif + { + _call.period = _call_interval - MPU6000_TIMER_REDUCTION; + } /* if we need to start the poll state machine, do it */ if (want_start) { @@ -1521,18 +1614,40 @@ 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 defined(USE_I2C) + + if (_use_hrt) { +#endif + /* start polling at the specified rate */ + hrt_call_every(&_call, + 1000, + _call_interval - MPU6000_TIMER_REDUCTION, + (hrt_callout)&MPU6000::measure_trampoline, this); +#if defined(USE_I2C) + + } else { + /* 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 defined(USE_I2C) + if (_use_hrt) { +#endif + hrt_cancel(&_call); +#if defined(USE_I2C) + + } else { + work_cancel(HPWORK, &_work); + } + +#endif /* reset internal states */ memset(_last_accel, 0, sizeof(_last_accel)); @@ -1541,6 +1656,43 @@ 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(MPU6000_CONVERSION_INTERVAL)); +//todo:This is set long for the moment +// As the bus is running at 100KHX and the +// Transaction time is 2.163Ms +// It should be USEC2TICK(_call_interval - MPU6000_TIMER_REDUCTION)); +// When I get the bus running at 400Khz + } +} +#endif + void MPU6000::measure_trampoline(void *arg) { @@ -1549,7 +1701,6 @@ MPU6000::measure_trampoline(void *arg) /* make another measurement */ dev->measure(); } - void MPU6000::check_registers(void) { @@ -1611,17 +1762,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; @@ -1648,7 +1799,7 @@ MPU6000::measure() if (sizeof(mpu_report) != _interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_HIGH_BUS_SPEED), (uint8_t *)&mpu_report, sizeof(mpu_report))) { - return; + return -EIO; } check_registers(); @@ -1666,7 +1817,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); @@ -1700,7 +1851,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); @@ -1710,7 +1861,7 @@ MPU6000::measure() // the sensor again. We still increment // _good_transfers, but don't return any data yet _register_wait--; - return; + return OK; } @@ -1734,7 +1885,7 @@ MPU6000::measure() /* * Report buffers. */ - accel_report arb; + accel_report arb; gyro_report grb; /* @@ -1865,6 +2016,7 @@ MPU6000::measure() /* stop measuring */ perf_end(_sample_perf); + return OK; } void @@ -2054,6 +2206,7 @@ start_bus(struct mpu6000_bus_option &bus, enum Rotation rotation, int range, int 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; @@ -2136,9 +2289,7 @@ start(enum MPU6000_BUS busid, enum Rotation rotation, int range, int device_type started |= start_bus(bus_options[i], rotation, range, device_type, external); } - if (!started) { - exit(1); - } + exit(started ? 0 : 1); } diff --git a/src/drivers/mpu6000/mpu6000.h b/src/drivers/mpu6000/mpu6000.h index 70c01ed1cb..f12a720eb1 100644 --- a/src/drivers/mpu6000/mpu6000.h +++ b/src/drivers/mpu6000/mpu6000.h @@ -174,6 +174,8 @@ #define EXTERNAL_BUS 0 #endif +#define MPUIOCGIS_I2C (unsigned)(DEVIOCGDEVICEID+100) + #pragma pack(push, 1) /** * Report conversation within the MPU6000, including command byte and diff --git a/src/drivers/mpu6000/mpu6000_i2c.cpp b/src/drivers/mpu6000/mpu6000_i2c.cpp index ef84973af8..884e521296 100644 --- a/src/drivers/mpu6000/mpu6000_i2c.cpp +++ b/src/drivers/mpu6000/mpu6000_i2c.cpp @@ -121,6 +121,9 @@ MPU6000_I2C::ioctl(unsigned operation, unsigned &arg) case DEVIOCGDEVICEID: return CDev::ioctl(nullptr, operation, arg); + case MPUIOCGIS_I2C: + return 1; + default: ret = -EINVAL; } diff --git a/src/drivers/mpu6000/mpu6000_spi.cpp b/src/drivers/mpu6000/mpu6000_spi.cpp index 39595f2f66..428e232d4c 100644 --- a/src/drivers/mpu6000/mpu6000_spi.cpp +++ b/src/drivers/mpu6000/mpu6000_spi.cpp @@ -181,6 +181,9 @@ MPU6000_SPI::ioctl(unsigned operation, unsigned &arg) case DEVIOCGDEVICEID: return CDev::ioctl(nullptr, operation, arg); + case MPUIOCGIS_I2C: + return 0; + default: { ret = -EINVAL; }