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

lis3mdl move to px4 work queue

Browse Source
This commit is contained in:
Daniel Agar 2019-02-22 10:50:44 -05:00
parent 537c139134
commit dffaf544ab
2 changed files with 22 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
src/drivers/magnetometer/lis3mdl/lis3mdl.cpp
View File

@ -44,8 +44,8 @@
LIS3MDL::LIS3MDL(device::Device *interface, const char *path, enum Rotation rotation) :
CDev("LIS3MDL", path),
ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
_interface(interface),
_work{},
_reports(nullptr),
_scale{},
_last_report{},
@ -58,7 +58,7 @@ LIS3MDL::LIS3MDL(device::Device *interface, const char *path, enum Rotation rota
_continuous_mode_set(false),
_mode(CONTINUOUS),
_rotation(rotation),
_measure_ticks(0),
_measure_interval(0),
_class_instance(-1),
_orb_class_instance(-1),
_range_ga(4.0f),
@ -80,9 +80,6 @@ LIS3MDL::LIS3MDL(device::Device *interface, const char *path, enum Rotation rota
_device_id.devid_s.address = _interface->get_device_address();
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LIS3MDL;
// enable debug() calls
_debug_enabled = false;
// default scaling
_scale.x_offset = 0;
_scale.x_scale = 1.0f;
@ -90,9 +87,6 @@ LIS3MDL::LIS3MDL(device::Device *interface, const char *path, enum Rotation rota
_scale.y_scale = 1.0f;
_scale.z_offset = 0;
_scale.z_scale = 1.0f;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
LIS3MDL::~LIS3MDL()
@ -444,10 +438,10 @@ LIS3MDL::collect()
}
void
LIS3MDL::cycle()
LIS3MDL::Run()
{
/* _measure_ticks == 0 is used as _task_should_exit */
if (_measure_ticks == 0) {
/* _measure_interval == 0 is used as _task_should_exit */
if (_measure_interval == 0) {
return;
}
@ -464,24 +458,12 @@ LIS3MDL::cycle()
DEVICE_DEBUG("measure error");
}
if (_measure_ticks > 0) {
if (_measure_interval > 0) {
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&LIS3MDL::cycle_trampoline,
this,
USEC2TICK(LIS3MDL_CONVERSION_INTERVAL));
ScheduleDelayed(LIS3MDL_CONVERSION_INTERVAL);
}
}
void
LIS3MDL::cycle_trampoline(void *arg)
{
LIS3MDL *dev = (LIS3MDL *)arg;
dev->cycle();
}
int
LIS3MDL::init()
{
@ -524,10 +506,10 @@ LIS3MDL::ioctl(struct file *file_pointer, int cmd, unsigned long arg)
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool not_started = (_measure_ticks == 0);
bool not_started = (_measure_interval == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(LIS3MDL_CONVERSION_INTERVAL);
_measure_interval = (LIS3MDL_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (not_started) {
@ -540,13 +522,13 @@ LIS3MDL::ioctl(struct file *file_pointer, int cmd, unsigned long arg)
/* Uses arg (hz) for a custom poll rate */
default: {
/* do we need to start internal polling? */
bool not_started = (_measure_ticks == 0);
bool not_started = (_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 (not_started) {
@ -621,7 +603,7 @@ LIS3MDL::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
PX4_INFO("poll interval: %u ticks", _measure_ticks);
PX4_INFO("poll interval: %u", _measure_interval);
print_message(_last_report);
_reports->print_info("report queue");
}
@ -659,7 +641,7 @@ LIS3MDL::read(struct file *file_pointer, char *buffer, size_t buffer_len)
}
/* 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.
* Note that we may be pre-empted by the workq thread while we are doing this;
@ -808,16 +790,16 @@ LIS3MDL::start()
set_default_register_values();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&LIS3MDL::cycle_trampoline, this, 1);
ScheduleNow();
}
void
LIS3MDL::stop()
{
if (_measure_ticks > 0) {
if (_measure_interval > 0) {
/* ensure no new items are queued while we cancel this one */
_measure_ticks = 0;
work_cancel(HPWORK, &_work);
_measure_interval = 0;
ScheduleClear();
}
}

20
src/drivers/magnetometer/lis3mdl/lis3mdl.h
View File

@ -48,14 +48,11 @@
#include <lib/conversion/rotation.h>
#include <systemlib/err.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <perf/perf_counter.h>
#include <px4_defines.h>
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
/**
* LIS3MDL internal constants and data structures.
*/
@ -111,7 +108,7 @@ enum OPERATING_MODE {
};
class LIS3MDL : public device::CDev
class LIS3MDL : public device::CDev, public px4::ScheduledWorkItem
{
public:
LIS3MDL(device::Device *interface, const char *path, enum Rotation rotation);
@ -143,7 +140,6 @@ protected:
Device *_interface;
private:
work_s _work;
ringbuffer::RingBuffer *_reports;
@ -165,7 +161,7 @@ private:
enum OPERATING_MODE _mode;
enum Rotation _rotation;
unsigned int _measure_ticks;
unsigned int _measure_interval;
int _class_instance;
int _orb_class_instance;
@ -227,15 +223,7 @@ private:
* and measurement to provide the most recent measurement possible
* at the next interval.
*/
void cycle();
/**
* @brief 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);
void Run() override;
/**
* Issue a measurement command.