diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 1ee3ed8ba7..0c8018ab64 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -137,6 +137,7 @@ px4_add_module( sensors/flow.cpp sensors/gnss.cpp sensors/mag.cpp + sensors/rangefinder.cpp DEPENDS px4_uavcan_dsdlc diff --git a/src/drivers/uavcan/sensors/airspeed.cpp b/src/drivers/uavcan/sensors/airspeed.cpp index 64ee0f0d04..6de09c7bdb 100644 --- a/src/drivers/uavcan/sensors/airspeed.cpp +++ b/src/drivers/uavcan/sensors/airspeed.cpp @@ -94,7 +94,6 @@ UavcanAirspeedBridge::tas_sub_cb(const { _last_tas_m_s = msg.true_airspeed; } - void UavcanAirspeedBridge::ias_sub_cb(const uavcan::ReceivedDataStructure &msg) diff --git a/src/drivers/uavcan/sensors/rangefinder.cpp b/src/drivers/uavcan/sensors/rangefinder.cpp new file mode 100644 index 0000000000..fce04e9a12 --- /dev/null +++ b/src/drivers/uavcan/sensors/rangefinder.cpp @@ -0,0 +1,155 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author RJ Gritter + */ + +#include +#include +#include "rangefinder.hpp" +#include + +const char *const UavcanRangefinderBridge::NAME = "rangefinder"; + +UavcanRangefinderBridge::UavcanRangefinderBridge(uavcan::INode &node) : + UavcanCDevSensorBridgeBase("uavcan_rangefinder", "/dev/uavcan/rangefinder", RANGE_FINDER_BASE_DEVICE_PATH, + ORB_ID(distance_sensor)), + _sub_range_data(node) +{ } + +int UavcanRangefinderBridge::init() +{ + int res = device::CDev::init(); + + if (res < 0) { + return res; + } + + // Initialize min/max range from params + param_get(param_find("UAVCAN_RNG_MIN"), &_range_min_m); + param_get(param_find("UAVCAN_RNG_MAX"), &_range_max_m); + + res = _sub_range_data.start(RangeCbBinder(this, &UavcanRangefinderBridge::range_sub_cb)); + + if (res < 0) { + DEVICE_LOG("failed to start uavcan sub: %d", res); + return res; + } + + return 0; +} + +void +UavcanRangefinderBridge::range_sub_cb(const + uavcan::ReceivedDataStructure &msg) +{ + uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); + + if (channel == nullptr || channel->class_instance < 0) { + // Something went wrong - no channel to publish on; return + return; + } + + // Cast our generic CDev pointer to the sensor-specific driver class + PX4Rangefinder *rangefinder = (PX4Rangefinder *)channel->h_driver; + + if (rangefinder == nullptr) { + return; + } + + if (!_inited) { + + uint8_t device_type = 0; + + switch (msg.sensor_type) { + case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_SONAR: + device_type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + break; + + case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_RADAR: + device_type = distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR; + break; + + case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_LIDAR: + case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_UNDEFINED: + default: + device_type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + break; + } + + rangefinder->set_device_type(device_type); + rangefinder->set_fov(msg.field_of_view); + rangefinder->set_min_distance(_range_min_m); + rangefinder->set_max_distance(_range_max_m); + + _inited = true; + } + + /* + * FIXME HACK + * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT. + * It stopped working when the time sync feature has been introduced, because it caused libuavcan + * to use an independent time source (based on hardware TIM5) instead of HRT. + * The proper solution is to be developed. + */ + rangefinder->update(hrt_absolute_time(), msg.range); +} + +int UavcanRangefinderBridge::init_driver(uavcan_bridge::Channel *channel) +{ + // update device id as we now know our device node_id + DeviceId device_id{_device_id}; + + device_id.devid_s.devtype = DRV_DIST_DEVTYPE_UAVCAN; + device_id.devid_s.address = static_cast(channel->node_id); + + channel->h_driver = new PX4Rangefinder(device_id.devid, distance_sensor_s::ROTATION_DOWNWARD_FACING); + + if (channel->h_driver == nullptr) { + return PX4_ERROR; + } + + PX4Rangefinder *rangefinder = (PX4Rangefinder *)channel->h_driver; + + channel->class_instance = rangefinder->get_class_instance(); + + if (channel->class_instance < 0) { + PX4_ERR("UavcanRangefinder: Unable to get a class instance"); + delete rangefinder; + channel->h_driver = nullptr; + return PX4_ERROR; + } + + return PX4_OK; +} diff --git a/src/drivers/uavcan/sensors/rangefinder.hpp b/src/drivers/uavcan/sensors/rangefinder.hpp new file mode 100644 index 0000000000..c84e7e2aa5 --- /dev/null +++ b/src/drivers/uavcan/sensors/rangefinder.hpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author RJ Gritter + */ + +#pragma once + +#include "sensor_bridge.hpp" +#include +#include +#include + +#include + +class UavcanRangefinderBridge : public UavcanCDevSensorBridgeBase +{ +public: + static const char *const NAME; + + UavcanRangefinderBridge(uavcan::INode &node); + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + + int init_driver(uavcan_bridge::Channel *channel) override; + + void range_sub_cb(const uavcan::ReceivedDataStructure &msg); + + typedef uavcan::MethodBinder < UavcanRangefinderBridge *, + void (UavcanRangefinderBridge::*) + (const uavcan::ReceivedDataStructure &) > + RangeCbBinder; + + uavcan::Subscriber _sub_range_data; + + float _range_min_m{0.0f}; + float _range_max_m{0.0f}; + + bool _inited{false}; + +}; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index de81d74556..21bd20b19c 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -45,6 +45,7 @@ #include "gnss.hpp" #include "flow.hpp" #include "mag.hpp" +#include "rangefinder.hpp" /* * IUavcanSensorBridge @@ -58,6 +59,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t device_orientation) : + CDev(nullptr), _distance_sensor_pub{ORB_ID(distance_sensor)} { + _class_device_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); + _distance_sensor_pub.advertise(); set_device_id(device_id); @@ -46,6 +49,10 @@ PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t device_or PX4Rangefinder::~PX4Rangefinder() { + if (_class_device_instance != -1) { + unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_device_instance); + } + _distance_sensor_pub.unadvertise(); } diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp index f8dd776e75..d5225d1bd4 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp @@ -35,11 +35,12 @@ #include #include +#include #include #include #include -class PX4Rangefinder +class PX4Rangefinder : public cdev::CDev { public: @@ -63,8 +64,12 @@ public: void update(const hrt_abstime ×tamp_sample, const float distance, const int8_t quality = -1); + int get_class_instance() { return _class_device_instance; }; + private: uORB::PublicationMultiData _distance_sensor_pub; + int _class_device_instance{-1}; + };