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

mpl3115a2 move to px4 work queue

Browse Source
This commit is contained in:
Daniel Agar 2019-02-22 10:40:45 -05:00
parent b8befe36b9
commit e73dd73ea7
1 changed files with 30 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

92
src/drivers/barometer/mpl3115a2/mpl3115a2.cpp
View File

@ -57,7 +57,7 @@
#include <px4_getopt.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <nuttx/clock.h>
#include <arch/board/board.h>
@ -81,10 +81,6 @@ enum MPL3115A2_BUS {
MPL3115A2_BUS_I2C_EXTERNAL,
};
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
@ -101,7 +97,7 @@ enum MPL3115A2_BUS {
#define MPL3115A2_BARO_DEVICE_PATH_EXT "/dev/mpl3115a2_ext"
#define MPL3115A2_BARO_DEVICE_PATH_INT "/dev/mpl3115a2_int"
class MPL3115A2 : public cdev::CDev
class MPL3115A2 : public cdev::CDev, public px4::ScheduledWorkItem
{
public:
MPL3115A2(device::Device *interface, const char *path);
@ -120,8 +116,7 @@ public:
protected:
device::Device *_interface;
struct work_s _work;
unsigned _measure_ticks;
unsigned _measure_interval;
ringbuffer::RingBuffer *_reports;
bool _collect_phase;
@ -141,17 +136,17 @@ protected:
/**
* Initialize the automatic measurement state machine and start it.
*
* @param delay_ticks the number of queue ticks before executing the next cycle
* @param delay_us the number of microseconds before executing the next cycle
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start_cycle(unsigned delay_ticks = 1);
void start(unsigned delay_us = 1);
/**
* Stop the automatic measurement state machine.
*/
void stop_cycle();
void stop();
/**
* Perform a poll cycle; collect from the previous measurement
@ -166,15 +161,7 @@ protected:
* 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;
/**
* Issue a measurement command for the current state.
@ -197,8 +184,9 @@ extern "C" __EXPORT int mpl3115a2_main(int argc, char *argv[]);
MPL3115A2::MPL3115A2(device::Device *interface, const char *path) :
CDev(path),
ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
_interface(interface),
_measure_ticks(0),
_measure_interval(0),
_reports(nullptr),
_collect_phase(false),
_P(0),
@ -210,16 +198,13 @@ MPL3115A2::MPL3115A2(device::Device *interface, const char *path) :
_measure_perf(perf_alloc(PC_ELAPSED, "mpl3115a2_measure")),
_comms_errors(perf_alloc(PC_COUNT, "mpl3115a2_com_err"))
{
// work_cancel in stop_cycle called from the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
_interface->set_device_type(DRV_BARO_DEVTYPE_MPL3115A2);
}
MPL3115A2::~MPL3115A2()
{
/* make sure we are truly inactive */
stop_cycle();
stop();
if (_class_instance != -1) {
unregister_class_devname(get_devname(), _class_instance);
@ -332,7 +317,7 @@ MPL3115A2::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.
@ -398,14 +383,14 @@ MPL3115A2::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(MPL3115A2_CONVERSION_INTERVAL);
_measure_interval = MPL3115A2_CONVERSION_INTERVAL;
/* if we need to start the poll state machine, do it */
if (want_start) {
start_cycle();
start();
}
return OK;
@ -414,22 +399,22 @@ MPL3115A2::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(MPL3115A2_CONVERSION_INTERVAL)) {
if (interval < MPL3115A2_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) {
start_cycle();
start();
}
return OK;
@ -457,33 +442,24 @@ MPL3115A2::ioctl(struct file *filp, int cmd, unsigned long arg)
}
void
MPL3115A2::start_cycle(unsigned delay_ticks)
MPL3115A2::start(unsigned delay_us)
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&MPL3115A2::cycle_trampoline, this, delay_ticks);
ScheduleDelayed(delay_us);
}
void
MPL3115A2::stop_cycle()
MPL3115A2::stop()
{
work_cancel(HPWORK, &_work);
ScheduleClear();
}
void
MPL3115A2::cycle_trampoline(void *arg)
{
MPL3115A2 *dev = reinterpret_cast<MPL3115A2 *>(arg);
dev->cycle();
}
void
MPL3115A2::cycle()
MPL3115A2::Run()
{
int ret;
unsigned dummy;
@ -501,19 +477,15 @@ MPL3115A2::cycle()
* to wait 2.8 ms after issuing the sensor reset command
* according to the MPL3115A2 datasheet
*/
start_cycle(USEC2TICK(2800));
start(2800);
return;
}
if (ret == -EAGAIN) {
/* Ready read it on next cycle */
work_queue(HPWORK,
&_work,
(worker_t)&MPL3115A2::cycle_trampoline,
this,
USEC2TICK(MPL3115A2_CONVERSION_INTERVAL));
ScheduleDelayed(MPL3115A2_CONVERSION_INTERVAL);
return;
}
@ -529,7 +501,7 @@ MPL3115A2::cycle()
/* issue a reset command to the sensor */
_interface->ioctl(IOCTL_RESET, dummy);
/* reset the collection state machine and try again */
start_cycle();
start();
return;
}
@ -537,11 +509,7 @@ MPL3115A2::cycle()
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&MPL3115A2::cycle_trampoline,
this,
USEC2TICK(MPL3115A2_CONVERSION_INTERVAL));
ScheduleDelayed(MPL3115A2_CONVERSION_INTERVAL);
}
int
@ -633,7 +601,7 @@ MPL3115A2::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");
sensor_baro_s brp = {};