Fixed up init sequence of all sensors - we can publish in interrupt context, but not advertise! All advertisements now contain valid data

1 files changed, 18 insertions, 12 deletions

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index e885b1bf9..90c3db9ae 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -382,6 +382,21 @@ L3GD20::init()
 
 	reset();
 
+	measure();
+
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+
+		/* 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");
+
+	}
+
 	ret = OK;
 out:
 	return ret;
@@ -888,18 +903,9 @@ L3GD20::measure()
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
-
-		if (_gyro_topic > 0) {
-			/* publish it */
-			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
-		} else {
-			_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &report);
-
-			if (_gyro_topic < 0)
-				debug("failed to create sensor_gyro publication");
-		}
-
+	if (_gyro_topic > 0 && !(_pub_blocked)) {
+		/* publish it */
+		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
 	}
 
 	_read++;