From cc62a52553252efd42b6c2def59074aa474d1630 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 26 Apr 2020 21:59:46 -0400 Subject: [PATCH] PX4Rangerfinder: delete unused CDev --- .../distance_sensor/cm8jl65/CM8JL65.cpp | 2 ++ .../distance_sensor/leddar_one/LeddarOne.cpp | 1 + src/drivers/distance_sensor/sf0x/SF0X.cpp | 1 + src/drivers/distance_sensor/tfmini/TFMINI.cpp | 2 ++ .../drivers/rangefinder/PX4Rangefinder.cpp | 27 ++++--------------- .../drivers/rangefinder/PX4Rangefinder.hpp | 10 +++---- 6 files changed, 14 insertions(+), 29 deletions(-) diff --git a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp index 4b591e3f45..5ede856f70 100644 --- a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp +++ b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp @@ -33,6 +33,8 @@ #include "CM8JL65.hpp" +#include + static constexpr unsigned char crc_msb_vector[] = { 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, diff --git a/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp b/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp index 410c04f843..edc447ac3c 100644 --- a/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp +++ b/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp @@ -33,6 +33,7 @@ #include "LeddarOne.hpp" +#include #include #include diff --git a/src/drivers/distance_sensor/sf0x/SF0X.cpp b/src/drivers/distance_sensor/sf0x/SF0X.cpp index 45800e10f4..eefdb781f8 100644 --- a/src/drivers/distance_sensor/sf0x/SF0X.cpp +++ b/src/drivers/distance_sensor/sf0x/SF0X.cpp @@ -33,6 +33,7 @@ #include "SF0X.hpp" +#include #include /* Configuration Constants */ diff --git a/src/drivers/distance_sensor/tfmini/TFMINI.cpp b/src/drivers/distance_sensor/tfmini/TFMINI.cpp index 4e18c849e2..25c5b80d55 100644 --- a/src/drivers/distance_sensor/tfmini/TFMINI.cpp +++ b/src/drivers/distance_sensor/tfmini/TFMINI.cpp @@ -33,6 +33,8 @@ #include "TFMINI.hpp" +#include + TFMINI::TFMINI(const char *port, uint8_t rotation) : ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), _px4_rangefinder(0 /* TODO: device id */, ORB_PRIO_DEFAULT, rotation) diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp index 7bcdf36406..bb8a98ed0a 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp @@ -36,24 +36,13 @@ #include PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t priority, const uint8_t device_orientation) : - CDev(nullptr), _distance_sensor_pub{ORB_ID(distance_sensor), priority} { - _class_device_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - set_device_id(device_id); set_orientation(device_orientation); } -PX4Rangefinder::~PX4Rangefinder() -{ - if (_class_device_instance != -1) { - unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_device_instance); - } -} - -void -PX4Rangefinder::set_device_type(uint8_t device_type) +void PX4Rangefinder::set_device_type(uint8_t device_type) { // TODO: range finders should have device ids @@ -68,18 +57,16 @@ PX4Rangefinder::set_device_type(uint8_t device_type) // _distance_sensor_pub.get().device_id = device_id.devid; } -void -PX4Rangefinder::set_orientation(const uint8_t device_orientation) +void PX4Rangefinder::set_orientation(const uint8_t device_orientation) { _distance_sensor_pub.get().orientation = device_orientation; } -void -PX4Rangefinder::update(const hrt_abstime timestamp, const float distance, const int8_t quality) +void PX4Rangefinder::update(const hrt_abstime ×tamp_sample, const float distance, const int8_t quality) { distance_sensor_s &report = _distance_sensor_pub.get(); - report.timestamp = timestamp; + report.timestamp = timestamp_sample; report.current_distance = distance; report.signal_quality = quality; @@ -93,10 +80,6 @@ PX4Rangefinder::update(const hrt_abstime timestamp, const float distance, const _distance_sensor_pub.update(); } -void -PX4Rangefinder::print_status() +void PX4Rangefinder::print_status() { - PX4_INFO(RANGE_FINDER_BASE_DEVICE_PATH " device instance: %d", _class_device_instance); - - print_message(_distance_sensor_pub.get()); } diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp index bbef352384..a405bc75e3 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp @@ -35,20 +35,18 @@ #include #include -#include #include -#include #include #include -class PX4Rangefinder : public cdev::CDev +class PX4Rangefinder { public: PX4Rangefinder(const uint32_t device_id, const uint8_t priority = ORB_PRIO_DEFAULT, const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING); - ~PX4Rangefinder() override; + ~PX4Rangefinder() = default; void print_status(); @@ -66,12 +64,10 @@ public: void set_orientation(const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING); - void update(const hrt_abstime timestamp, const float distance, const int8_t quality = -1); + void update(const hrt_abstime ×tamp_sample, const float distance, const int8_t quality = -1); private: uORB::PublicationMultiData _distance_sensor_pub; - int _class_device_instance{-1}; - };