Skip to content

Commit

Permalink
PX4Rangerfinder: delete unused CDev
Browse files Browse the repository at this point in the history
  • Loading branch information
dagar committed Apr 27, 2020
1 parent 5739cf2 commit cc62a52
Show file tree
Hide file tree
Showing 6 changed files with 14 additions and 29 deletions.
2 changes: 2 additions & 0 deletions src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,8 @@

#include "CM8JL65.hpp"

#include <fcntl.h>

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,
Expand Down
1 change: 1 addition & 0 deletions src/drivers/distance_sensor/leddar_one/LeddarOne.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@

#include "LeddarOne.hpp"

#include <fcntl.h>
#include <stdlib.h>
#include <string.h>

Expand Down
1 change: 1 addition & 0 deletions src/drivers/distance_sensor/sf0x/SF0X.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@

#include "SF0X.hpp"

#include <fcntl.h>
#include <termios.h>

/* Configuration Constants */
Expand Down
2 changes: 2 additions & 0 deletions src/drivers/distance_sensor/tfmini/TFMINI.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,8 @@

#include "TFMINI.hpp"

#include <fcntl.h>

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)
Expand Down
27 changes: 5 additions & 22 deletions src/lib/drivers/rangefinder/PX4Rangefinder.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,24 +36,13 @@
#include <lib/drivers/device/Device.hpp>

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

Expand All @@ -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 &timestamp_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;

Expand All @@ -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());
}
10 changes: 3 additions & 7 deletions src/lib/drivers/rangefinder/PX4Rangefinder.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,20 +35,18 @@

#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <lib/cdev/CDev.hpp>
#include <lib/conversion/rotation.h>
#include <uORB/uORB.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/distance_sensor.h>

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();

Expand All @@ -66,12 +64,10 @@ class PX4Rangefinder : public cdev::CDev

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 &timestamp_sample, const float distance, const int8_t quality = -1);

private:

uORB::PublicationMultiData<distance_sensor_s> _distance_sensor_pub;

int _class_device_instance{-1};

};

0 comments on commit cc62a52

Please sign in to comment.