diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-10 15:08:21 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-10 15:08:21 +0200 |
| commit | c35a25e70a57229b58fe065fda23003de856e9f5 (patch) | |
| tree | 4c594f6421dd732e6c3a848565602a87ba4c83bd /src/drivers/l3gd20 | |
| parent | 795f3693f26f1671a417cb7b6e36d743cab72304 (diff) | |
| download | px4-firmware-c35a25e70a57229b58fe065fda23003de856e9f5.tar.gz px4-firmware-c35a25e70a57229b58fe065fda23003de856e9f5.tar.bz2 px4-firmware-c35a25e70a57229b58fe065fda23003de856e9f5.zip | |
L3GD20: Add support for multi uORB topics
Diffstat (limited to 'src/drivers/l3gd20')
| -rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 30 |
1 files changed, 20 insertions, 10 deletions
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 37e72388b..2fcbdc0ad 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -398,17 +398,19 @@ L3GD20::init() measure(); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - - /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; - _reports->get(&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 (_class_instance == CLASS_DEVICE_PRIMARY) { + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro0), &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++; |