mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 11:03:33 +08:00
airspeed sensor startup improvements (#7903)
This commit is contained in:
Daniel Agar
@@ -60,7 +60,6 @@
|
||||
|
||||
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) :
|
||||
I2C("Airspeed", path, bus, address, 100000),
|
||||
_reports(nullptr),
|
||||
_sensor_ok(false),
|
||||
_last_published_sensor_ok(true), /* initialize differently to force publication */
|
||||
_measure_ticks(0),
|
||||
@@ -90,11 +89,6 @@ Airspeed::~Airspeed()
|
||||
unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance);
|
||||
}
|
||||
|
||||
/* free any existing reports */
|
||||
if (_reports != nullptr) {
|
||||
delete _reports;
|
||||
}
|
||||
|
||||
// free perf counters
|
||||
perf_free(_sample_perf);
|
||||
perf_free(_comms_errors);
|
||||
@@ -103,18 +97,9 @@ Airspeed::~Airspeed()
|
||||
int
|
||||
Airspeed::init()
|
||||
{
|
||||
int ret = PX4_ERROR;
|
||||
|
||||
/* do I2C init (and probe) first */
|
||||
if (I2C::init() != OK) {
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s));
|
||||
|
||||
if (_reports == nullptr) {
|
||||
goto out;
|
||||
if (I2C::init() != PX4_OK) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
/* register alternate interfaces if we have to */
|
||||
@@ -122,8 +107,7 @@ Airspeed::init()
|
||||
|
||||
/* advertise sensor topic, measure manually to initialize valid report */
|
||||
measure();
|
||||
differential_pressure_s arp;
|
||||
_reports->get(&arp);
|
||||
differential_pressure_s arp = {};
|
||||
|
||||
/* measurement will have generated a report, publish */
|
||||
_airspeed_pub = orb_advertise_multi(ORB_ID(differential_pressure), &arp, &_airspeed_orb_class_instance,
|
||||
@@ -133,10 +117,7 @@ Airspeed::init()
|
||||
PX4_WARN("uORB started?");
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
|
||||
out:
|
||||
return ret;
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int
|
||||
@@ -217,6 +198,7 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case SENSORIOCGPOLLRATE:
|
||||
if (_measure_ticks == 0) {
|
||||
@@ -225,27 +207,6 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
|
||||
return (1000 / _measure_ticks);
|
||||
|
||||
case SENSORIOCSQUEUEDEPTH: {
|
||||
/* lower bound is mandatory, upper bound is a sanity check */
|
||||
if ((arg < 1) || (arg > 100)) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
ATOMIC_ENTER;
|
||||
|
||||
if (!_reports->resize(arg)) {
|
||||
ATOMIC_LEAVE;
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
ATOMIC_LEAVE;
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
case SENSORIOCGQUEUEDEPTH:
|
||||
return _reports->size();
|
||||
|
||||
case SENSORIOCRESET:
|
||||
/* XXX implement this */
|
||||
return -EINVAL;
|
||||
@@ -269,72 +230,11 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t
|
||||
Airspeed::read(device::file_t *filp, char *buffer, size_t buflen)
|
||||
{
|
||||
unsigned count = buflen / sizeof(differential_pressure_s);
|
||||
differential_pressure_s *abuf = reinterpret_cast<differential_pressure_s *>(buffer);
|
||||
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 (_reports->get(abuf)) {
|
||||
ret += sizeof(*abuf);
|
||||
abuf++;
|
||||
}
|
||||
}
|
||||
|
||||
/* if there was no data, warn the caller */
|
||||
return ret ? ret : -EAGAIN;
|
||||
}
|
||||
|
||||
/* manual measurement - run one conversion */
|
||||
do {
|
||||
_reports->flush();
|
||||
|
||||
/* 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 */
|
||||
if (_reports->get(abuf)) {
|
||||
ret = sizeof(*abuf);
|
||||
}
|
||||
|
||||
} while (0);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
Airspeed::start()
|
||||
{
|
||||
/* reset the report ring and state machine */
|
||||
_collect_phase = false;
|
||||
_reports->flush();
|
||||
|
||||
/* schedule a cycle to start things */
|
||||
work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1);
|
||||
@@ -372,25 +272,7 @@ void
|
||||
Airspeed::cycle_trampoline(void *arg)
|
||||
{
|
||||
Airspeed *dev = (Airspeed *)arg;
|
||||
|
||||
dev->cycle();
|
||||
// XXX we do not know if this is
|
||||
// really helping - do not update the
|
||||
// subsys state right now
|
||||
//dev->update_status();
|
||||
}
|
||||
|
||||
void
|
||||
Airspeed::print_info()
|
||||
{
|
||||
perf_print_counter(_sample_perf);
|
||||
perf_print_counter(_comms_errors);
|
||||
warnx("poll interval: %u ticks", _measure_ticks);
|
||||
_reports->print_info("report queue");
|
||||
}
|
||||
|
||||
void
|
||||
Airspeed::new_report(const differential_pressure_s &report)
|
||||
{
|
||||
_reports->force(&report);
|
||||
dev->update_status();
|
||||
}
|
||||
|
||||
@@ -31,23 +31,13 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file airspeed.h
|
||||
* @author Simon Wilks
|
||||
*
|
||||
* Generic driver for airspeed sensors connected via I2C.
|
||||
*/
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
#include <drivers/drv_airspeed.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4_config.h>
|
||||
#include <px4_defines.h>
|
||||
#include <px4_workqueue.h>
|
||||
#include <systemlib/airspeed.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
@@ -56,10 +46,6 @@
|
||||
/* Default I2C bus */
|
||||
static constexpr uint8_t PX4_I2C_BUS_DEFAULT = PX4_I2C_BUS_EXPANSION;
|
||||
|
||||
#ifndef CONFIG_SCHED_WORKQUEUE
|
||||
# error This requires CONFIG_SCHED_WORKQUEUE.
|
||||
#endif
|
||||
|
||||
class __EXPORT Airspeed : public device::I2C
|
||||
{
|
||||
public:
|
||||
@@ -68,17 +54,9 @@ public:
|
||||
|
||||
virtual int init();
|
||||
|
||||
virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen);
|
||||
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
|
||||
|
||||
/**
|
||||
* Diagnostics - print some basic information about the driver.
|
||||
*/
|
||||
virtual void print_info();
|
||||
|
||||
private:
|
||||
ringbuffer::RingBuffer *_reports;
|
||||
|
||||
/* this class has pointer data members and should not be copied */
|
||||
Airspeed(const Airspeed &);
|
||||
Airspeed &operator=(const Airspeed &);
|
||||
@@ -118,16 +96,6 @@ protected:
|
||||
perf_counter_t _sample_perf;
|
||||
perf_counter_t _comms_errors;
|
||||
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*
|
||||
|
||||
Reference in New Issue
Block a user