fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 09:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

leddar_one move to px4 work queue

Browse Source
This commit is contained in:
Daniel Agar
2019-05-24 12:58:55 -04:00
parent 9a0f145fc0
commit 4d41dcaee2
1 changed files with 12 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/leddar_one/leddar_one.cpp
+12 -16
View File
@@ -32,7 +32,7 @@
****************************************************************************/ ****************************************************************************/
#include <px4_config.h> #include <px4_config.h>
#include <px4_workqueue.h> #include <px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_getopt.h> #include <px4_getopt.h>
#include <px4_defines.h> #include <px4_defines.h>
#include <px4_module.h> #include <px4_module.h>
@@ -109,20 +109,19 @@ struct __attribute__((__packed__)) reading_msg {
uint16_t crc; /* little-endian */ uint16_t crc; /* little-endian */
}; };
class leddar_one : public cdev::CDev class leddar_one : public cdev::CDev, public px4::ScheduledWorkItem
{ {
public: public:
leddar_one(const char *device_path, const char *serial_port, uint8_t rotation); leddar_one(const char *device_path, const char *serial_port, uint8_t rotation);
virtual ~leddar_one(); virtual ~leddar_one();
virtual int init(); virtual int init() override;
void start(); void start();
void stop(); void stop();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
private: private:
work_s _work;
int _fd = -1; int _fd = -1;
const char *_serial_port; const char *_serial_port;
@@ -151,7 +150,7 @@ private:
void _publish(uint16_t distance_cm); void _publish(uint16_t distance_cm);
int _cycle(); int _cycle();
static void cycle_trampoline(void *arg); void Run() override;
}; };
extern "C" __EXPORT int leddar_one_main(int argc, char *argv[]); extern "C" __EXPORT int leddar_one_main(int argc, char *argv[]);
@@ -269,12 +268,12 @@ int leddar_one_main(int argc, char *argv[])
leddar_one::leddar_one(const char *device_path, const char *serial_port, uint8_t rotation): leddar_one::leddar_one(const char *device_path, const char *serial_port, uint8_t rotation):
CDev(device_path), CDev(device_path),
ScheduledWorkItem(px4::wq_configurations::hp_default),
_rotation(rotation), _rotation(rotation),
_collect_timeout_perf(perf_alloc(PC_COUNT, "leddar_one_collect_timeout")), _collect_timeout_perf(perf_alloc(PC_COUNT, "leddar_one_collect_timeout")),
_comm_error(perf_alloc(PC_COUNT, "leddar_one_comm_errors")), _comm_error(perf_alloc(PC_COUNT, "leddar_one_comm_errors")),
_sample_perf(perf_alloc(PC_ELAPSED, "leddar_one_sample")) _sample_perf(perf_alloc(PC_ELAPSED, "leddar_one_sample"))
{ {
memset(&_work, 0, sizeof(_work));
_serial_port = strdup(serial_port); _serial_port = strdup(serial_port);
} }
@@ -394,7 +393,7 @@ int leddar_one::init()
void void
leddar_one::stop() leddar_one::stop()
{ {
work_cancel(HPWORK, &_work); ScheduleClear();
} }
void leddar_one::start() void leddar_one::start()
@@ -407,23 +406,20 @@ void leddar_one::start()
::close(_fd); ::close(_fd);
_fd = -1; _fd = -1;
work_queue(HPWORK, &_work, (worker_t)&leddar_one::cycle_trampoline, this, USEC2TICK(WORK_USEC_INTERVAL)); ScheduleDelayed(WORK_USEC_INTERVAL);
} }
void void
leddar_one::cycle_trampoline(void *arg) leddar_one::Run()
{ {
leddar_one *dev = reinterpret_cast<leddar_one * >(arg); if (_fd != -1) {
_cycle();
if (dev->_fd != -1) {
dev->_cycle();
} else { } else {
dev->_fd_open(); _fd_open();
} }
work_queue(HPWORK, &(dev->_work), (worker_t)&leddar_one::cycle_trampoline, ScheduleDelayed(WORK_USEC_INTERVAL);
dev, USEC2TICK(WORK_USEC_INTERVAL));
} }
void leddar_one::_publish(uint16_t distance_mm) void leddar_one::_publish(uint16_t distance_mm)