From d72c82f66bf6dac8e6d10bf1024641908d3b854c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 11:53:15 +0100 Subject: Airspeed does not publish if disabled --- src/drivers/airspeed/airspeed.cpp | 12 +++++++----- src/drivers/airspeed/airspeed.h | 4 +++- src/drivers/ets_airspeed/ets_airspeed.cpp | 15 +++++++++++++-- src/drivers/meas_airspeed/meas_airspeed.cpp | 15 +++++++++++++-- 4 files changed, 36 insertions(+), 10 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 5e45cc936..215d3792e 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 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 @@ -86,6 +86,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : _collect_phase(false), _diff_pres_offset(0.0f), _airspeed_pub(-1), + _class_instance(-1), _conversion_interval(conversion_interval), _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) @@ -102,6 +103,9 @@ Airspeed::~Airspeed() /* make sure we are truly inactive */ stop(); + if (_class_instance != -1) + unregister_class_devname(AIRSPEED_DEVICE_PATH, _class_instance); + /* free any existing reports */ if (_reports != nullptr) delete _reports; @@ -126,10 +130,8 @@ Airspeed::init() if (_reports == nullptr) goto out; - /* get a publish handle on the airspeed topic */ - differential_pressure_s zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report); + /* register alternate interfaces if we have to */ + _class_instance = register_class_devname(AIRSPEED_DEVICE_PATH); if (_airspeed_pub < 0) warnx("failed to create airspeed sensor object. Did you start uOrb?"); diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index c341aa2c6..c27b1bcd8 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 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 @@ -127,6 +127,8 @@ protected: orb_advert_t _airspeed_pub; + int _class_instance; + unsigned _conversion_interval; perf_counter_t _sample_perf; diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index de371bf32..cdc70ac37 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 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 @@ -185,7 +185,18 @@ ETSAirspeed::collect() report.max_differential_pressure_pa = _max_differential_pressure_pa; /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + + if (_airspeed_pub > 0) { + /* publish it */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + } else { + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &report); + + if (_airspeed_pub < 0) + debug("failed to create differential_pressure publication"); + } + } new_report(report); diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index a95c4576b..fee13f139 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 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 @@ -217,7 +217,18 @@ MEASAirspeed::collect() report.max_differential_pressure_pa = _max_differential_pressure_pa; /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + + if (_airspeed_pub > 0) { + /* publish it */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + } else { + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &report); + + if (_airspeed_pub < 0) + debug("failed to create differential_pressure publication"); + } + } new_report(report); -- cgit v1.2.3