fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 12:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

vl53lxx move to px4 work queue

Browse Source
This commit is contained in:
Daniel Agar
2019-05-24 12:58:55 -04:00
parent 593aaa48ca
commit 835a38f6dc
1 changed files with 25 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp
+25 -64
View File
@@ -41,7 +41,7 @@
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_getopt.h>
#include <px4_workqueue.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <drivers/device/i2c.h>
@@ -101,11 +101,7 @@
#define VL53LXX_MAX_RANGING_DISTANCE 2.0f
#define VL53LXX_MIN_RANGING_DISTANCE 0.0f
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
class VL53LXX : public device::I2C
class VL53LXX : public device::I2C, public px4::ScheduledWorkItem
{
public:
VL53LXX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING,
@@ -113,11 +109,11 @@ public:
virtual ~VL53LXX();
virtual int init();
virtual int init() override;
virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen);
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);
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
/**
* Diagnostics - print some basic information about the driver.
@@ -125,14 +121,13 @@ public:
void print_info();
protected:
virtual int probe();
virtual int probe() override;
private:
uint8_t _rotation;
work_s _work;
ringbuffer::RingBuffer *_reports;
bool _sensor_ok;
int _measure_ticks;
int _measure_interval;
bool _collect_phase;
bool _new_measurement;
bool _measurement_started;
@@ -162,7 +157,7 @@ private:
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
void Run() override;
int measure();
int collect();
@@ -176,15 +171,6 @@ private:
bool spadCalculations();
bool sensorTuning();
bool singleRefCalibration(uint8_t byte);
/**
* 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);
};
@@ -195,10 +181,11 @@ extern "C" __EXPORT int vl53lxx_main(int argc, char *argv[]);
VL53LXX::VL53LXX(uint8_t rotation, int bus, int address) :
I2C("VL53LXX", VL53LXX_DEVICE_PATH, bus, address, 400000),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
_rotation(rotation),
_reports(nullptr),
_sensor_ok(false),
_measure_ticks(0),
_measure_interval(0),
_collect_phase(false),
_new_measurement(true),
_measurement_started(false),
@@ -211,12 +198,6 @@ VL53LXX::VL53LXX(uint8_t rotation, int bus, int address) :
{
// up the retries since the device misses the first measure attempts
I2C::_retries = 3;
// enable debug() calls
_debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
VL53LXX::~VL53LXX()
@@ -351,10 +332,10 @@ VL53LXX::ioctl(device::file_t *filp, int cmd, unsigned long arg)
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(VL53LXX_SAMPLE_RATE);
_measure_interval = (VL53LXX_SAMPLE_RATE);
/* if we need to start the poll state machine, do it */
if (want_start) {
@@ -369,13 +350,13 @@ VL53LXX::ioctl(device::file_t *filp, int cmd, unsigned long arg)
/* adjust to a legal polling interval in Hz */
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 */
unsigned ticks = USEC2TICK(1000000 / arg);
unsigned interval = (1000000 / arg);
/* 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) {
@@ -407,7 +388,7 @@ VL53LXX::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.
@@ -585,8 +566,8 @@ VL53LXX::measure()
readRegister(SYSRANGE_START_REG, system_start);
if ((system_start & 0x01) == 1) {
work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this,
USEC2TICK(VL53LXX_US)); // reschedule every 1 ms until measurement is ready
ScheduleDelayed(VL53LXX_US);
ret = OK;
return ret;
@@ -601,8 +582,7 @@ VL53LXX::measure()
readRegister(SYSRANGE_START_REG, system_start);
if ((system_start & 0x01) == 1) {
work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this,
USEC2TICK(VL53LXX_US)); // reschedule every 1 ms until measurement is ready
ScheduleDelayed(VL53LXX_US);
ret = OK;
return ret;
@@ -614,8 +594,7 @@ VL53LXX::measure()
readRegister(RESULT_INTERRUPT_STATUS_REG, wait_for_measurement);
if ((wait_for_measurement & 0x07) == 0) {
work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this,
USEC2TICK(VL53LXX_US)); // reschedule every 1 ms until measurement is ready
ScheduleDelayed(VL53LXX_US); // reschedule every 1 ms until measurement is ready
ret = OK;
return ret;
}
@@ -699,28 +678,17 @@ VL53LXX::start()
_reports->flush();
/* schedule a cycle to start things */
work_queue(LPWORK, &_work, (worker_t)&VL53LXX::cycle_trampoline, this, USEC2TICK(VL53LXX_US));
ScheduleDelayed(VL53LXX_US);
}
void
VL53LXX::stop()
{
work_cancel(LPWORK, &_work);
ScheduleClear();
}
void
VL53LXX::cycle_trampoline(void *arg)
{
VL53LXX *dev = (VL53LXX *)arg;
dev->cycle();
}
void
VL53LXX::cycle()
VL53LXX::Run()
{
measure();
@@ -731,26 +699,19 @@ VL53LXX::cycle()
collect();
work_queue(LPWORK,
&_work,
(worker_t)&VL53LXX::cycle_trampoline,
this,
_measure_ticks);
ScheduleDelayed(_measure_interval);
}
}
void
VL53LXX::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("poll interval: %u ticks\n", _measure_ticks);
printf("poll interval: %u\n", _measure_interval);
_reports->print_info("report queue");
}
bool
VL53LXX::spadCalculations()
{