mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-23 06:13:34 +08:00
Reworked airspeed driver to keep most of it abstract
This commit is contained in:
Lorenz Meier
@@ -72,9 +72,7 @@
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
|
||||
/* Default I2C bus */
|
||||
#define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
|
||||
#include <drivers/airspeed/airspeed.h>
|
||||
|
||||
/* I2C bus address */
|
||||
#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */
|
||||
@@ -91,349 +89,33 @@
|
||||
/* Measurement rate is 100Hz */
|
||||
#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */
|
||||
|
||||
/* Oddly, ERROR is not defined for C++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
#ifndef CONFIG_SCHED_WORKQUEUE
|
||||
# error This requires CONFIG_SCHED_WORKQUEUE.
|
||||
#endif
|
||||
|
||||
class ETSAirspeed : public device::I2C
|
||||
class ETSAirspeed : public Airspeed
|
||||
{
|
||||
public:
|
||||
ETSAirspeed(int bus, int address = I2C_ADDRESS);
|
||||
virtual ~ETSAirspeed();
|
||||
|
||||
virtual int init();
|
||||
|
||||
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
|
||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
|
||||
/**
|
||||
* Diagnostics - print some basic information about the driver.
|
||||
*/
|
||||
void print_info();
|
||||
|
||||
protected:
|
||||
virtual int probe();
|
||||
|
||||
private:
|
||||
work_s _work;
|
||||
unsigned _num_reports;
|
||||
volatile unsigned _next_report;
|
||||
volatile unsigned _oldest_report;
|
||||
differential_pressure_s *_reports;
|
||||
bool _sensor_ok;
|
||||
int _measure_ticks;
|
||||
bool _collect_phase;
|
||||
float _diff_pres_offset;
|
||||
|
||||
orb_advert_t _airspeed_pub;
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
perf_counter_t _comms_errors;
|
||||
perf_counter_t _buffer_overflows;
|
||||
|
||||
|
||||
/**
|
||||
* Test whether the device supported by the driver is present at a
|
||||
* specific address.
|
||||
*
|
||||
* @param address The I2C bus address to probe.
|
||||
* @return True if the device is present.
|
||||
*/
|
||||
int probe_address(uint8_t address);
|
||||
|
||||
/**
|
||||
* Initialise the automatic measurement state machine and start it.
|
||||
*
|
||||
* @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();
|
||||
|
||||
/**
|
||||
* Stop the automatic measurement state machine.
|
||||
*/
|
||||
void stop();
|
||||
|
||||
/**
|
||||
* Perform a poll cycle; collect from the previous measurement
|
||||
* and start a new one.
|
||||
*/
|
||||
void cycle();
|
||||
int measure();
|
||||
int collect();
|
||||
|
||||
/**
|
||||
* 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);
|
||||
|
||||
virtual void cycle();
|
||||
virtual int measure();
|
||||
virtual int collect();
|
||||
|
||||
};
|
||||
|
||||
/* helper macro for handling report buffer indices */
|
||||
#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
|
||||
|
||||
/*
|
||||
* Driver 'main' command.
|
||||
*/
|
||||
extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);
|
||||
|
||||
ETSAirspeed::ETSAirspeed(int bus, int address) :
|
||||
I2C("ETSAirspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
|
||||
_num_reports(0),
|
||||
_next_report(0),
|
||||
_oldest_report(0),
|
||||
_reports(nullptr),
|
||||
_sensor_ok(false),
|
||||
_measure_ticks(0),
|
||||
_collect_phase(false),
|
||||
_diff_pres_offset(0.0f),
|
||||
_airspeed_pub(-1),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "ets_airspeed_read")),
|
||||
_comms_errors(perf_alloc(PC_COUNT, "ets_airspeed_comms_errors")),
|
||||
_buffer_overflows(perf_alloc(PC_COUNT, "ets_airspeed_buffer_overflows"))
|
||||
ETSAirspeed::ETSAirspeed(int bus, int address) : Airspeed(bus, address,
|
||||
CONVERSION_INTERVAL)
|
||||
{
|
||||
// enable debug() calls
|
||||
_debug_enabled = true;
|
||||
|
||||
// work_cancel in the dtor will explode if we don't do this...
|
||||
memset(&_work, 0, sizeof(_work));
|
||||
}
|
||||
|
||||
ETSAirspeed::~ETSAirspeed()
|
||||
{
|
||||
/* make sure we are truly inactive */
|
||||
stop();
|
||||
|
||||
/* free any existing reports */
|
||||
if (_reports != nullptr)
|
||||
delete[] _reports;
|
||||
}
|
||||
|
||||
int
|
||||
ETSAirspeed::init()
|
||||
{
|
||||
int ret = ERROR;
|
||||
|
||||
/* do I2C init (and probe) first */
|
||||
if (I2C::init() != OK)
|
||||
goto out;
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_num_reports = 2;
|
||||
_reports = new struct differential_pressure_s[_num_reports];
|
||||
|
||||
for (unsigned i = 0; i < _num_reports; i++)
|
||||
_reports[i].max_differential_pressure_pa = 0;
|
||||
|
||||
if (_reports == nullptr)
|
||||
goto out;
|
||||
|
||||
_oldest_report = _next_report = 0;
|
||||
|
||||
/* get a publish handle on the airspeed topic */
|
||||
memset(&_reports[0], 0, sizeof(_reports[0]));
|
||||
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]);
|
||||
|
||||
if (_airspeed_pub < 0)
|
||||
debug("failed to create airspeed sensor object. Did you start uOrb?");
|
||||
|
||||
ret = OK;
|
||||
/* sensor is ok, but we don't really know if it is within range */
|
||||
_sensor_ok = true;
|
||||
out:
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
ETSAirspeed::probe()
|
||||
{
|
||||
return measure();
|
||||
}
|
||||
|
||||
int
|
||||
ETSAirspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
switch (cmd) {
|
||||
|
||||
case SENSORIOCSPOLLRATE: {
|
||||
switch (arg) {
|
||||
|
||||
/* switching to manual polling */
|
||||
case SENSOR_POLLRATE_MANUAL:
|
||||
stop();
|
||||
_measure_ticks = 0;
|
||||
return OK;
|
||||
|
||||
/* external signalling (DRDY) not supported */
|
||||
case SENSOR_POLLRATE_EXTERNAL:
|
||||
|
||||
/* zero would be bad */
|
||||
case 0:
|
||||
return -EINVAL;
|
||||
|
||||
/* set default/max polling rate */
|
||||
case SENSOR_POLLRATE_MAX:
|
||||
case SENSOR_POLLRATE_DEFAULT: {
|
||||
/* do we need to start internal polling? */
|
||||
bool want_start = (_measure_ticks == 0);
|
||||
|
||||
/* set interval for next measurement to minimum legal value */
|
||||
_measure_ticks = USEC2TICK(CONVERSION_INTERVAL);
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
start();
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
/* adjust to a legal polling interval in Hz */
|
||||
default: {
|
||||
/* do we need to start internal polling? */
|
||||
bool want_start = (_measure_ticks == 0);
|
||||
|
||||
/* convert hz to tick interval via microseconds */
|
||||
unsigned ticks = USEC2TICK(1000000 / arg);
|
||||
|
||||
/* check against maximum rate */
|
||||
if (ticks < USEC2TICK(CONVERSION_INTERVAL))
|
||||
return -EINVAL;
|
||||
|
||||
/* update interval for next measurement */
|
||||
_measure_ticks = ticks;
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
start();
|
||||
|
||||
return OK;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
case SENSORIOCGPOLLRATE:
|
||||
if (_measure_ticks == 0)
|
||||
return SENSOR_POLLRATE_MANUAL;
|
||||
|
||||
return (1000 / _measure_ticks);
|
||||
|
||||
case SENSORIOCSQUEUEDEPTH: {
|
||||
/* add one to account for the sentinel in the ring */
|
||||
arg++;
|
||||
|
||||
/* lower bound is mandatory, upper bound is a sanity check */
|
||||
if ((arg < 2) || (arg > 100))
|
||||
return -EINVAL;
|
||||
|
||||
/* allocate new buffer */
|
||||
struct differential_pressure_s *buf = new struct differential_pressure_s[arg];
|
||||
|
||||
if (nullptr == buf)
|
||||
return -ENOMEM;
|
||||
|
||||
/* reset the measurement state machine with the new buffer, free the old */
|
||||
stop();
|
||||
delete[] _reports;
|
||||
_num_reports = arg;
|
||||
_reports = buf;
|
||||
start();
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
case SENSORIOCGQUEUEDEPTH:
|
||||
return _num_reports - 1;
|
||||
|
||||
case SENSORIOCRESET:
|
||||
/* XXX implement this */
|
||||
return -EINVAL;
|
||||
|
||||
case AIRSPEEDIOCSSCALE: {
|
||||
struct airspeed_scale *s = (struct airspeed_scale*)arg;
|
||||
_diff_pres_offset = s->offset_pa;
|
||||
return OK;
|
||||
}
|
||||
|
||||
case AIRSPEEDIOCGSCALE: {
|
||||
struct airspeed_scale *s = (struct airspeed_scale*)arg;
|
||||
s->offset_pa = _diff_pres_offset;
|
||||
s->scale = 1.0f;
|
||||
return OK;
|
||||
}
|
||||
|
||||
default:
|
||||
/* give it to the superclass */
|
||||
return I2C::ioctl(filp, cmd, arg);
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t
|
||||
ETSAirspeed::read(struct file *filp, char *buffer, size_t buflen)
|
||||
{
|
||||
unsigned count = buflen / sizeof(struct differential_pressure_s);
|
||||
int ret = 0;
|
||||
|
||||
/* buffer must be large enough */
|
||||
if (count < 1)
|
||||
return -ENOSPC;
|
||||
|
||||
/* if automatic measurement is enabled */
|
||||
if (_measure_ticks > 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;
|
||||
* we are careful to avoid racing with them.
|
||||
*/
|
||||
while (count--) {
|
||||
if (_oldest_report != _next_report) {
|
||||
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
|
||||
ret += sizeof(_reports[0]);
|
||||
INCREMENT(_oldest_report, _num_reports);
|
||||
}
|
||||
}
|
||||
|
||||
/* if there was no data, warn the caller */
|
||||
return ret ? ret : -EAGAIN;
|
||||
}
|
||||
|
||||
/* manual measurement - run one conversion */
|
||||
/* XXX really it'd be nice to lock against other readers here */
|
||||
do {
|
||||
_oldest_report = _next_report = 0;
|
||||
|
||||
/* trigger a measurement */
|
||||
if (OK != measure()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
/* wait for it to complete */
|
||||
usleep(CONVERSION_INTERVAL);
|
||||
|
||||
/* run the collection phase */
|
||||
if (OK != collect()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
/* state machine will have generated a report, copy it out */
|
||||
memcpy(buffer, _reports, sizeof(*_reports));
|
||||
ret = sizeof(*_reports);
|
||||
|
||||
} while (0);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
@@ -516,47 +198,6 @@ ETSAirspeed::collect()
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
ETSAirspeed::start()
|
||||
{
|
||||
/* reset the report ring and state machine */
|
||||
_collect_phase = false;
|
||||
_oldest_report = _next_report = 0;
|
||||
|
||||
/* schedule a cycle to start things */
|
||||
work_queue(HPWORK, &_work, (worker_t)&ETSAirspeed::cycle_trampoline, this, 1);
|
||||
|
||||
/* notify about state change */
|
||||
struct subsystem_info_s info = {
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
SUBSYSTEM_TYPE_DIFFPRESSURE
|
||||
};
|
||||
static orb_advert_t pub = -1;
|
||||
|
||||
if (pub > 0) {
|
||||
orb_publish(ORB_ID(subsystem_info), pub, &info);
|
||||
|
||||
} else {
|
||||
pub = orb_advertise(ORB_ID(subsystem_info), &info);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
ETSAirspeed::stop()
|
||||
{
|
||||
work_cancel(HPWORK, &_work);
|
||||
}
|
||||
|
||||
void
|
||||
ETSAirspeed::cycle_trampoline(void *arg)
|
||||
{
|
||||
ETSAirspeed *dev = (ETSAirspeed *)arg;
|
||||
|
||||
dev->cycle();
|
||||
}
|
||||
|
||||
void
|
||||
ETSAirspeed::cycle()
|
||||
{
|
||||
@@ -582,7 +223,7 @@ ETSAirspeed::cycle()
|
||||
/* schedule a fresh cycle call when we are ready to measure again */
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&ETSAirspeed::cycle_trampoline,
|
||||
(worker_t)&Airspeed::cycle_trampoline,
|
||||
this,
|
||||
_measure_ticks - USEC2TICK(CONVERSION_INTERVAL));
|
||||
|
||||
@@ -600,22 +241,11 @@ ETSAirspeed::cycle()
|
||||
/* schedule a fresh cycle call when the measurement is done */
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&ETSAirspeed::cycle_trampoline,
|
||||
(worker_t)&Airspeed::cycle_trampoline,
|
||||
this,
|
||||
USEC2TICK(CONVERSION_INTERVAL));
|
||||
}
|
||||
|
||||
void
|
||||
ETSAirspeed::print_info()
|
||||
{
|
||||
perf_print_counter(_sample_perf);
|
||||
perf_print_counter(_comms_errors);
|
||||
perf_print_counter(_buffer_overflows);
|
||||
printf("poll interval: %u ticks\n", _measure_ticks);
|
||||
printf("report queue: %u (%u/%u @ %p)\n",
|
||||
_num_reports, _oldest_report, _next_report, _reports);
|
||||
}
|
||||
|
||||
/**
|
||||
* Local functions in support of the shell command.
|
||||
*/
|
||||
@@ -790,11 +420,11 @@ info()
|
||||
static void
|
||||
ets_airspeed_usage()
|
||||
{
|
||||
fprintf(stderr, "usage: ets_airspeed command [options]\n");
|
||||
fprintf(stderr, "options:\n");
|
||||
fprintf(stderr, "\t-b --bus i2cbus (%d)\n", PX4_I2C_BUS_DEFAULT);
|
||||
fprintf(stderr, "command:\n");
|
||||
fprintf(stderr, "\tstart|stop|reset|test|info\n");
|
||||
warnx("usage: ets_airspeed command [options]");
|
||||
warnx("options:");
|
||||
warnx("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
|
||||
warnx("command:");
|
||||
warnx("\tstart|stop|reset|test|info");
|
||||
}
|
||||
|
||||
int
|
||||
|
||||
Reference in New Issue
Block a user