From c35a25e70a57229b58fe065fda23003de856e9f5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 10 Jun 2014 15:08:21 +0200
Subject: [PATCH] L3GD20: Add support for multi uORB topics

---
 src/drivers/l3gd20/l3gd20.cpp | 30 ++++++++++++++++++++----------
 1 file changed, 20 insertions(+), 10 deletions(-)

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 37e72388be..2fcbdc0ade 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -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++;