fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Modify the LeddarOne driver class to utilize the PX4RangeFinder library.

Browse Source
This commit is contained in:
mcsauder 2019-09-01 19:01:12 -06:00 committed by Julian Oes
parent 94d39e4466
commit ddd9a97d42
3 changed files with 56 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
src/drivers/distance_sensor/leddar_one/leddar_one.cpp
View File

@ -51,11 +51,11 @@
using namespace time_literals;
#define DEVICE_PATH "/dev/LeddarOne"
#define LEDDAR_ONE_DEFAULT_SERIAL_PORT "/dev/ttyS3"
#define DEVICE_PATH "/dev/LeddarOne"
#define LEDDAR_ONE_DEFAULT_SERIAL_PORT "/dev/ttyS3"
#define MAX_DISTANCE 40.0f
#define MIN_DISTANCE 0.01f
#define LEDDAR_ONE_MAX_DISTANCE 40.0f
#define LEDDAR_ONE_MIN_DISTANCE 0.01f
#define LEDDAR_ONE_MEASURE_INTERVAL 100_ms // 10Hz
@ -107,7 +107,7 @@ class LeddarOne : public cdev::CDev, public px4::ScheduledWorkItem
public:
LeddarOne(const char *device_path,
const char *serial_port = LEDDAR_ONE_DEFAULT_SERIAL_PORT,
const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
virtual ~LeddarOne();
virtual int init() override;
@ -156,12 +156,15 @@ private:
const char *_serial_port{nullptr};
PX4Rangefinder _px4_rangefinder;
int _file_descriptor{-1};
int _orb_class_instance{-1};
uint8_t _buffer[sizeof(reading_msg)];
uint8_t _buffer_len{0};
uint8_t _rotation{distance_sensor_s::ROTATION_DOWNWARD_FACING};
hrt_abstime _measurement_time{0};
perf_counter_t _comms_error{perf_alloc(PC_COUNT, "leddar_one_comms_error")};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "leddar_one_sample")};
@ -169,12 +172,18 @@ private:
orb_advert_t _distance_sensor_topic{nullptr};
};
LeddarOne::LeddarOne(const char *device_path, const char *serial_port, uint8_t rotation):
LeddarOne::LeddarOne(const char *device_path, const char *serial_port, uint8_t device_orientation):
CDev(device_path),
ScheduledWorkItem(px4::wq_configurations::hp_default),
_rotation(rotation)
_px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, device_orientation)
{
_serial_port = strdup(serial_port);
_px4_rangefinder.set_device_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
_px4_rangefinder.set_max_distance(LEDDAR_ONE_MAX_DISTANCE);
_px4_rangefinder.set_min_distance(LEDDAR_ONE_MIN_DISTANCE);
_px4_rangefinder.set_fov(0.008); // Divergence 8 mRadian
_px4_rangefinder.set_orientation(device_orientation);
}
LeddarOne::~LeddarOne()
@ -256,19 +265,12 @@ LeddarOne::collect()
// NOTE: little-endian support only.
uint16_t distance_mm = (msg->first_dist_high_byte << 8 | msg->first_dist_low_byte);
float distance_m = static_cast<float>(distance_mm) / 1000.0f;
distance_sensor_s report = {};
report.current_distance = static_cast<float>(distance_mm) / 1000.0f;
report.id = 0;
report.max_distance = MAX_DISTANCE;
report.min_distance = MIN_DISTANCE;
report.orientation = _rotation;
report.signal_quality = -1;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.variance = 0.0f;
// @TODO - implement a meaningful signal quality value.
int8_t signal_quality = -1;
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
_px4_rangefinder.update(_measurement_time, distance_m, signal_quality);
perf_end(_sample_perf);
@ -288,15 +290,6 @@ LeddarOne::init()
return PX4_ERROR;
}
// Get a publish handle on the range finder topic.
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 == nullptr) {
PX4_ERR("failed to create distance_sensor object");
}
hrt_abstime time_now = hrt_absolute_time();
const hrt_abstime timeout_usec = time_now + 500000_us; // 0.5sec
@ -334,6 +327,7 @@ LeddarOne::measure()
return PX4_ERROR;
}
_measurement_time = hrt_absolute_time();
_buffer_len = 0;
return PX4_OK;
}

22
src/lib/drivers/rangefinder/PX4Rangefinder.cpp
View File

@ -35,16 +35,14 @@
#include <lib/drivers/device/Device.hpp>
PX4Rangefinder::PX4Rangefinder(uint32_t device_id, uint8_t priority, uint8_t rotation) :
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);
// TODO: range finders should have device ids
//_distance_sensor_pub.get().device_id = device_id;
_distance_sensor_pub.get().orientation = rotation;
set_device_id(device_id);
set_orientation(device_orientation);
}
PX4Rangefinder::~PX4Rangefinder()
@ -55,7 +53,7 @@ PX4Rangefinder::~PX4Rangefinder()
}
void
PX4Rangefinder::set_device_type(uint8_t devtype)
PX4Rangefinder::set_device_type(uint8_t device_type)
{
// TODO: range finders should have device ids
@ -71,13 +69,19 @@ PX4Rangefinder::set_device_type(uint8_t devtype)
}
void
PX4Rangefinder::update(hrt_abstime timestamp, float distance, int8_t quality)
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)
{
distance_sensor_s &report = _distance_sensor_pub.get();
report.timestamp = timestamp;
report.current_distance = distance;
report.signal_quality = quality;
report.signal_quality = quality;
report.timestamp = timestamp;
_distance_sensor_pub.update();
}

36
src/lib/drivers/rangefinder/PX4Rangefinder.hpp
View File

@ -45,27 +45,33 @@ class PX4Rangefinder : public cdev::CDev
{
public:
PX4Rangefinder(uint32_t device_id, uint8_t priority, uint8_t rotation);
PX4Rangefinder(const uint32_t device_id,
const uint8_t priority,
const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
~PX4Rangefinder() override;
void set_device_type(uint8_t devtype);
//void set_error_count(uint64_t error_count) { _distance_sensor_pub.get().error_count = error_count; }
void set_min_distance(float distance) { _distance_sensor_pub.get().min_distance = distance; }
void set_max_distance(float distance) { _distance_sensor_pub.get().max_distance = distance; }
void set_hfov(float fov) { _distance_sensor_pub.get().h_fov = fov; }
void set_vfov(float fov) { _distance_sensor_pub.get().v_fov = fov; }
void set_fov(float fov) { set_hfov(fov); set_vfov(fov); }
void update(hrt_abstime timestamp, float distance, int8_t quality = -1);
void print_status();
void set_device_type(uint8_t device_type);
//void set_error_count(uint64_t error_count) { _distance_sensor_pub.get().error_count = error_count; }
void set_device_id(const uint8_t device_id) { _distance_sensor_pub.get().id = device_id; };
void set_fov(const float fov) { set_hfov(fov); set_vfov(fov); }
void set_hfov(const float fov) { _distance_sensor_pub.get().h_fov = fov; }
void set_vfov(const float fov) { _distance_sensor_pub.get().v_fov = fov; }
void set_max_distance(const float distance) { _distance_sensor_pub.get().max_distance = distance; }
void set_min_distance(const float distance) { _distance_sensor_pub.get().min_distance = distance; }
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);
private:
uORB::PublicationMultiData<distance_sensor_s> _distance_sensor_pub;
uORB::PublicationMultiData<distance_sensor_s> _distance_sensor_pub;
int _class_device_instance{-1};
int _class_device_instance{-1};
};