Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 22:32:11 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

mpu9250:More cleanup

Browse Source
This commit is contained in:
David Sidrane
2017-07-19 19:45:43 -10:00
committed by Lorenz Meier
parent 5bb084408d
commit ed74530da8
3 changed files with 68 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/mpu9250/mag.cpp
+57 -55
View File
@@ -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;
}
src/drivers/mpu9250/mag.h
+1 -1
View File
@@ -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:
src/drivers/mpu9250/mpu9250.cpp
+10 -9
View File
@@ -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: