From ed74530da858177972bb9f6532ae34a5247ce4a8 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 19 Jul 2017 19:45:43 -1000 Subject: [PATCH] mpu9250:More cleanup --- src/drivers/mpu9250/mag.cpp | 112 ++++++++++++++++---------------- src/drivers/mpu9250/mag.h | 2 +- src/drivers/mpu9250/mpu9250.cpp | 19 +++--- 3 files changed, 68 insertions(+), 65 deletions(-) diff --git a/src/drivers/mpu9250/mag.cpp b/src/drivers/mpu9250/mag.cpp index 393052585b..3f56a74def 100644 --- a/src/drivers/mpu9250/mag.cpp +++ b/src/drivers/mpu9250/mag.cpp @@ -113,6 +113,8 @@ MPU9250_mag::~MPU9250_mag() delete _mag_reports; } + orb_unadvertise(_mag_topic); + perf_free(_mag_reads); perf_free(_mag_errors); perf_free(_mag_overruns); @@ -123,42 +125,37 @@ MPU9250_mag::~MPU9250_mag() int MPU9250_mag::init() { - int ret = ak8963_setup(); + int ret = CDev::init(); - if (ret == OK) { - - ret = CDev::init(); - - /* if setup failed, bail now */ - if (ret != OK) { - DEVICE_DEBUG("MPU9250 mag init failed"); - return ret; - } - - _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); - - if (_mag_reports == nullptr) { - goto out; - } - - _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); - - - /* advertise sensor topic, measure manually to initialize valid report */ - struct mag_report mrp; - _mag_reports->get(&mrp); - - _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, - &_mag_orb_class_instance, ORB_PRIO_LOW); - // &_mag_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); - - if (_mag_topic == nullptr) { - PX4_ERR("ADVERT FAIL"); - } + /* if cdev init failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("MPU9250 mag init failed"); + return ret; } -out: - return ret; + _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); + + if (_mag_reports == nullptr) { + return -ENOMEM;; + } + + _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); + + + /* advertise sensor topic, measure manually to initialize valid report */ + struct mag_report mrp; + _mag_reports->get(&mrp); + + _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, + &_mag_orb_class_instance, ORB_PRIO_LOW); + // &_mag_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); + + if (_mag_topic == nullptr) { + PX4_ERR("ADVERT FAIL"); + return -ENOMEM; + } + + return OK; } bool MPU9250_mag::check_duplicate(uint8_t *mag_data) @@ -557,6 +554,23 @@ MPU9250_mag::ak8963_read_adjustments(void) return true; } +int +MPU9250_mag::ak8963_setup_master_i2c(void) +{ + /* When _interface is null we are using SPI and must + * use the parent interface to configure the device to act + * in master mode (SPI to I2C bridge) + */ + if (_interface == nullptr) { + _parent->modify_checked_reg(MPUREG_USER_CTRL, 0, 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 { + _parent->modify_checked_reg(MPUREG_USER_CTRL, BIT_I2C_MST_EN, 0); + } + + return OK; +} int MPU9250_mag::ak8963_setup(void) { @@ -564,17 +578,8 @@ MPU9250_mag::ak8963_setup(void) do { - /* When _interface is null we are using SPI and must - * use the parent interface to configure the device to act - * in master mode (SPI to I2C bridge) - */ - if (_interface == nullptr) { - _parent->modify_checked_reg(MPUREG_USER_CTRL, 0, 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 { - _parent->modify_checked_reg(MPUREG_USER_CTRL, BIT_I2C_MST_EN, 0); - } + ak8963_setup_master_i2c(); + write_reg(AK8963REG_CNTL2, AK8963_RESET); uint8_t id = 0; @@ -583,12 +588,9 @@ MPU9250_mag::ak8963_setup(void) } retries--; - - if (retries < 8) { - PX4_ERR("AK8963: bad id %d retries %d", id, retries); - _parent->modify_checked_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST); - up_udelay(100); - } + PX4_ERR("AK8963: bad id %d retries %d", id, retries); + _parent->modify_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST); + up_udelay(100); } while (retries > 0); if (retries > 0) { @@ -598,16 +600,16 @@ MPU9250_mag::ak8963_setup(void) retries--; PX4_ERR("AK8963: failed to read adjustment data. Retries %d", retries); - if (retries == 5) { - _parent->modify_checked_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST); - up_udelay(100); - } + _parent->modify_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST); + up_udelay(100); + ak8963_setup_master_i2c(); + write_reg(AK8963REG_CNTL2, AK8963_RESET); } } if (retries == 0) { PX4_ERR("AK8963: failed to initialize, disabled!"); - _parent->modify_checked_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST); + _parent->modify_checked_reg(MPUREG_USER_CTRL, BIT_I2C_MST_EN, 0); _parent->write_reg(MPUREG_I2C_MST_CTRL, 0); return -EIO; } diff --git a/src/drivers/mpu9250/mag.h b/src/drivers/mpu9250/mag.h index ea14db299b..4d847438e1 100644 --- a/src/drivers/mpu9250/mag.h +++ b/src/drivers/mpu9250/mag.h @@ -106,6 +106,7 @@ public: int ak8963_reset(void); int ak8963_setup(void); + int ak8963_setup_master_i2c(void); bool ak8963_check_id(uint8_t &id); bool ak8963_read_adjustments(void); @@ -127,7 +128,6 @@ protected: bool is_passthrough() { return _interface == nullptr; } - int self_test(void); private: diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index 3fd1d18b27..0618bbce83 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -218,6 +218,9 @@ MPU9250::~MPU9250() /* make sure we are truly inactive */ stop(); + orb_unadvertise(_accel_topic); + orb_unadvertise(_gyro->_gyro_topic); + /* delete the gyro subdriver */ delete _gyro; @@ -257,7 +260,6 @@ MPU9250::init() use_i2c(_interface->ioctl(MPUIOCGIS_I2C, dummy)); #endif - int ret = probe(); if (ret != OK) { @@ -265,7 +267,6 @@ MPU9250::init() return ret; } - /* do init */ ret = CDev::init(); @@ -276,11 +277,9 @@ MPU9250::init() return ret; } - - - /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + ret = -ENOMEM; if (_accel_reports == nullptr) { goto out; @@ -318,7 +317,7 @@ MPU9250::init() /* if probe/setup failed, bail now */ if (ret != OK) { DEVICE_DEBUG("gyro init failed"); - return ret; + goto out; } #ifdef USE_I2C @@ -337,7 +336,7 @@ MPU9250::init() /* if probe/setup failed, bail now */ if (ret != OK) { DEVICE_DEBUG("mag init failed"); - return ret; + goto out; } @@ -364,7 +363,8 @@ MPU9250::init() &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); if (_accel_topic == nullptr) { - warnx("ADVERT FAIL"); + PX4_ERR("ADVERT FAIL"); + goto out; } /* advertise sensor topic, measure manually to initialize valid report */ @@ -375,7 +375,8 @@ MPU9250::init() &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); if (_gyro->_gyro_topic == nullptr) { - warnx("ADVERT FAIL"); + PX4_ERR("ADVERT FAIL"); + goto out; } out: