fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 23:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Support for publication blocking: MPU6000, cleaned up device start

Browse Source
This commit is contained in:
Lorenz Meier
2014-01-12 11:54:38 +01:00
parent a34a14ce86
commit 7af62bbe9e
1 changed files with 26 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/mpu6000/mpu6000.cpp
+26 -20
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@@ -443,7 +443,6 @@ int
MPU6000::init()
{
int ret;
int gyro_ret;
/* do SPI init (and probe) first */
ret = SPI::init();
@@ -488,16 +487,7 @@ MPU6000::init()
return ret;
}
/* fetch an initial set of measurements for advertisement */
measure();
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise accel topic */
accel_report ar;
_accel_reports->get(&ar);
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
}
out:
return ret;
@@ -1307,11 +1297,32 @@ MPU6000::measure()
poll_notify(POLLIN);
_gyro->parent_poll_notify();
if (_accel_topic != -1) {
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
if (_accel_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
if (_accel_topic > 0) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
} else {
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arb);
if (_accel_topic < 0)
debug("failed to create sensor_accel publication");
}
}
if (_gyro->_gyro_topic != -1) {
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
if (_gyro->_gyro_topic > 0) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
} else {
_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grb);
if (_gyro->_gyro_topic < 0)
debug("failed to create sensor_gyro publication");
}
}
/* stop measuring */
@@ -1356,11 +1367,6 @@ MPU6000_gyro::init()
}
_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
gyro_report gr;
memset(&gr, 0, sizeof(gr));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
}
out:
return ret;