diff options
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 64d1a7e55..9e5eb00db 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -403,17 +403,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; @@ -931,9 +933,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++; |