Skip to content

Commit

Permalink
ms5611 move to px4 work queue
Browse files Browse the repository at this point in the history
  • Loading branch information
@dagar
dagar committed May 24, 2019
1 parent ea734f6 commit 56024cd
Show file tree
Hide file tree
Showing 2 changed files with 33 additions and 68 deletions.
2 changes: 2 additions & 0 deletions src/drivers/barometer/ms5611/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,4 +41,6 @@ px4_add_module(
ms5611_spi.cpp
ms5611_i2c.cpp
ms5611.cpp
DEPENDS
px4_work_queue
)
99 changes: 31 additions & 68 deletions src/drivers/barometer/ms5611/ms5611.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@

/**
* @file ms5611.cpp
* Driver for the MS5611 and MS6507 barometric pressure sensor connected via I2C or SPI.
* Driver for the MS5611 and MS5607 barometric pressure sensor connected via I2C or SPI.
*/

#include <px4_config.h>
Expand All @@ -51,21 +51,16 @@
#include <math.h>
#include <unistd.h>

#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>

#include <arch/board/board.h>
#include <board_config.h>

#include <drivers/device/device.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>

#include <perf/perf_counter.h>
#include <lib/perf/perf_counter.h>
#include <systemlib/err.h>
#include <platforms/px4_getopt.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>

#include "ms5611.h"

Expand All @@ -83,10 +78,6 @@ enum MS5611_BUS {
MS5611_BUS_SPI_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)

Expand Down Expand Up @@ -126,7 +117,7 @@ enum MS5611_BUS {
#define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext"
#define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int"

class MS5611 : public cdev::CDev
class MS5611 : public cdev::CDev, public px4::ScheduledWorkItem
{
public:
MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path, enum MS56XX_DEVICE_TYPES device_type);
Expand All @@ -147,8 +138,7 @@ class MS5611 : public cdev::CDev

ms5611::prom_s _prom;

struct work_s _work {};
unsigned _measure_ticks;
unsigned _measure_interval{0};

ringbuffer::RingBuffer *_reports;
enum MS56XX_DEVICE_TYPES _device_type;
Expand All @@ -173,17 +163,15 @@ class MS5611 : public cdev::CDev
/**
* Initialize the automatic measurement state machine and start it.
*
* @param delay_ticks the number of queue ticks 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();

/**
* Stop the automatic measurement state machine.
*/
void stop_cycle();
void stop();

/**
* Perform a poll cycle; collect from the previous measurement
Expand All @@ -198,15 +186,7 @@ class MS5611 : public cdev::CDev
* 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.
Expand All @@ -229,9 +209,9 @@ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path,
enum MS56XX_DEVICE_TYPES device_type) :
CDev(path),
ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
_interface(interface),
_prom(prom_buf.s),
_measure_ticks(0),
_reports(nullptr),
_device_type(device_type),
_collect_phase(false),
Expand All @@ -251,7 +231,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *
MS5611::~MS5611()
{
/* make sure we are truly inactive */
stop_cycle();
stop();

if (_class_instance != -1) {
unregister_class_devname(get_devname(), _class_instance);
Expand Down Expand Up @@ -400,7 +380,7 @@ MS5611::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.
Expand Down Expand Up @@ -474,14 +454,14 @@ MS5611::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(MS5611_CONVERSION_INTERVAL);
_measure_interval = MS5611_CONVERSION_INTERVAL;

/* if we need to start the poll state machine, do it */
if (want_start) {
start_cycle();
start();
}

return OK;
Expand All @@ -490,22 +470,22 @@ MS5611::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);
/* convert hz to interval via microseconds */
unsigned interval = (1000000 / arg);

/* check against maximum rate */
if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL)) {
if (_measure_interval < MS5611_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;
Expand All @@ -530,34 +510,25 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
}

void
MS5611::start_cycle(unsigned delay_ticks)
MS5611::start()
{

/* reset the report ring and state machine */
_collect_phase = false;
_measure_phase = 0;
_reports->flush();

/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, delay_ticks);
ScheduleNow();
}

void
MS5611::stop_cycle()
MS5611::stop()
{
work_cancel(HPWORK, &_work);
}

void
MS5611::cycle_trampoline(void *arg)
{
MS5611 *dev = reinterpret_cast<MS5611 *>(arg);

dev->cycle();
ScheduleClear();
}

void
MS5611::cycle()
MS5611::Run()
{
int ret;
unsigned dummy;
Expand Down Expand Up @@ -585,7 +556,7 @@ MS5611::cycle()
* to wait 2.8 ms after issuing the sensor reset command
* according to the MS5611 datasheet
*/
start_cycle(USEC2TICK(2800));
ScheduleDelayed(2800);
return;
}

Expand All @@ -598,14 +569,10 @@ MS5611::cycle()
* doing pressure measurements at something close to the desired rate.
*/
if ((_measure_phase != 0) &&
(_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) {
(_measure_interval > MS5611_CONVERSION_INTERVAL)) {

/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&MS5611::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL));
ScheduleDelayed(_measure_interval - MS5611_CONVERSION_INTERVAL);

return;
}
Expand All @@ -618,19 +585,15 @@ MS5611::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;
}

/* next phase is collection */
_collect_phase = true;

/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&MS5611::cycle_trampoline,
this,
USEC2TICK(MS5611_CONVERSION_INTERVAL));
ScheduleDelayed(MS5611_CONVERSION_INTERVAL);
}

int
Expand Down Expand Up @@ -788,7 +751,7 @@ MS5611::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");
printf("device: %s\n", _device_type == MS5611_DEVICE ? "ms5611" : "ms5607");

Expand Down

0 comments on commit 56024cd

Please sign in to comment.