From e6a67b1deb0e7d5b315d3c570ff010de4d54b65f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 11:54:55 +0100 Subject: Support for publication blocking: MS5611, cleaned up device start --- src/drivers/ms5611/ms5611.cpp | 33 +++++++++++++++++++++------------ 1 file changed, 21 insertions(+), 12 deletions(-) (limited to 'src/drivers/ms5611') diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 6326cf7fc..089331566 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.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 @@ -132,6 +132,8 @@ protected: orb_advert_t _baro_topic; + int _class_instance; + perf_counter_t _sample_perf; perf_counter_t _measure_perf; perf_counter_t _comms_errors; @@ -204,6 +206,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _SENS(0), _msl_pressure(101325), _baro_topic(-1), + _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), @@ -218,6 +221,9 @@ MS5611::~MS5611() /* make sure we are truly inactive */ stop_cycle(); + if (_class_instance != -1) + unregister_class_devname(BARO_DEVICE_PATH, _class_instance); + /* free any existing reports */ if (_reports != nullptr) delete _reports; @@ -251,16 +257,8 @@ MS5611::init() goto out; } - /* get a publish handle on the baro topic */ - struct baro_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report); - - if (_baro_topic < 0) { - debug("failed to create sensor_baro object"); - ret = -ENOSPC; - goto out; - } + /* register alternate interfaces if we have to */ + _class_instance = register_class_devname(BARO_DEVICE_PATH); ret = OK; out: @@ -670,7 +668,18 @@ MS5611::collect() report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + + if (_baro_topic > 0) { + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + } else { + _baro_topic = orb_advertise(ORB_ID(sensor_baro), &report); + + if (_baro_topic < 0) + debug("failed to create sensor_baro publication"); + } + } if (_reports->force(&report)) { perf_count(_buffer_overflows); -- cgit v1.2.3