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

tfmini move to px4 work queue

Browse Source
This commit is contained in:
Daniel Agar 2019-02-22 10:44:27 -05:00
parent eb8fbaf93d
commit 593aaa48ca
1 changed files with 28 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
src/drivers/distance_sensor/tfmini/tfmini.cpp
View File

@ -43,7 +43,7 @@
*/
#include <px4_config.h>
#include <px4_workqueue.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_getopt.h>
#include <px4_module.h>
@ -82,20 +82,16 @@
/* Configuration Constants */
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
class TFMINI : public cdev::CDev
class TFMINI : public cdev::CDev, public px4::ScheduledWorkItem
{
public:
TFMINI(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
virtual ~TFMINI();
virtual int init();
virtual int init() override;
virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen);
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override;
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
/**
* Diagnostics - print some basic information about the driver.
@ -108,9 +104,8 @@ private:
float _min_distance;
float _max_distance;
int _conversion_interval;
work_s _work{};
ringbuffer::RingBuffer *_reports;
int _measure_ticks;
int _measure_interval;
bool _collect_phase;
int _fd;
char _linebuf[10];
@ -151,18 +146,9 @@ private:
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
void Run() override;
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
/*
@ -172,12 +158,13 @@ extern "C" __EXPORT int tfmini_main(int argc, char *argv[]);
TFMINI::TFMINI(const char *port, uint8_t rotation) :
CDev(RANGE_FINDER0_DEVICE_PATH),
ScheduledWorkItem(px4::wq_configurations::hp_default),
_rotation(rotation),
_min_distance(0.30f),
_max_distance(12.0f),
_conversion_interval(9000),
_reports(nullptr),
_measure_ticks(0),
_measure_interval(0),
_collect_phase(false),
_fd(-1),
_linebuf_index(0),
@ -191,6 +178,7 @@ TFMINI::TFMINI(const char *port, uint8_t rotation) :
{
/* store port name */
strncpy(_port, port, sizeof(_port) - 1);
/* enforce null termination */
_port[sizeof(_port) - 1] = '\0';
}
@ -370,10 +358,10 @@ TFMINI::ioctl(device::file_t *filp, int cmd, unsigned long arg)
/* set default polling rate */
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
bool want_start = (_measure_interval == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(_conversion_interval);
_measure_interval = (_conversion_interval);
/* if we need to start the poll state machine, do it */
if (want_start) {
@ -387,18 +375,18 @@ TFMINI::ioctl(device::file_t *filp, int cmd, unsigned long arg)
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
bool want_start = (_measure_interval == 0);
/* convert hz to tick interval via microseconds */
int ticks = USEC2TICK(1000000 / arg);
int interval = (1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(_conversion_interval)) {
if (interval < _conversion_interval) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
_measure_interval = interval;
/* if we need to start the poll state machine, do it */
if (want_start) {
@ -429,7 +417,7 @@ TFMINI::read(device::file_t *filp, char *buffer, size_t buflen)
}
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
if (_measure_interval > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
@ -567,25 +555,17 @@ TFMINI::start()
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&TFMINI::cycle_trampoline, this, 1);
ScheduleNow();
}
void
TFMINI::stop()
{
work_cancel(HPWORK, &_work);
ScheduleClear();
}
void
TFMINI::cycle_trampoline(void *arg)
{
TFMINI *dev = (TFMINI *)arg;
dev->cycle();
}
void
TFMINI::cycle()
TFMINI::Run()
{
/* fds initialized? */
if (_fd < 0) {
@ -601,11 +581,8 @@ TFMINI::cycle()
if (collect_ret == -EAGAIN) {
/* reschedule to grab the missing bits, time to transmit 9 bytes @ 115200 bps */
work_queue(HPWORK,
&_work,
(worker_t)&TFMINI::cycle_trampoline,
this,
USEC2TICK(87 * 9));
ScheduleDelayed(87 * 9);
return;
}
@ -615,14 +592,10 @@ TFMINI::cycle()
/*
* Is there a collect->measure gap?
*/
if (_measure_ticks > USEC2TICK(_conversion_interval)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&TFMINI::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(_conversion_interval));
if (_measure_interval > (_conversion_interval)) {
/* schedule a fresh cycle call when
* we are ready to measure again */
ScheduleDelayed(_measure_interval - _conversion_interval);
return;
}
@ -632,11 +605,7 @@ TFMINI::cycle()
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&TFMINI::cycle_trampoline,
this,
USEC2TICK(_conversion_interval));
ScheduleDelayed(_conversion_interval);
}
void
@ -645,7 +614,7 @@ TFMINI::print_info()
printf("Using port '%s'\n", _port);
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("poll interval: %d ticks\n", _measure_ticks);
printf("poll interval: %d \n", _measure_interval);
_reports->print_info("report queue");
}