From 69cbbd9b5eae2c475549518b1785bd1f5edeb7ea Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 19:20:25 +0100 Subject: [PATCH] distance_sensor: changed all distance sensors to orb_advertise_multi --- src/drivers/ll40ls/LidarLiteI2C.cpp | 4 +++- src/drivers/ll40ls/LidarLiteI2C.h | 1 + src/drivers/ll40ls/LidarLitePWM.cpp | 4 +++- src/drivers/ll40ls/LidarLitePWM.h | 1 + src/drivers/mb12xx/mb12xx.cpp | 9 +++++--- src/drivers/px4flow/px4flow.cpp | 32 ++++++++++++++++++-------- src/drivers/sf0x/sf0x.cpp | 35 ++++++++++++++++++++++------- src/drivers/trone/trone.cpp | 16 ++++++++----- 8 files changed, 74 insertions(+), 28 deletions(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 4a2922f7df..a9626c8c45 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -61,6 +61,7 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) : _sensor_ok(false), _collect_phase(false), _class_instance(-1), + _orb_class_instance(-1), _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_i2c_read")), _comms_errors(perf_alloc(PC_COUNT, "ll40ls_i2c_comms_errors")), @@ -128,7 +129,8 @@ int LidarLiteI2C::init() struct distance_sensor_s ds_report; measure(); _reports->get(&ds_report); - _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &ds_report); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic < 0) { debug("failed to create distance_sensor object. Did you start uOrb?"); diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index 9f58475b98..f03784fc4d 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -103,6 +103,7 @@ private: bool _sensor_ok; bool _collect_phase; int _class_instance; + int _orb_class_instance; orb_advert_t _distance_sensor_topic; diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 2adb9e8ed3..14ce921b4d 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -58,6 +58,7 @@ LidarLitePWM::LidarLitePWM(const char *path) : _work{}, _reports(nullptr), _class_instance(-1), + _orb_class_instance(-1), _pwmSub(-1), _pwm{}, _distance_sensor_topic(-1), @@ -112,7 +113,8 @@ int LidarLitePWM::init() struct distance_sensor_s ds_report; measure(); _reports->get(&ds_report); - _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &ds_report); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic < 0) { debug("failed to create distance_sensor object. Did you start uOrb?"); diff --git a/src/drivers/ll40ls/LidarLitePWM.h b/src/drivers/ll40ls/LidarLitePWM.h index 93cb80e29a..8d337e48ba 100644 --- a/src/drivers/ll40ls/LidarLitePWM.h +++ b/src/drivers/ll40ls/LidarLitePWM.h @@ -108,6 +108,7 @@ private: work_s _work; ringbuffer::RingBuffer *_reports; int _class_instance; + int _orb_class_instance; int _pwmSub; struct pwm_input_s _pwm; orb_advert_t _distance_sensor_topic; diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 7fa38e0ab4..fcac2e1b81 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -130,7 +130,8 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; - int _class_instance; + int _class_instance; + int _orb_class_instance; orb_advert_t _distance_sensor_topic; @@ -209,6 +210,7 @@ MB12XX::MB12XX(int bus, int address) : _measure_ticks(0), _collect_phase(false), _class_instance(-1), + _orb_class_instance(-1), _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), @@ -269,9 +271,10 @@ MB12XX::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct distance_sensor_s rf_report = {}; + struct distance_sensor_s ds_report = {}; - _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic < 0) { log("failed to create sensor_range_finder object. Did you start uOrb?"); diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 2993037cff..e48b0b3b48 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -67,6 +67,7 @@ #include #include +#include #include #include @@ -125,10 +126,12 @@ protected: private: work_s _work; - ringbuffer::RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; - int _measure_ticks; + int _measure_ticks; bool _collect_phase; + int _class_instance; + int _orb_class_instance; orb_advert_t _px4flow_topic; orb_advert_t _distance_sensor_topic; @@ -188,6 +191,8 @@ PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) : _sensor_ok(false), _measure_ticks(0), _collect_phase(false), + _class_instance(-1), + _orb_class_instance(-1), _px4flow_topic(-1), _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), @@ -195,7 +200,7 @@ PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) : _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")), _sensor_rotation(rotation) { - // enable debug() calls + // disable debug() calls _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... @@ -230,6 +235,20 @@ PX4FLOW::init() goto out; } + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report = {}; + + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_HIGH); + + if (_distance_sensor_topic < 0) { + log("failed to create sensor_range_finder object. Did you start uOrb?"); + } + } + ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; @@ -518,12 +537,7 @@ PX4FLOW::collect() distance_report.id = 0; distance_report.orientation = 8; - if (_distance_sensor_topic < 0) { - _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &distance_report); - } else { - /* publish it */ - orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report); - } + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report); /* post a report to the ring */ if (_reports->force(&report)) { diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 71660c390a..bace2aaa0d 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2015 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 @@ -119,7 +119,7 @@ private: float _min_distance; float _max_distance; work_s _work; - ringbuffer::RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -129,6 +129,9 @@ private: enum SF0X_PARSE_STATE _parse_state; hrt_abstime _last_read; + int _class_instance; + int _orb_class_instance; + orb_advert_t _distance_sensor_topic; unsigned _consecutive_fail_count; @@ -195,6 +198,8 @@ SF0X::SF0X(const char *port) : _linebuf_index(0), _parse_state(SF0X_PARSE_STATE0_UNSYNC), _last_read(0), + _class_instance(-1), + _orb_class_instance(-1), _distance_sensor_topic(-1), _consecutive_fail_count(0), _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), @@ -256,6 +261,14 @@ SF0X::~SF0X() if (_reports != nullptr) { delete _reports; } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); + } + + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); } int @@ -278,14 +291,20 @@ SF0X::init() break; } - /* get a publish handle on the range finder topic */ - struct distance_sensor_s zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &zero_report); + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_distance_sensor_topic < 0) { - warnx("advert err"); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report = {}; + + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_HIGH); + + if (_distance_sensor_topic < 0) { + log("failed to create sensor_range_finder object. Did you start uOrb?"); + } } + } while(0); /* close the fd */ diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index aba9749717..691c509e23 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 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,7 +127,8 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; - int _class_instance; + int _class_instance; + int _orb_class_instance; orb_advert_t _distance_sensor_topic; @@ -236,6 +237,7 @@ TRONE::TRONE(int bus, int address) : _measure_ticks(0), _collect_phase(false), _class_instance(-1), + _orb_class_instance(-1), _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "trone_read")), _comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")), @@ -292,13 +294,15 @@ TRONE::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct distance_sensor_s rf_report; + struct distance_sensor_s ds_report; measure(); - _reports->get(&rf_report); - _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report); + _reports->get(&ds_report); + + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + log("failed to create sensor_range_finder object. Did you start uOrb?"); } }