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

ll40ls move to px4 work queue

Browse Source
This commit is contained in:
Daniel Agar
2019-05-24 12:58:55 -04:00
parent 4d41dcaee2
commit 22b89034d9
7 changed files with 37 additions and 122 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/ll40ls/LidarLite.cpp
+6 -38
View File
@@ -41,38 +41,6 @@
#include "LidarLite.h"
LidarLite::LidarLite() :
_min_distance(LL40LS_MIN_DISTANCE),
_max_distance(LL40LS_MAX_DISTANCE_V3),
_measure_ticks(0)
{
}
void LidarLite::set_minimum_distance(const float min)
{
_min_distance = min;
}
void LidarLite::set_maximum_distance(const float max)
{
_max_distance = max;
}
float LidarLite::get_minimum_distance() const
{
return _min_distance;
}
float LidarLite::get_maximum_distance() const
{
return _max_distance;
}
uint32_t LidarLite::getMeasureTicks() const
{
return _measure_ticks;
}
int LidarLite::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
@@ -87,10 +55,10 @@ int LidarLite::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(LL40LS_CONVERSION_INTERVAL);
_measure_interval = (LL40LS_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start) {
@@ -103,18 +71,18 @@ int LidarLite::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);
/* check against maximum rate */
if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
if (interval < (LL40LS_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) {
src/drivers/distance_sensor/ll40ls/LidarLite.h
+9 -10
View File
@@ -57,8 +57,7 @@
class LidarLite
{
public:
LidarLite();
LidarLite() = default;
virtual ~LidarLite() = default;
virtual int init() = 0;
@@ -89,12 +88,12 @@ protected:
* range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE
* and LL40LS_MAX_DISTANCE_V3
*/
void set_minimum_distance(const float min);
void set_maximum_distance(const float max);
float get_minimum_distance() const;
float get_maximum_distance() const;
void set_minimum_distance(const float min) { _min_distance = min; }
void set_maximum_distance(const float max) { _max_distance = max; }
float get_minimum_distance() const { return _min_distance; }
float get_maximum_distance() const { return _max_distance; }
uint32_t getMeasureTicks() const;
uint32_t getMeasureInterval() const { return _measure_interval; }
virtual int measure() = 0;
virtual int collect() = 0;
@@ -102,7 +101,7 @@ protected:
virtual int reset_sensor() = 0;
private:
float _min_distance;
float _max_distance;
uint32_t _measure_ticks;
float _min_distance{LL40LS_MIN_DISTANCE};
float _max_distance{LL40LS_MAX_DISTANCE_V3};
uint32_t _measure_interval{0};
};
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
+9 -27
View File
@@ -52,8 +52,8 @@
LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int address) :
I2C("LL40LS", path, bus, address, 100000),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
_rotation(rotation),
_work{},
_reports(nullptr),
_sensor_ok(false),
_collect_phase(false),
@@ -74,9 +74,6 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, uint8_t rotation, int addr
{
// up the retries since the device misses the first measure attempts
_retries = 3;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
LidarLiteI2C::~LidarLiteI2C()
@@ -254,7 +251,7 @@ ssize_t LidarLiteI2C::read(device::file_t *filp, char *buffer, size_t buflen)
}
/* if automatic measurement is enabled */
if (getMeasureTicks() > 0) {
if (getMeasureInterval() > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
@@ -570,22 +567,15 @@ void LidarLiteI2C::start()
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&LidarLiteI2C::cycle_trampoline, this, 1);
ScheduleNow();
}
void LidarLiteI2C::stop()
{
work_cancel(HPWORK, &_work);
ScheduleClear();
}
void LidarLiteI2C::cycle_trampoline(void *arg)
{
LidarLiteI2C *dev = (LidarLiteI2C *)arg;
dev->cycle();
}
void LidarLiteI2C::cycle()
void LidarLiteI2C::Run()
{
/* collection phase? */
if (_collect_phase) {
@@ -607,14 +597,10 @@ void LidarLiteI2C::cycle()
/*
* Is there a collect->measure gap?
*/
if (getMeasureTicks() > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
if (getMeasureInterval() > (LL40LS_CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&LidarLiteI2C::cycle_trampoline,
this,
getMeasureTicks() - USEC2TICK(LL40LS_CONVERSION_INTERVAL));
ScheduleDelayed(getMeasureInterval() - LL40LS_CONVERSION_INTERVAL);
return;
}
@@ -635,11 +621,7 @@ void LidarLiteI2C::cycle()
}
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&LidarLiteI2C::cycle_trampoline,
this,
USEC2TICK(LL40LS_CONVERSION_INTERVAL));
ScheduleDelayed(LL40LS_CONVERSION_INTERVAL);
}
void LidarLiteI2C::print_info()
@@ -648,7 +630,7 @@ void LidarLiteI2C::print_info()
perf_print_counter(_comms_errors);
perf_print_counter(_sensor_resets);
perf_print_counter(_sensor_zero_resets);
printf("poll interval: %u ticks\n", getMeasureTicks());
printf("poll interval: %u \n", getMeasureInterval());
_reports->print_info("report queue");
printf("distance: %ucm (0x%04x)\n",
(unsigned)_last_distance, (unsigned)_last_distance);
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h
+3 -12
View File
@@ -42,7 +42,7 @@
#include "LidarLite.h"
#include <px4_workqueue.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <perf/perf_counter.h>
@@ -78,7 +78,7 @@
#define LL40LS_PEAK_STRENGTH_LOW 135 // Minimum peak strength raw value for accepting a measurement
#define LL40LS_PEAK_STRENGTH_HIGH 234 // Max peak strength raw value
class LidarLiteI2C : public LidarLite, public device::I2C
class LidarLiteI2C : public LidarLite, public device::I2C, public px4::ScheduledWorkItem
{
public:
LidarLiteI2C(int bus, const char *path,
@@ -113,7 +113,6 @@ protected:
private:
uint8_t _rotation;
work_s _work;
ringbuffer::RingBuffer *_reports;
bool _sensor_ok;
bool _collect_phase;
@@ -173,17 +172,9 @@ private:
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
void Run() override;
int collect() override;
/**
* 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);
private:
LidarLiteI2C(const LidarLiteI2C &copy) = delete;
LidarLiteI2C operator=(const LidarLiteI2C &assignment) = delete;
src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp
+7 -18
View File
@@ -51,8 +51,8 @@
LidarLitePWM::LidarLitePWM(const char *path, uint8_t rotation) :
CDev(path),
ScheduledWorkItem(px4::wq_configurations::hp_default),
_rotation(rotation),
_work{},
_reports(nullptr),
_class_instance(-1),
_orb_class_instance(-1),
@@ -122,7 +122,7 @@ void LidarLitePWM::print_info()
perf_print_counter(_sample_perf);
perf_print_counter(_read_errors);
perf_print_counter(_sensor_zero_resets);
PX4_INFO("poll interval: %u ticks", getMeasureTicks());
PX4_INFO("poll interval: %u ", getMeasureInterval());
print_message(_range);
}
@@ -135,31 +135,20 @@ void LidarLitePWM::print_registers()
void LidarLitePWM::start()
{
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&LidarLitePWM::cycle_trampoline, this, 1);
ScheduleNow();
}
void LidarLitePWM::stop()
{
work_cancel(HPWORK, &_work);
ScheduleClear();
}
void LidarLitePWM::cycle_trampoline(void *arg)
{
LidarLitePWM *dev = (LidarLitePWM *)arg;
dev->cycle();
}
void LidarLitePWM::cycle()
void LidarLitePWM::Run()
{
measure();
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&LidarLitePWM::cycle_trampoline,
this,
getMeasureTicks());
ScheduleDelayed(getMeasureInterval());
}
int LidarLitePWM::measure()
@@ -213,7 +202,7 @@ ssize_t LidarLitePWM::read(device::file_t *filp, char *buffer, size_t buflen)
}
/* if automatic measurement is enabled */
if (getMeasureTicks() > 0) {
if (getMeasureInterval() > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
src/drivers/distance_sensor/ll40ls/LidarLitePWM.h
+3 -13
View File
@@ -45,8 +45,7 @@
#include "LidarLite.h"
#include <px4_workqueue.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <drivers/device/ringbuffer.h>
#include <perf/perf_counter.h>
@@ -57,7 +56,7 @@
class LidarLitePWM : public LidarLite, public cdev::CDev
class LidarLitePWM : public LidarLite, public cdev::CDev, public px4::ScheduledWorkItem
{
public:
LidarLitePWM(const char *path, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
@@ -72,7 +71,7 @@ public:
void stop() override;
void cycle();
void Run() override;
/**
* @brief
@@ -86,14 +85,6 @@ public:
*/
void print_registers() override;
/**
* 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);
const char *get_dev_name() override;
protected:
@@ -108,7 +99,6 @@ protected:
private:
uint8_t _rotation;
work_s _work;
ringbuffer::RingBuffer *_reports;
int _class_instance;
int _orb_class_instance;
src/drivers/distance_sensor/ll40ls/ll40ls.cpp
-4
View File
@@ -51,10 +51,6 @@
#include <stdio.h>
#include <px4_getopt.h>
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
#define LL40LS_DEVICE_PATH_PWM "/dev/ll40ls_pwm"
enum LL40LS_BUS {