|
|
|
|
@ -60,7 +60,6 @@
@@ -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()
@@ -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()
@@ -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()
@@ -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()
@@ -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)
@@ -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)
@@ -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)
@@ -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
@@ -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(); |
|
|
|
|
} |
|
|
|
|
|
Daniel Agar
7 years ago
committed by
GitHub