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

L3GD20: Add support for multi uORB topics

Browse Source
This commit is contained in:
Lorenz Meier
2014-06-10 15:08:21 +02:00
parent 795f3693f2
commit c35a25e70a
1 changed files with 20 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/l3gd20/l3gd20.cpp
+20 -10
View File
@@ -398,17 +398,19 @@ L3GD20::init()
measure();
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_reports->get(&grp);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro0), &grp);
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_reports->get(&grp);
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
if (_gyro_topic < 0)
debug("failed to create sensor_gyro publication");
} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro1), &grp);
}
if (_gyro_topic < 0) {
debug("failed to create sensor_gyro publication");
}
ret = OK;
@@ -923,9 +925,17 @@ L3GD20::measure()
poll_notify(POLLIN);
/* publish for subscribers */
if (_gyro_topic > 0 && !(_pub_blocked)) {
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
switch (_class_instance) {
case CLASS_DEVICE_PRIMARY:
orb_publish(ORB_ID(sensor_gyro0), _gyro_topic, &report);
break;
case CLASS_DEVICE_SECONDARY:
orb_publish(ORB_ID(sensor_gyro1), _gyro_topic, &report);
break;
}
}
_read++;