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

qmc5883 move to px4 work queue

Browse Source
This commit is contained in:
Daniel Agar 2019-03-15 00:05:11 -04:00
parent 5e427b4e46
commit 8e6708bdcb
2 changed files with 25 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/drivers/magnetometer/qmc5883/CMakeLists.txt
View File

@ -40,4 +40,6 @@ px4_add_module(
qmc5883_i2c.cpp
qmc5883_spi.cpp
qmc5883.cpp
DEPENDS
px4_work_queue
)

83
src/drivers/magnetometer/qmc5883/qmc5883.cpp
View File

@ -56,7 +56,7 @@
#include <unistd.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <nuttx/clock.h>
#include <board_config.h>
@ -134,11 +134,7 @@ enum QMC5883_BUS {
QMC5883_BUS_SPI
};
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
class QMC5883 : public device::CDev
class QMC5883 : public device::CDev, public px4::ScheduledWorkItem
{
public:
QMC5883(device::Device *interface, const char *path, enum Rotation rotation);
@ -163,8 +159,7 @@ protected:
Device *_interface;
private:
work_s _work{};
unsigned _measure_ticks;
unsigned _measure_interval{0};
ringbuffer::RingBuffer *_reports;
struct mag_calibration_s _scale;
@ -228,15 +223,7 @@ private:
* and measurement to provide the most recent measurement possible
* at the next interval.
*/
void cycle();
/**
* 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;
/**
* Write a register.
@ -274,8 +261,8 @@ extern "C" __EXPORT int qmc5883_main(int argc, char *argv[]);
QMC5883::QMC5883(device::Device *interface, const char *path, enum Rotation rotation) :
CDev("QMC5883", path),
ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
_interface(interface),
_measure_ticks(0),
_reports(nullptr),
_scale{},
_range_scale(1.0f / 12000.0f),
@ -301,9 +288,6 @@ QMC5883::QMC5883(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_QMC5883;
// enable debug() calls
_debug_enabled = false;
// default scaling
_scale.x_offset = 0;
_scale.x_scale = 1.0f;
@ -311,9 +295,6 @@ QMC5883::QMC5883(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));
}
QMC5883::~QMC5883()
@ -407,7 +388,7 @@ QMC5883::read(struct file *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.
* Note that we may be pre-empted by the workq thread while we are doing this;
@ -462,10 +443,10 @@ QMC5883::ioctl(struct file *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(QMC5883_CONVERSION_INTERVAL);
_measure_interval = QMC5883_CONVERSION_INTERVAL;
/* if we need to start the poll state machine, do it */
if (want_start) {
@ -478,18 +459,18 @@ QMC5883::ioctl(struct file *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(QMC5883_CONVERSION_INTERVAL)) {
if (interval < QMC5883_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) {
@ -535,16 +516,16 @@ QMC5883::start()
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&QMC5883::cycle_trampoline, this, 1);
ScheduleNow();
}
void
QMC5883::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();
}
}
@ -558,7 +539,6 @@ QMC5883::reset()
/* software reset */
write_reg(QMC5883_ADDR_CONTROL_2, QMC5883_SOFT_RESET);
/* set reset period to 0x01 */
write_reg(QMC5883_ADDR_SET_RESET, QMC5883_SET_DEFAULT);
@ -578,17 +558,9 @@ QMC5883::reset()
}
void
QMC5883::cycle_trampoline(void *arg)
QMC5883::Run()
{
QMC5883 *dev = (QMC5883 *)arg;
dev->cycle();
}
void
QMC5883::cycle()
{
if (_measure_ticks == 0) {
if (_measure_interval == 0) {
return;
}
@ -609,14 +581,9 @@ QMC5883::cycle()
/*
* Is there a collect->measure gap?
*/
if (_measure_ticks > USEC2TICK(QMC5883_CONVERSION_INTERVAL)) {
if (_measure_interval > QMC5883_CONVERSION_INTERVAL) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&QMC5883::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(QMC5883_CONVERSION_INTERVAL));
ScheduleDelayed(_measure_interval - QMC5883_CONVERSION_INTERVAL);
return;
}
@ -625,13 +592,9 @@ QMC5883::cycle()
/* next phase is collection */
_collect_phase = true;
if (_measure_ticks > 0) {
if (_measure_interval > 0) {
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&QMC5883::cycle_trampoline,
this,
USEC2TICK(QMC5883_CONVERSION_INTERVAL));
ScheduleDelayed(QMC5883_CONVERSION_INTERVAL);
}
}
@ -829,7 +792,7 @@ QMC5883::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
printf("poll interval: %u ticks\n", _measure_ticks);
printf("poll interval: %u us\n", _measure_interval);
print_message(_last_report);
_reports->print_info("report queue");
}