diff --git a/src/drivers/imu/mpu9250/mag_i2c.cpp b/src/drivers/imu/mpu9250/mag_i2c.cpp index a3c163ea4a..23a4ba8b74 100644 --- a/src/drivers/imu/mpu9250/mag_i2c.cpp +++ b/src/drivers/imu/mpu9250/mag_i2c.cpp @@ -37,20 +37,7 @@ * I2C interface for AK8963 */ -/* XXX trim includes */ #include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - #include #include #include @@ -58,9 +45,6 @@ #include "mpu9250.h" #include "mag.h" -#include "board_config.h" - - #ifdef USE_I2C device::Device *AK8963_I2C_interface(int bus, bool external_bus); @@ -69,19 +53,16 @@ class AK8963_I2C : public device::I2C { public: AK8963_I2C(int bus); - virtual ~AK8963_I2C() = default; + ~AK8963_I2C() override = default; - 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); + int read(unsigned address, void *data, unsigned count) override; + int write(unsigned address, void *data, unsigned count) override; protected: - virtual int probe(); + int probe() override; }; - device::Device * AK8963_I2C_interface(int bus, bool external_bus) { @@ -94,25 +75,6 @@ AK8963_I2C::AK8963_I2C(int bus) : _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250; } -int -AK8963_I2C::ioctl(unsigned operation, unsigned &arg) -{ - int ret; - - switch (operation) { - 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) { @@ -134,7 +96,6 @@ AK8963_I2C::read(unsigned reg_speed, void *data, unsigned count) return transfer(&cmd, 1, (uint8_t *)data, count); } - int AK8963_I2C::probe() { diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index 25b88c015a..bd48a61b18 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -255,8 +255,7 @@ MPU9250::init() { #if defined(USE_I2C) - unsigned dummy; - use_i2c(_interface->ioctl(MPUIOCGIS_I2C, dummy)); + use_i2c(_interface->get_device_bus_type() == Device::DeviceBusType_I2C); #endif /* diff --git a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp index 0279d28a12..3893d74621 100644 --- a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp @@ -37,28 +37,13 @@ * I2C interface for MPU9250 */ -/* XXX trim includes */ #include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - #include #include #include #include "mpu9250.h" -#include "board_config.h" - #ifdef USE_I2C device::Device *MPU9250_I2C_interface(int bus, uint32_t address, bool external_bus); @@ -67,15 +52,13 @@ class MPU9250_I2C : public device::I2C { public: MPU9250_I2C(int bus, uint32_t address); - virtual ~MPU9250_I2C() = default; + ~MPU9250_I2C() override = default; - 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); + int read(unsigned address, void *data, unsigned count) override; + int write(unsigned address, void *data, unsigned count) override; protected: - virtual int probe(); + int probe() override; }; @@ -88,26 +71,7 @@ MPU9250_I2C_interface(int bus, uint32_t address, bool external_bus) MPU9250_I2C::MPU9250_I2C(int bus, uint32_t address) : I2C("MPU9250_I2C", nullptr, bus, address, 400000) { - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; -} - -int -MPU9250_I2C::ioctl(unsigned operation, unsigned &arg) -{ - int ret = PX4_ERROR; - - switch (operation) { - case DEVIOCGDEVICEID: - return CDev::ioctl(nullptr, operation, arg); - - case MPUIOCGIS_I2C: - return 1; - - default: - ret = -EINVAL; - } - - return ret; + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; } int diff --git a/src/drivers/imu/mpu9250/mpu9250_spi.cpp b/src/drivers/imu/mpu9250/mpu9250_spi.cpp index de62326344..c02d6f2dc4 100644 --- a/src/drivers/imu/mpu9250/mpu9250_spi.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_spi.cpp @@ -42,24 +42,11 @@ */ #include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - #include #include #include #include "mpu9250.h" -#include #define DIR_READ 0x80 #define DIR_WRITE 0x00 @@ -76,27 +63,23 @@ #define MPU9250_LOW_SPI_BUS_SPEED 1000*1000 #define MPU9250_HIGH_SPI_BUS_SPEED 20*1000*1000 - device::Device *MPU9250_SPI_interface(int bus, uint32_t cs, bool external_bus); - class MPU9250_SPI : public device::SPI { public: MPU9250_SPI(int bus, uint32_t device); - virtual ~MPU9250_SPI() = default; + ~MPU9250_SPI() override = default; - virtual int read(unsigned address, void *data, unsigned count); - virtual int write(unsigned address, void *data, unsigned count); + int read(unsigned address, void *data, unsigned count) override; + int write(unsigned address, void *data, unsigned count) override; - virtual int ioctl(unsigned operation, unsigned &arg); protected: - virtual int probe(); + int probe() override; private: /* Helper to set the desired speed and isolate the register on return */ - void set_bus_frequency(unsigned ®_speed_reg_out); }; @@ -121,38 +104,16 @@ MPU9250_SPI_interface(int bus, uint32_t cs, bool external_bus) MPU9250_SPI::MPU9250_SPI(int bus, uint32_t device) : SPI("MPU9250", nullptr, bus, device, SPIDEV_MODE3, MPU9250_LOW_SPI_BUS_SPEED) { - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; -} - -int -MPU9250_SPI::ioctl(unsigned operation, unsigned &arg) -{ - int ret; - - switch (operation) { - case DEVIOCGDEVICEID: - return CDev::ioctl(nullptr, operation, arg); - - case MPUIOCGIS_I2C: - return 0; - - default: { - ret = -EINVAL; - } - } - - return ret; + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; } void MPU9250_SPI::set_bus_frequency(unsigned ®_speed) { /* Set the desired speed */ - set_frequency(MPU9250_IS_HIGH_SPEED(reg_speed) ? MPU9250_HIGH_SPI_BUS_SPEED : MPU9250_LOW_SPI_BUS_SPEED); /* Isoolate the register on return */ - reg_speed = MPU9250_REG(reg_speed); } @@ -187,34 +148,25 @@ MPU9250_SPI::read(unsigned reg_speed, void *data, unsigned count) 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;