diff --git a/ROMFS/px4fmu_common/init.d/4900_crazyflie b/ROMFS/px4fmu_common/init.d/4900_crazyflie index 153c6d2134..9fe3ea47d5 100644 --- a/ROMFS/px4fmu_common/init.d/4900_crazyflie +++ b/ROMFS/px4fmu_common/init.d/4900_crazyflie @@ -10,5 +10,12 @@ sh /etc/init.d/4001_quad_x +if [ $AUTOCNF == yes ] +then + param set COM_RC_IN_MODE 2 + param set BAT_N_CELLS 1 + param set BAT_CAPACITY 240 + param set BAT_SOURCE 1 +fi + set OUTPUT_MODE crazyflie -set USE_IO no diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 3ec0cd0d95..d809b8146b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -109,6 +109,11 @@ then set MIXER_AUX none fi +if ver hwcmp CRAZYFLIE +then + set MIXER_AUX none +fi + if [ $MIXER_AUX != none -a $AUX_MODE != none ] then # diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index c6e00f8614..1c0c59537c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -8,12 +8,20 @@ then if ms5611 start then fi + else + if ver hwcmp CRAZYFLIE + then + # Crazyflie uses separate driver + else + # Configure all I2C buses to 100 KHz as they # are all external or slow fmu i2c 1 100000 fmu i2c 2 100000 + fi + if ver hwcmp PX4FMU_V4 then # We know there are sketchy boards out there @@ -198,6 +206,19 @@ then fi fi +if ver hwcmp CRAZYFLIE +then + # Onboard I2C + if mpu9250 -R 12 start + then + fi + + # I2C bypass of mpu + if lps25h start + then + fi +fi + if meas_airspeed start then else diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 47a23e70d1..8adcb853a2 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -164,10 +164,16 @@ then then set USE_IO no fi + + if ver hwcmp CRAZYFLIE + then + set USE_IO no + fi else set USE_IO no fi + # should set to 0.8 for mindpx-v2 borad. if param compare INAV_LIDAR_ERR 0.5 then @@ -675,6 +681,15 @@ then ardrone_interface start -d /dev/ttyS1 fi + + # + # Start Crazyflie driver + # + if ver hwcmp CRAZYFLIE + then + crazyflie start + fi + # # Fixed wing setup # diff --git a/cmake/configs/nuttx_crazyflie_default.cmake b/cmake/configs/nuttx_crazyflie_default.cmake index a31b27aa47..844260c4cf 100644 --- a/cmake/configs/nuttx_crazyflie_default.cmake +++ b/cmake/configs/nuttx_crazyflie_default.cmake @@ -11,7 +11,7 @@ set(config_module_list drivers/led drivers/boards/crazyflie drivers/crazyflie - #drivers/mpu9250 + drivers/mpu9250 #drivers/ak8963 drivers/lps25h drivers/gps diff --git a/src/drivers/mpu9250/CMakeLists.txt b/src/drivers/mpu9250/CMakeLists.txt index 2616a19177..98fc65a551 100644 --- a/src/drivers/mpu9250/CMakeLists.txt +++ b/src/drivers/mpu9250/CMakeLists.txt @@ -44,6 +44,7 @@ px4_add_module( main.cpp gyro.cpp mag.cpp + mag_i2c.cpp DEPENDS platforms__common ) diff --git a/src/drivers/mpu9250/gyro.h b/src/drivers/mpu9250/gyro.h index b76b0f81bc..d13304a3ed 100644 --- a/src/drivers/mpu9250/gyro.h +++ b/src/drivers/mpu9250/gyro.h @@ -31,6 +31,8 @@ * ****************************************************************************/ +#pragma once + class MPU9250; /** diff --git a/src/drivers/mpu9250/mag.cpp b/src/drivers/mpu9250/mag.cpp index d46efffeb0..fec1df32e5 100644 --- a/src/drivers/mpu9250/mag.cpp +++ b/src/drivers/mpu9250/mag.cpp @@ -149,8 +149,11 @@ #define AK8963_RESET 0x01 -MPU9250_mag::MPU9250_mag(MPU9250 *parent, const char *path) : +// If interface is non-null, then it will used for interacting with the device. +// Otherwise, it will passthrough the parent MPU9250 +MPU9250_mag::MPU9250_mag(MPU9250 *parent, device::Device *interface, const char *path) : CDev("MPU9250_mag", path), + _interface(interface), _parent(parent), _mag_topic(nullptr), _mag_orb_class_instance(-1), @@ -249,7 +252,16 @@ bool MPU9250_mag::check_duplicate(uint8_t *mag_data) } void -MPU9250_mag::measure(struct ak8963_regs data) +MPU9250_mag::measure() +{ + struct ak8963_regs data; + if(OK == _interface->read(AK8963REG_ST1, &data, sizeof(struct ak8963_regs))){ + _measure(data); + } +} + +void +MPU9250_mag::_measure(struct ak8963_regs data) { bool mag_notify = true; @@ -508,12 +520,7 @@ MPU9250_mag::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out) void MPU9250_mag::read_block(uint8_t reg, uint8_t *val, uint8_t count) { - uint8_t addr = reg | 0x80; - uint8_t tx[32] = { addr, }; - uint8_t rx[32]; - - _parent->transfer(tx, rx, count + 1); - memcpy(val, rx + 1, count); + _parent->_interface->read(reg, val, count); } void @@ -525,12 +532,26 @@ MPU9250_mag::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size) _parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new reads } +uint8_t +MPU9250_mag::read_reg(unsigned int reg) +{ + uint8_t buf; + if (_interface == nullptr) { + passthrough_read(reg, &buf, 0x01); + } + else { + _interface->read(reg, &buf, 1); + } + return buf; +} + + bool MPU9250_mag::ak8963_check_id(void) { for (int i = 0; i < 5; i++) { - uint8_t deviceid = 0; - passthrough_read(AK8963REG_WIA, &deviceid, 0x01); + + uint8_t deviceid = read_reg(AK8963REG_WIA); if (AK8963_DEVICE_ID == deviceid) { return true; @@ -551,10 +572,27 @@ MPU9250_mag::passthrough_write(uint8_t reg, uint8_t val) _parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new writes } + + +void +MPU9250_mag::write_reg(unsigned reg, uint8_t value) +{ + // general register transfer at low clock speed + if (_interface == nullptr) { + passthrough_write(reg, value); + } + else { + _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1); + } +} + + + + void MPU9250_mag::ak8963_reset(void) { - passthrough_write(AK8963REG_CNTL2, AK8963_RESET); + write_reg(AK8963REG_CNTL2, AK8963_RESET); } bool @@ -563,10 +601,15 @@ MPU9250_mag::ak8963_read_adjustments(void) uint8_t response[3]; float ak8963_ASA[3]; - passthrough_write(AK8963REG_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC); + write_reg(AK8963REG_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC); usleep(50); - passthrough_read(AK8963REG_ASAX, response, 3); - passthrough_write(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE); + if (_interface != nullptr) { + _interface->read(AK8963REG_ASAX, response, 3); + } + else { + passthrough_read(AK8963REG_ASAX, response, 3); + } + write_reg(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE); for (int i = 0; i < 3; i++) { if (0 != response[i] && 0xff != response[i]) { @@ -589,10 +632,18 @@ MPU9250_mag::ak8963_setup(void) { int retries = 10; - // enable the I2C master to slaves on the aux bus - uint8_t user_ctrl = _parent->read_reg(MPUREG_USER_CTRL); - _parent->write_checked_reg(MPUREG_USER_CTRL, user_ctrl | BIT_I2C_MST_EN); - _parent->write_reg(MPUREG_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | BIT_I2C_MST_WAIT_FOR_ES | BITS_I2C_MST_CLOCK_400HZ); + /* Configures the parent to act in master mode */ + if(_interface == nullptr) { + uint8_t user_ctrl = _parent->read_reg(MPUREG_USER_CTRL); + _parent->write_checked_reg(MPUREG_USER_CTRL, user_ctrl | BIT_I2C_MST_EN); + _parent->write_reg(MPUREG_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | BIT_I2C_MST_WAIT_FOR_ES | BITS_I2C_MST_CLOCK_400HZ); + } + else { + // uint8_t user_ctrl = _parent->read_reg(MPUREG_USER_CTRL); + + // // Passthrough mode + // _parent->write_checked_reg(MPUREG_USER_CTRL, user_ctrl & (~BIT_I2C_MST_EN)); + } if (!ak8963_check_id()) { ::printf("AK8963: bad id\n"); @@ -605,9 +656,13 @@ MPU9250_mag::ak8963_setup(void) } } - passthrough_write(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC); + write_reg(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC); - set_passthrough(AK8963REG_ST1, sizeof(struct ak8963_regs)); + + if(_interface == NULL) { + /* Configure mpu to internally read ak8963 data */ + set_passthrough(AK8963REG_ST1, sizeof(struct ak8963_regs)); + } return true; } diff --git a/src/drivers/mpu9250/mag.h b/src/drivers/mpu9250/mag.h index 9eb9de6e5e..7f5e66400c 100644 --- a/src/drivers/mpu9250/mag.h +++ b/src/drivers/mpu9250/mag.h @@ -31,6 +31,8 @@ * ****************************************************************************/ +#pragma once + class MPU9250; #pragma pack(push, 1) @@ -43,13 +45,18 @@ struct ak8963_regs { }; #pragma pack(pop) +extern device::Device *AK8963_I2C_interface(int bus, bool external_bus); + +typedef device::Device *(*MPU9250_mag_constructor)(int, bool); + + /** * Helper class implementing the magnetometer driver node. */ class MPU9250_mag : public device::CDev { public: - MPU9250_mag(MPU9250 *parent, const char *path); + MPU9250_mag(MPU9250 *parent, device::Device *interface, const char *path); ~MPU9250_mag(); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); @@ -67,9 +74,24 @@ public: bool ak8963_read_adjustments(void); protected: + Device *_interface; + friend class MPU9250; - void measure(struct ak8963_regs data); + /* Directly measure from the _interface if possible */ + void measure(); + + /* Update the state with prefetched data (internally called by the regular measure() )*/ + void _measure(struct ak8963_regs data); + + + uint8_t read_reg(unsigned reg); + void write_reg(unsigned reg, uint8_t value); + + + bool is_passthrough() { return _interface == nullptr; } + + int self_test(void); private: diff --git a/src/drivers/mpu9250/mag_i2c.cpp b/src/drivers/mpu9250/mag_i2c.cpp new file mode 100644 index 0000000000..b7bb414c84 --- /dev/null +++ b/src/drivers/mpu9250/mag_i2c.cpp @@ -0,0 +1,179 @@ +/**************************************************************************** + * + * 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 mag_i2c.cpp + * + * I2C interface for AK8963 + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "MPU9250.h" +#include "mag.h" + +#include "board_config.h" + + +// TODO: This is redundant with mag.cpp +#define AK8963_I2C_ADDR 0x0C +#define ADDR_ID 0x00 + +#define ID_WHO_AM_I 0x48 + + + +#ifdef USE_I2C + +device::Device *AK8963_I2C_interface(int bus, bool external_bus); + +class AK8963_I2C : public device::I2C +{ +public: + AK8963_I2C(int bus); + virtual ~AK8963_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(); + +}; + + +device::Device * +AK8963_I2C_interface(int bus, bool external_bus) +{ + return new AK8963_I2C(bus); +} + +AK8963_I2C::AK8963_I2C(int bus) : + I2C("AK8963_I2C", nullptr, bus, AK8963_I2C_ADDR, 400000) +{ + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250; +} + +AK8963_I2C::~AK8963_I2C() +{ +} + +int +AK8963_I2C::init() +{ + /* this will call probe() */ + return I2C::init(); +} + +int +AK8963_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 +AK8963_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] = MPU9250_REG(reg_speed); + cmd[1] = *(uint8_t *)data; + return transfer(&cmd[0], count + 1, nullptr, 0); +} + +int +AK8963_I2C::read(unsigned reg_speed, void *data, unsigned count) +{ + uint8_t cmd = MPU9250_REG(reg_speed); + return transfer(&cmd, 1, (uint8_t *)data, count); +} + + +int +AK8963_I2C::probe() +{ + uint8_t whoami = 0; + uint8_t expected = ID_WHO_AM_I; + + if (read(ADDR_ID, &whoami, 1)) { + return -EIO; + } + + if (whoami != expected) { + return -EIO; + } + + return OK; +} + +#endif diff --git a/src/drivers/mpu9250/main.cpp b/src/drivers/mpu9250/main.cpp index 01542e3989..94f9f850b5 100644 --- a/src/drivers/mpu9250/main.cpp +++ b/src/drivers/mpu9250/main.cpp @@ -34,7 +34,7 @@ /** * @file main.cpp * - * Driver for the Invensense mpu9250 connected via SPI. + * Driver for the Invensense mpu9250 connected via I2C or SPI. * * @authors Andrew Tridgell * Robert Dickenson @@ -83,11 +83,6 @@ /** driver 'main' command */ extern "C" { __EXPORT int mpu9250_main(int argc, char *argv[]); } -/** - * Local functions in support of the shell command. - */ -namespace mpu9250 -{ enum MPU9250_BUS { MPU9250_BUS_ALL = 0, @@ -97,6 +92,13 @@ enum MPU9250_BUS { MPU9250_BUS_SPI_EXTERNAL }; + +/** + * Local functions in support of the shell command. + */ +namespace mpu9250 +{ + /* list of supported bus configurations */ @@ -107,30 +109,31 @@ struct mpu9250_bus_option { const char *gyropath; const char *magpath; MPU9250_constructor interface_constructor; + bool magpassthrough; uint8_t busnum; - MPU6000 *dev; + MPU9250 *dev; } bus_options[] = { #if defined (USE_I2C) # if defined(PX4_I2C_BUS_ONBOARD) - { MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, + { MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_I2C_interface, false, PX4_I2C_BUS_ONBOARD, NULL }, # endif # if defined(PX4_I2C_BUS_EXPANSION) - { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, + { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, NULL }, # endif #endif #ifdef PX4_SPIDEV_MPU - { MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_SPI_interface, PX4_SPI_BUS_SENSORS, NULL }, + { MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, NULL }, #endif #if defined(PX4_SPI_BUS_EXT) - { MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT &MPU9250_SPI_interface, PX4_SPI_BUS_EXT, NULL }, + { MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT &MPU9250_SPI_interface, true, PX4_SPI_BUS_EXT, NULL }, #endif }; #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) -void start(enum MPU9250_BUS busid, enum Rotation rotation); -bool start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation); +void start(enum MPU9250_BUS busid, enum Rotation rotation, bool external_bus); +bool start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external_bus); struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid); void stop(enum MPU9250_BUS busid); void test(enum MPU9250_BUS busid); @@ -159,7 +162,7 @@ struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid) * start driver for a specific bus option */ bool -start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, int range, int device_type, bool external) +start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external) { int fd = -1; @@ -168,7 +171,7 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, int range, int return false; } - device::Device *interface = bus.interface_constructor(bus.busnum, device_type, external); + device::Device *interface = bus.interface_constructor(bus.busnum, external); if (interface == nullptr) { warnx("no device on bus %u", (unsigned)bus.busid); @@ -181,7 +184,17 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, int range, int return false; } - bus.dev = new MPU9250(interface, bus.accelpath, bus.gyropath, bus.magpath, rotation, device_type); + device::Device *mag_interface = nullptr; + +#ifdef USE_I2C + /* For i2c interfaces, connect to the magnetomer directly */ + bool is_i2c = bus.busid == MPU9250_BUS_I2C_INTERNAL || bus.busid == MPU9250_BUS_I2C_EXTERNAL; + if (is_i2c) { + mag_interface = AK8963_I2C_interface(bus.busnum, external); + } +#endif + + bus.dev = new MPU9250(interface, mag_interface, bus.accelpath, bus.gyropath, bus.magpath, rotation); if (bus.dev == nullptr) { delete interface; @@ -203,9 +216,6 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, int range, int goto fail; } - if (ioctl(fd, ACCELIOCSRANGE, range) < 0) { - goto fail; - } close(fd); @@ -232,7 +242,7 @@ fail: * or failed to detect the sensor. */ void -start(enum MPU9250_BUS busid, enum Rotation rotation, int range, int device_type, bool external) +start(enum MPU9250_BUS busid, enum Rotation rotation, bool external) { bool started = false; @@ -248,7 +258,7 @@ start(enum MPU9250_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, external); } exit(started ? 0 : 1); @@ -473,7 +483,6 @@ usage() warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); - warnx(" -a accel range (in g)"); } } // namespace @@ -485,13 +494,13 @@ mpu9250_main(int argc, char *argv[]) int ch; bool external = false; enum Rotation rotation = ROTATION_NONE; - int accel_range = 8; + //int accel_range = 8; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc, argv, "XISsR:a:")) != EOF) { + while ((ch = getopt(argc, argv, "XISsR:")) != EOF) { switch (ch) { case 'X': - busid = MPU9260_BUS_I2C_EXTERNAL; + busid = MPU9250_BUS_I2C_EXTERNAL; break; case 'I': @@ -503,17 +512,13 @@ mpu9250_main(int argc, char *argv[]) break; case 's': - busid = MPU9260_BUS_SPI_INTERNAL; + busid = MPU9250_BUS_SPI_INTERNAL; break; case 'R': rotation = (enum Rotation)atoi(optarg); break; - case 'a': - accel_range = atoi(optarg); - break; - default: mpu9250::usage(); exit(0); @@ -529,7 +534,7 @@ mpu9250_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) { - mpu9250::start(busid, rotation, accel_range, device_type, external); + mpu9250::start(busid, rotation, external); } if (!strcmp(verb, "stop")) { diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index 422ac7f574..7e5eeb71d6 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include @@ -81,113 +82,6 @@ #include "gyro.h" #include "mpu9250.h" -#define DIR_READ 0x80 -#define DIR_WRITE 0x00 - -// MPU 9250 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_ACCEL_CONFIG2 0x1D -#define MPUREG_LPACCEL_ODR 0x1E -#define MPUREG_WOM_THRESH 0x1F -#define MPUREG_FIFO_EN 0x23 -#define MPUREG_I2C_MST_CTRL 0x24 -#define MPUREG_I2C_SLV0_ADDR 0x25 -#define MPUREG_I2C_SLV0_REG 0x26 -#define MPUREG_I2C_SLV0_CTRL 0x27 -#define MPUREG_I2C_SLV1_ADDR 0x28 -#define MPUREG_I2C_SLV1_REG 0x29 -#define MPUREG_I2C_SLV1_CTRL 0x2A -#define MPUREG_I2C_SLV2_ADDR 0x2B -#define MPUREG_I2C_SLV2_REG 0x2C -#define MPUREG_I2C_SLV2_CTRL 0x2D -#define MPUREG_I2C_SLV3_ADDR 0x2E -#define MPUREG_I2C_SLV3_REG 0x2F -#define MPUREG_I2C_SLV3_CTRL 0x30 -#define MPUREG_I2C_SLV4_ADDR 0x31 -#define MPUREG_I2C_SLV4_REG 0x32 -#define MPUREG_I2C_SLV4_DO 0x33 -#define MPUREG_I2C_SLV4_CTRL 0x34 -#define MPUREG_I2C_SLV4_DI 0x35 -#define MPUREG_I2C_MST_STATUS 0x36 -#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_EXT_SENS_DATA_00 0x49 -#define MPUREG_I2C_SLV0_D0 0x63 -#define MPUREG_I2C_SLV1_D0 0x64 -#define MPUREG_I2C_SLV2_D0 0x65 -#define MPUREG_I2C_SLV3_D0 0x66 -#define MPUREG_I2C_MST_DELAY_CTRL 0x67 -#define MPUREG_SIGNAL_PATH_RESET 0x68 -#define MPUREG_MOT_DETECT_CTRL 0x69 -#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 - -// Configuration bits MPU 9250 -#define BIT_SLEEP 0x40 -#define BIT_H_RESET 0x80 -#define MPU_CLK_SEL_AUTO 0x01 - -#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_250HZ 0x00 -#define BITS_DLPF_CFG_184HZ 0x01 -#define BITS_DLPF_CFG_92HZ 0x02 -#define BITS_DLPF_CFG_41HZ 0x03 -#define BITS_DLPF_CFG_20HZ 0x04 -#define BITS_DLPF_CFG_10HZ 0x05 -#define BITS_DLPF_CFG_5HZ 0x06 -#define BITS_DLPF_CFG_3600HZ 0x07 -#define BITS_DLPF_CFG_MASK 0x07 - -#define BITS_ACCEL_CONFIG2_41HZ 0x03 - -#define BIT_RAW_RDY_EN 0x01 -#define BIT_INT_ANYRD_2CLEAR 0x10 - -#define MPU_WHOAMI_9250 0x71 - -#define MPU9250_ACCEL_DEFAULT_RATE 1000 -#define MPU9250_ACCEL_MAX_OUTPUT_RATE 280 -#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 -#define MPU9250_GYRO_DEFAULT_RATE 1000 -/* rates need to be the same between accel and gyro */ -#define MPU9250_GYRO_MAX_OUTPUT_RATE MPU9250_ACCEL_MAX_OUTPUT_RATE -#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 - -#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 41 - -#define MPU9250_ONE_G 9.80665f - /* we set the timer interrupt to run a bit faster than the desired sample rate and then throw away duplicates by comparing @@ -215,15 +109,19 @@ const uint8_t MPU9250::_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = { MPU }; -MPU9250::MPU9250(device::Device *interface, const char *path_accel, const char *path_gyro, const char *path_mag, +MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, const char *path_mag, enum Rotation rotation) : CDev("MPU9250", path_accel), _interface(interface), - _interface_bus(interface_bus), - //SPI("MPU9250", path_accel, bus, device, SPIDEV_MODE3, MPU9250_LOW_BUS_SPEED), _gyro(new MPU9250_gyro(this, path_gyro)), - _mag(new MPU9250_mag(this, path_mag)), + _mag(new MPU9250_mag(this, mag_interface, path_mag)), _whoami(0), +#if defined(USE_I2C) + _work {}, + _use_hrt(false), +#else + _use_hrt(true), +#endif _call{}, _call_interval(0), _accel_reports(nullptr), @@ -277,6 +175,9 @@ MPU9250::MPU9250(device::Device *interface, const char *path_accel, const char * _mag->_device_id.devid = _device_id.devid; _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250; + /* For an independent mag, ensure that it is connected to the i2c bus */ + + // default accel scale factors _accel_scale.x_offset = 0; _accel_scale.x_scale = 1.0f; @@ -294,6 +195,7 @@ MPU9250::MPU9250(device::Device *interface, const char *path_accel, const char * _gyro_scale.z_scale = 1.0f; memset(&_call, 0, sizeof(_call)); + memset(&_work, 0, sizeof(_work)); } MPU9250::~MPU9250() @@ -334,8 +236,19 @@ MPU9250::~MPU9250() int MPU9250::init() { - int ret; +#if defined(USE_I2C) + unsigned dummy; + use_i2c(_interface->ioctl(MPUIOCGIS_I2C, dummy)); +#endif + + + int ret = probe(); + + if (ret != OK) { + DEVICE_DEBUG("MPU9250 probe failed"); + return ret; + } /* do init */ @@ -349,21 +262,7 @@ MPU9250::init() } -// /* do SPI init (and probe) first */ -// ret = SPI::init(); -// /* if probe/setup failed, bail now */ -// if (ret != OK) { -// DEVICE_DEBUG("SPI setup failed"); -// return ret; -// } - - ret = probe(); - - if (ret != OK) { - DEVICE_DEBUG("MPU9250 probe failed"); - return ret; - } /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); @@ -406,6 +305,11 @@ MPU9250::init() return ret; } + + if (!_mag->is_passthrough() && _mag->_interface->init() != OK) { + warnx("failed to setup ak8963 interface"); + } + /* do CDev init for the mag device node, keep it optional */ ret = _mag->init(); @@ -486,7 +390,7 @@ int MPU9250::reset() // INT CFG => Interrupt on Data Ready write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready usleep(1000); - write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read + write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (_mag->is_passthrough()? 0 : BIT_INT_BYPASS_EN)); // INT: Clear on any read, also use i2c bypass is master mode isn't needed usleep(1000); write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ); @@ -758,8 +662,7 @@ MPU9250::test_error() // development as a handy way to test the reset logic uint8_t data[16]; memset(data, 0, sizeof(data)); - _interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_LOW_BUS_SPEED), data, sizeof(data)); - //transfer(data, data, sizeof(data)); + _interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_LOW_BUS_SPEED), data, sizeof(data)); ::printf("error triggered\n"); print_registers(); } @@ -973,7 +876,7 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); + return CDev::ioctl(filp, cmd, arg); } } @@ -1071,23 +974,9 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) uint8_t MPU9250::read_reg(unsigned reg, uint32_t speed) { - - // From MPU6000 implementation uint8_t buf; - _interface->read(MPU6000_SET_SPEED(reg, speed), &buf, 1); + _interface->read(MPU9250_SET_SPEED(reg, speed), &buf, 1); return buf; - - - - - uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; - - // general register transfer at low clock speed - set_frequency(speed); - - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[1]; } uint16_t @@ -1099,18 +988,6 @@ MPU9250::read_reg16(unsigned reg) _interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf)); return (uint16_t)(buf[0] << 8) | buf[1]; - - - - - uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; - - // general register transfer at low clock speed - set_frequency(MPU9250_LOW_BUS_SPEED); - - transfer(cmd, cmd, sizeof(cmd)); - - return (uint16_t)(cmd[1] << 8) | cmd[2]; } void @@ -1118,19 +995,7 @@ MPU9250::write_reg(unsigned reg, uint8_t value) { // general register transfer at low clock speed - return _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1); - - - - uint8_t cmd[2]; - - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; - - // general register transfer at low clock speed - set_frequency(MPU9250_LOW_BUS_SPEED); - - transfer(cmd, nullptr, sizeof(cmd)); + _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1); } void @@ -1153,6 +1018,7 @@ MPU9250::write_checked_reg(unsigned reg, uint8_t value) if (reg == _checked_registers[i]) { _checked_values[i] = value; _checked_bad[i] = value; + break; // TODO: Assumes no duplicates } } } @@ -1203,19 +1069,70 @@ MPU9250::start() _gyro_reports->flush(); _mag->_mag_reports->flush(); - /* start polling at the specified rate */ - hrt_call_every(&_call, - 1000, - _call_interval - MPU9250_TIMER_REDUCTION, - (hrt_callout)&MPU9250::measure_trampoline, this); + if (_use_hrt) { + /* start polling at the specified rate */ + hrt_call_every(&_call, + 1000, + _call_interval - MPU9250_TIMER_REDUCTION, + (hrt_callout)&MPU9250::measure_trampoline, this);; + } else { +#ifdef USE_I2C + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MPU9250::cycle_trampoline, this, 1); +#endif + } + } void MPU9250::stop() { - hrt_cancel(&_call); + if (_use_hrt) { + hrt_cancel(&_call); + + } else { +#ifdef USE_I2C + work_cancel(HPWORK, &_work); +#endif + } } + +#if defined(USE_I2C) +void +MPU9250::cycle_trampoline(void *arg) +{ + MPU9250 *dev = (MPU9250 *)arg; + + dev->cycle(); +} + +void +MPU9250::cycle() +{ + +// int ret = measure(); + + measure(); + +// if (ret != OK) { +// /* issue a reset command to the sensor */ +// reset(); +// start(); +// return; +// } + + if (_call_interval != 0) { + work_queue(HPWORK, + &_work, + (worker_t)&MPU9250::cycle_trampoline, + this, + USEC2TICK(_call_interval - MPU9250_TIMER_REDUCTION)); + } +} +#endif + + void MPU9250::measure_trampoline(void *arg) { @@ -1350,12 +1267,13 @@ MPU9250::measure() /* start measuring */ perf_begin(_sample_perf); + // TODO: Don't fetch magnetometer data if it isn't being managed + // In that case, trigger a magnetometer read + /* * Fetch the full set of measurements from the MPU9250 in one pass. */ - mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - - if (sizeof(mpu_report) != _interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED), + if (OK != _interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED), (uint8_t *)&mpu_report, sizeof(mpu_report))) { return; @@ -1367,7 +1285,12 @@ MPU9250::measure() return; } - _mag->measure(mpu_report.mag); + if (_mag->is_passthrough()) { + _mag->_measure(mpu_report.mag); + } + else { + _mag->measure(); + } /* * Convert from big to little endian diff --git a/src/drivers/mpu9250/mpu9250.h b/src/drivers/mpu9250/mpu9250.h index b47492a42b..d17981d743 100644 --- a/src/drivers/mpu9250/mpu9250.h +++ b/src/drivers/mpu9250/mpu9250.h @@ -1,13 +1,250 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + + +#pragma once + + + +// TODO: All these includes are from main.cpp +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +// TODO: These three are new +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mag.h" +#include "gyro.h" + + + +#if defined(PX4_I2C_OBDEV_MPU9250) +# define USE_I2C +#endif + +#define DIR_READ 0x80 +#define DIR_WRITE 0x00 + +// MPU 9250 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_ACCEL_CONFIG2 0x1D +#define MPUREG_LPACCEL_ODR 0x1E +#define MPUREG_WOM_THRESH 0x1F +#define MPUREG_FIFO_EN 0x23 +#define MPUREG_I2C_MST_CTRL 0x24 +#define MPUREG_I2C_SLV0_ADDR 0x25 +#define MPUREG_I2C_SLV0_REG 0x26 +#define MPUREG_I2C_SLV0_CTRL 0x27 +#define MPUREG_I2C_SLV1_ADDR 0x28 +#define MPUREG_I2C_SLV1_REG 0x29 +#define MPUREG_I2C_SLV1_CTRL 0x2A +#define MPUREG_I2C_SLV2_ADDR 0x2B +#define MPUREG_I2C_SLV2_REG 0x2C +#define MPUREG_I2C_SLV2_CTRL 0x2D +#define MPUREG_I2C_SLV3_ADDR 0x2E +#define MPUREG_I2C_SLV3_REG 0x2F +#define MPUREG_I2C_SLV3_CTRL 0x30 +#define MPUREG_I2C_SLV4_ADDR 0x31 +#define MPUREG_I2C_SLV4_REG 0x32 +#define MPUREG_I2C_SLV4_DO 0x33 +#define MPUREG_I2C_SLV4_CTRL 0x34 +#define MPUREG_I2C_SLV4_DI 0x35 +#define MPUREG_I2C_MST_STATUS 0x36 +#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_EXT_SENS_DATA_00 0x49 +#define MPUREG_I2C_SLV0_D0 0x63 +#define MPUREG_I2C_SLV1_D0 0x64 +#define MPUREG_I2C_SLV2_D0 0x65 +#define MPUREG_I2C_SLV3_D0 0x66 +#define MPUREG_I2C_MST_DELAY_CTRL 0x67 +#define MPUREG_SIGNAL_PATH_RESET 0x68 +#define MPUREG_MOT_DETECT_CTRL 0x69 +#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 + + // Configuration bits MPU 9250 +#define BIT_SLEEP 0x40 +#define BIT_H_RESET 0x80 +#define MPU_CLK_SEL_AUTO 0x01 + +#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_250HZ 0x00 +#define BITS_DLPF_CFG_184HZ 0x01 +#define BITS_DLPF_CFG_92HZ 0x02 +#define BITS_DLPF_CFG_41HZ 0x03 +#define BITS_DLPF_CFG_20HZ 0x04 +#define BITS_DLPF_CFG_10HZ 0x05 +#define BITS_DLPF_CFG_5HZ 0x06 +#define BITS_DLPF_CFG_3600HZ 0x07 +#define BITS_DLPF_CFG_MASK 0x07 + +#define BITS_ACCEL_CONFIG2_41HZ 0x03 + +#define BIT_RAW_RDY_EN 0x01 +#define BIT_INT_ANYRD_2CLEAR 0x10 +#define BIT_INT_BYPASS_EN 0x02 + +#define MPU_WHOAMI_9250 0x71 + +#define MPU9250_ACCEL_DEFAULT_RATE 1000 +#define MPU9250_ACCEL_MAX_OUTPUT_RATE 280 +#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 +#define MPU9250_GYRO_DEFAULT_RATE 1000 +/* rates need to be the same between accel and gyro */ +#define MPU9250_GYRO_MAX_OUTPUT_RATE MPU9250_ACCEL_MAX_OUTPUT_RATE +#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 41 #define MPU9250_ONE_G 9.80665f +#define MPUIOCGIS_I2C (unsigned)(DEVIOCGDEVICEID+100) + + +#pragma pack(push, 1) +/** + * Report conversation within the mpu, 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]; + struct ak8963_regs mag; +}; +#pragma pack(pop) + +// TODO: Big enough? +#define MPU_MAX_WRITE_BUFFER_SIZE (2) + + +/* + The MPU9250 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 MPU9250_LOW_BUS_SPEED 0 +#define MPU9250_HIGH_BUS_SPEED 0x8000 +# define MPU9250_IS_HIGH_SPEED(r) ((r) & MPU9250_HIGH_BUS_SPEED) +# define MPU9250_REG(r) ((r) &~MPU9250_HIGH_BUS_SPEED) +# define MPU9250_SET_SPEED(r, s) ((r)|(s)) +# define MPU9250_HIGH_SPEED_OP(r) MPU9250_SET_SPEED((r), MPU9250_HIGH_BUS_SPEED) +# define MPU9250_LOW_SPEED_OP(r) MPU9250_REG((r)) + +/* interface factories */ +extern device::Device *MPU9250_SPI_interface(int bus, bool external_bus); +extern device::Device *MPU9250_I2C_interface(int bus, bool external_bus); +extern int MPU9250_probe(device::Device *dev, int device_type); + +typedef device::Device *(*MPU9250_constructor)(int, bool); + + + + class MPU9250_mag; class MPU9250_gyro; class MPU9250 : public device::CDev { public: - MPU9250(device::Device *interface, const char *path_accel, const char *path_gyro, const char *path_mag, + MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, const char *path_mag, enum Rotation rotation); virtual ~MPU9250(); @@ -42,6 +279,15 @@ private: MPU9250_mag *_mag; uint8_t _whoami; /** whoami result */ +#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; @@ -123,6 +369,42 @@ private: */ int reset(); + + +#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. @@ -193,7 +475,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 @@ -234,48 +520,4 @@ private: /* do not allow to copy this class due to pointer data members */ MPU9250(const MPU9250 &); MPU9250 operator=(const MPU9250 &); - -#pragma pack(push, 1) - /** - * Report conversation within the mpu, 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]; - struct ak8963_regs mag; - }; -#pragma pack(pop) }; - - - -/* - The MPU9250 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 MPU9250_LOW_BUS_SPEED 0 -#define MPU9250_HIGH_BUS_SPEED 0x8000 -# define MPU9250_IS_HIGH_SPEED(r) ((r) & MPU9250_HIGH_BUS_SPEED) -# define MPU9250_REG(r) ((r) &~MPU9250_HIGH_BUS_SPEED) -# define MPU9250_SET_SPEED(r, s) ((r)|(s)) -# define MPU9250_HIGH_SPEED_OP(r) MPU9250_SET_SPEED((r), MPU9250_HIGH_BUS_SPEED) -# define MPU9250_LOW_SPEED_OP(r) MPU9250_REG((r)) - - -/* interface factories */ -extern device::Device *MPU9250_SPI_interface(int bus, int device_type, bool external_bus); -extern device::Device *MPU9250_I2C_interface(int bus, int device_type, bool external_bus); -extern int MPU9250_probe(device::Device *dev, int device_type); - -typedef device::Device *(*MPU9250_constructor)(int, int, bool); diff --git a/src/drivers/mpu9250/mpu9250_i2c.cpp b/src/drivers/mpu9250/mpu9250_i2c.cpp index d546331fad..ef808155da 100644 --- a/src/drivers/mpu9250/mpu9250_i2c.cpp +++ b/src/drivers/mpu9250/mpu9250_i2c.cpp @@ -55,18 +55,18 @@ #include #include -#include "MPU9250.h" +#include "mpu9250.h" #include "board_config.h" #ifdef USE_I2C -device::Device *MPU9250_I2C_interface(int bus, int device_type, bool external_bus); +device::Device *MPU9250_I2C_interface(int bus, bool external_bus); class MPU9250_I2C : public device::I2C { public: - MPU9250_I2C(int bus, int device_type); + MPU9250_I2C(int bus); virtual ~MPU9250_I2C(); virtual int init(); @@ -78,21 +78,17 @@ public: protected: virtual int probe(); -private: - int _device_type; - }; device::Device * -MPU9250_I2C_interface(int bus, int device_type, bool external_bus) +MPU9250_I2C_interface(int bus, bool external_bus) { - return new MPU9250_I2C(bus, device_type); + return new MPU9250_I2C(bus); } -MPU9250_I2C::MPU9250_I2C(int bus, int device_type) : - I2C("MPU9250_I2C", nullptr, bus, PX4_I2C_OBDEV_MPU9250, 400000), - _device_type(device_type) +MPU9250_I2C::MPU9250_I2C(int bus) : + I2C("MPU9250_I2C", nullptr, bus, PX4_I2C_OBDEV_MPU9250, 400000) { _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; } @@ -156,8 +152,7 @@ MPU9250_I2C::read(unsigned reg_speed, void *data, unsigned count) */ uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, status); uint8_t cmd = MPU9250_REG(reg_speed); - int ret = transfer(&cmd, 1, &((uint8_t *)data)[offset], count); - return ret == OK ? count : ret; + return transfer(&cmd, 1, &((uint8_t *)data)[offset], count); } @@ -165,8 +160,8 @@ int MPU9250_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; - + uint8_t expected = MPU_WHOAMI_9250; + return (read(MPUREG_WHOAMI, &whoami, 1) == OK && (whoami == expected)) ? 0 : -EIO; } -#endif /* PX4_I2C_OBDEV_HMC5883 */ + +#endif /* USE_I2C */ diff --git a/src/drivers/mpu9250/mpu9250_spi.cpp b/src/drivers/mpu9250/mpu9250_spi.cpp index 6abdf4278c..b33ca32bf4 100644 --- a/src/drivers/mpu9250/mpu9250_spi.cpp +++ b/src/drivers/mpu9250/mpu9250_spi.cpp @@ -64,6 +64,7 @@ #define DIR_READ 0x80 #define DIR_WRITE 0x00 + #if PX4_SPIDEV_MPU #ifdef PX4_SPI_BUS_EXT #define EXTERNAL_BUS PX4_SPI_BUS_EXT @@ -78,16 +79,16 @@ SPI speed */ #define MPU9250_LOW_SPI_BUS_SPEED 1000*1000 -#define MPU9250_HIGH_SPI_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */ +#define MPU9250_HIGH_SPI_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU9250 */ -device::Device *MPU9250_SPI_interface(int bus, int device_type, bool external_bus); +device::Device *MPU9250_SPI_interface(int bus, bool external_bus); class MPU9250_SPI : public device::SPI { public: - MPU9250_SPI(int bus, spi_dev_e device, int device_type); + MPU9250_SPI(int bus, spi_dev_e device); virtual ~MPU9250_SPI(); virtual int init(); @@ -100,47 +101,37 @@ protected: 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 * -MPU9250_SPI_interface(int bus, int device_type, bool external_bus) +MPU9250_SPI_interface(int bus, 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; + errx(0, "External SPI not available"); #endif + } else { + cs = (spi_dev_e) PX4_SPIDEV_MPU; } if (cs != SPIDEV_NONE) { - interface = new MPU9250_SPI(bus, cs, device_type); + interface = new MPU9250_SPI(bus, cs); } return interface; } -MPU9250_SPI::MPU9250_SPI(int bus, spi_dev_e device, int device_type) : - SPI("MPU9250", nullptr, bus, device, SPIDEV_MODE3, MPU9250_LOW_SPI_BUS_SPEED), - _device_type(device_type) +MPU9250_SPI::MPU9250_SPI(int bus, spi_dev_e device) : + SPI("MPU9250", nullptr, bus, device, SPIDEV_MODE3, MPU9250_LOW_SPI_BUS_SPEED) { _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; } @@ -266,16 +257,15 @@ MPU9250_SPI::read(unsigned reg_speed, void *data, unsigned count) } - return ret == OK ? count : ret; + return ret; } int MPU9250_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_9250; + return (read(MPUREG_WHOAMI, &whoami, 1) == OK && (whoami == expected)) ? 0 : -EIO; } #endif // PX4_SPIDEV_MPU