From 3c7766db6c58dea67b66263d2a7c01bb57177db5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 11:54:10 +0100 Subject: Support for publication blocking: L3GD20(H) --- src/drivers/l3gd20/l3gd20.cpp | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 670e51b97..e885b1bf9 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -379,12 +379,6 @@ L3GD20::init() goto out; _class_instance = register_class_devname(GYRO_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* advertise sensor topic */ - struct gyro_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); - } reset(); @@ -894,8 +888,19 @@ L3GD20::measure() poll_notify(POLLIN); /* publish for subscribers */ - if (_gyro_topic > 0) - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); + 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"); + } + + } _read++; -- cgit v1.2.3