diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-12 11:54:10 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-12 11:54:10 +0100 |
| commit | 3c7766db6c58dea67b66263d2a7c01bb57177db5 (patch) | |
| tree | ba03fb1b4ca2e96ab45151b1133cea0bd6557de3 /src/drivers | |
| parent | 28a3dc726f8e4f3696736798a1d92bf7d9de6100 (diff) | |
| download | px4-firmware-3c7766db6c58dea67b66263d2a7c01bb57177db5.tar.gz px4-firmware-3c7766db6c58dea67b66263d2a7c01bb57177db5.tar.bz2 px4-firmware-3c7766db6c58dea67b66263d2a7c01bb57177db5.zip | |
Support for publication blocking: L3GD20(H)
Diffstat (limited to 'src/drivers')
| -rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 23 |
1 files changed, 14 insertions, 9 deletions
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++; |