From fc82ed0c693635ccfaeee4f060b93b1a1c083acb Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 20 Mar 2013 13:21:00 +0100 Subject: Skeleton code. --- apps/drivers/ets/Makefile | 42 ++ apps/drivers/ets/ets_airspeed.cpp | 784 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 826 insertions(+) create mode 100644 apps/drivers/ets/Makefile create mode 100644 apps/drivers/ets/ets_airspeed.cpp diff --git a/apps/drivers/ets/Makefile b/apps/drivers/ets/Makefile new file mode 100644 index 000000000..9089d97af --- /dev/null +++ b/apps/drivers/ets/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the Eagle Tree Airspeed V3 driver. +# + +APPNAME = ets_airspeed +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/ets/ets_airspeed.cpp b/apps/drivers/ets/ets_airspeed.cpp new file mode 100644 index 000000000..4ac707c3c --- /dev/null +++ b/apps/drivers/ets/ets_airspeed.cpp @@ -0,0 +1,784 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file airspeed.cpp + * @author Simon Wilks + * + * Driver for the Eagle Tree Airspeed V3 connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +#include + +#include +#include + + +/* Configuration Constants */ +#define ETS_BUS PX4_I2C_BUS_EXPANSION +#define ETS_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xEA */ + +/* ETS Registers addresses */ + +#define ETS_READ_CMD 0x07 /* Read the data */ + +#define ETS_CONVERSION_INTERVAL 60000 /* 60ms */ + +/* 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 ETS : public device::I2C +{ +public: + ETS(int bus = ETS_BUS, int address = ETS_BASEADDR); + virtual ~ETS(); + + 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; + range_finder_report *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + + orb_advert_t _differential_pressure_topic; + + 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); + + +}; + +/* 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_main(int argc, char *argv[]); + +ETS::ETS(int bus, int address) : + I2C("ETS", AIRSPEED_SENSOR_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), + _differential_pressure_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "ETS_read")), + _comms_errors(perf_alloc(PC_COUNT, "ETS_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ETS_buffer_overflows")) +{ + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +ETS::~ETS() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +int +ETS::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 range_finder_report[_num_reports]; + + 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])); + _differential_pressure_topic = orb_advertise(ORB_ID(_differential_pressure), &_reports[0]); + + if (_differential_pressure_topic < 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 +ETS::probe() +{ + return measure(); +} + +int +ETS::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(ETS_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(ETS_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 range_finder_report *buf = new struct range_finder_report[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; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +ETS::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + 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(ETS_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 +ETS::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + uint8_t cmd = ETS_READ_CMD; + ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) + { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + return ret; + } + ret = OK; + + return ret; +} + +int +ETS::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + + perf_begin(_sample_perf); + + ret = transfer(nullptr, 0, &val[0], 2); + + if (ret < 0) + { + log("error reading from sensor: %d", ret); + return ret; + } + + uint16_t distance = val[0] << 8 | val[1]; + float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + _reports[_next_report].timestamp = hrt_absolute_time(); + _reports[_next_report].distance = si_units; + _reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + + /* publish it */ + orb_publish(ORB_ID(_differential_pressure), _differential_pressure_topic, &_reports[_next_report]); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, toss it */ + if (_next_report == _oldest_report) { + perf_count(_buffer_overflows); + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + +out: + perf_end(_sample_perf); + return ret; + + return ret; +} + +void +ETS::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)&ETS::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 +ETS::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +ETS::cycle_trampoline(void *arg) +{ + ETS *dev = (ETS *)arg; + + dev->cycle(); +} + +void +ETS::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(ETS_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&ETS::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(ETS_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) + log("measure error"); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&ETS::cycle_trampoline, + this, + USEC2TICK(ETS_CONVERSION_INTERVAL)); +} + +void +ETS::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. + */ +namespace ETS +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +ETS *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new ETS(ETS_BUS); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(AIRSPEED_SENSOR_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); + +fail: + + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + else + { + errx(1, "driver not running"); + } + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + int ret; + + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'ETS start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "immediate read failed"); + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + errx(1, "failed to set 2Hz poll rate"); + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "periodic read failed"); + + warnx("periodic read %u", i); + warnx("measurement: %0.3f", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +ETS_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + ETS::start(); + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + ETS::stop(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + ETS::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + ETS::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + ETS::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} -- cgit v1.2.3 From 4f99200b0ff102c01f415a57e9f173a863483d2a Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 11 Apr 2013 08:36:54 +0200 Subject: Initial implementation that can read values from the ETS. --- apps/drivers/ets/Makefile | 42 -- apps/drivers/ets/ets_airspeed.cpp | 784 ---------------------------- apps/drivers/ets_airspeed/Makefile | 42 ++ apps/drivers/ets_airspeed/ets_airspeed.cpp | 785 +++++++++++++++++++++++++++++ apps/px4io/controls.c | 2 +- apps/uORB/objects_common.cpp | 3 + nuttx/configs/px4fmu/nsh/appconfig | 1 + 7 files changed, 832 insertions(+), 827 deletions(-) delete mode 100644 apps/drivers/ets/Makefile delete mode 100644 apps/drivers/ets/ets_airspeed.cpp create mode 100644 apps/drivers/ets_airspeed/Makefile create mode 100644 apps/drivers/ets_airspeed/ets_airspeed.cpp diff --git a/apps/drivers/ets/Makefile b/apps/drivers/ets/Makefile deleted file mode 100644 index 9089d97af..000000000 --- a/apps/drivers/ets/Makefile +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Makefile to build the Eagle Tree Airspeed V3 driver. -# - -APPNAME = ets_airspeed -PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 - -include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/ets/ets_airspeed.cpp b/apps/drivers/ets/ets_airspeed.cpp deleted file mode 100644 index 4ac707c3c..000000000 --- a/apps/drivers/ets/ets_airspeed.cpp +++ /dev/null @@ -1,784 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file airspeed.cpp - * @author Simon Wilks - * - * Driver for the Eagle Tree Airspeed V3 connected via I2C. - */ - -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include -#include - -#include - -#include -#include - - -/* Configuration Constants */ -#define ETS_BUS PX4_I2C_BUS_EXPANSION -#define ETS_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xEA */ - -/* ETS Registers addresses */ - -#define ETS_READ_CMD 0x07 /* Read the data */ - -#define ETS_CONVERSION_INTERVAL 60000 /* 60ms */ - -/* 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 ETS : public device::I2C -{ -public: - ETS(int bus = ETS_BUS, int address = ETS_BASEADDR); - virtual ~ETS(); - - 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; - range_finder_report *_reports; - bool _sensor_ok; - int _measure_ticks; - bool _collect_phase; - - orb_advert_t _differential_pressure_topic; - - 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); - - -}; - -/* 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_main(int argc, char *argv[]); - -ETS::ETS(int bus, int address) : - I2C("ETS", AIRSPEED_SENSOR_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), - _differential_pressure_topic(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "ETS_read")), - _comms_errors(perf_alloc(PC_COUNT, "ETS_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "ETS_buffer_overflows")) -{ - // enable debug() calls - _debug_enabled = true; - - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); -} - -ETS::~ETS() -{ - /* make sure we are truly inactive */ - stop(); - - /* free any existing reports */ - if (_reports != nullptr) - delete[] _reports; -} - -int -ETS::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 range_finder_report[_num_reports]; - - 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])); - _differential_pressure_topic = orb_advertise(ORB_ID(_differential_pressure), &_reports[0]); - - if (_differential_pressure_topic < 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 -ETS::probe() -{ - return measure(); -} - -int -ETS::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(ETS_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(ETS_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 range_finder_report *buf = new struct range_finder_report[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; - - default: - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); - } -} - -ssize_t -ETS::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct range_finder_report); - 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(ETS_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 -ETS::measure() -{ - int ret; - - /* - * Send the command to begin a measurement. - */ - uint8_t cmd = ETS_READ_CMD; - ret = transfer(&cmd, 1, nullptr, 0); - - if (OK != ret) - { - perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); - return ret; - } - ret = OK; - - return ret; -} - -int -ETS::collect() -{ - int ret = -EIO; - - /* read from the sensor */ - uint8_t val[2] = {0, 0}; - - perf_begin(_sample_perf); - - ret = transfer(nullptr, 0, &val[0], 2); - - if (ret < 0) - { - log("error reading from sensor: %d", ret); - return ret; - } - - uint16_t distance = val[0] << 8 | val[1]; - float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].distance = si_units; - _reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; - - /* publish it */ - orb_publish(ORB_ID(_differential_pressure), _differential_pressure_topic, &_reports[_next_report]); - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { - perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); - } - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - ret = OK; - -out: - perf_end(_sample_perf); - return ret; - - return ret; -} - -void -ETS::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)&ETS::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 -ETS::stop() -{ - work_cancel(HPWORK, &_work); -} - -void -ETS::cycle_trampoline(void *arg) -{ - ETS *dev = (ETS *)arg; - - dev->cycle(); -} - -void -ETS::cycle() -{ - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - if (OK != collect()) { - log("collection error"); - /* restart the measurement state machine */ - start(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - */ - if (_measure_ticks > USEC2TICK(ETS_CONVERSION_INTERVAL)) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&ETS::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(ETS_CONVERSION_INTERVAL)); - - return; - } - } - - /* measurement phase */ - if (OK != measure()) - log("measure error"); - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&ETS::cycle_trampoline, - this, - USEC2TICK(ETS_CONVERSION_INTERVAL)); -} - -void -ETS::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. - */ -namespace ETS -{ - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -const int ERROR = -1; - -ETS *g_dev; - -void start(); -void stop(); -void test(); -void reset(); -void info(); - -/** - * Start the driver. - */ -void -start() -{ - int fd; - - if (g_dev != nullptr) - errx(1, "already started"); - - /* create the driver */ - g_dev = new ETS(ETS_BUS); - - if (g_dev == nullptr) - goto fail; - - if (OK != g_dev->init()) - goto fail; - - /* set the poll rate to default, starts automatic data collection */ - fd = open(AIRSPEED_SENSOR_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - goto fail; - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; - - exit(0); - -fail: - - if (g_dev != nullptr) - { - delete g_dev; - g_dev = nullptr; - } - - errx(1, "driver start failed"); -} - -/** - * Stop the driver - */ -void stop() -{ - if (g_dev != nullptr) - { - delete g_dev; - g_dev = nullptr; - } - else - { - errx(1, "driver not running"); - } - exit(0); -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test() -{ - struct range_finder_report report; - ssize_t sz; - int ret; - - int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "%s open failed (try 'ETS start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); - - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) - err(1, "immediate read failed"); - - warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); - warnx("time: %lld", report.timestamp); - - /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) - errx(1, "failed to set 2Hz poll rate"); - - /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = poll(&fds, 1, 2000); - - if (ret != 1) - errx(1, "timed out waiting for sensor data"); - - /* now go get it */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) - err(1, "periodic read failed"); - - warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); - warnx("time: %lld", report.timestamp); - } - - errx(0, "PASS"); -} - -/** - * Reset the driver. - */ -void -reset() -{ - int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); - - if (fd < 0) - err(1, "failed "); - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) - err(1, "driver reset failed"); - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); - - exit(0); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - if (g_dev == nullptr) - errx(1, "driver not running"); - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} - -} // namespace - -int -ETS_main(int argc, char *argv[]) -{ - /* - * Start/load the driver. - */ - if (!strcmp(argv[1], "start")) - ETS::start(); - - /* - * Stop the driver - */ - if (!strcmp(argv[1], "stop")) - ETS::stop(); - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - ETS::test(); - - /* - * Reset the driver. - */ - if (!strcmp(argv[1], "reset")) - ETS::reset(); - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) - ETS::info(); - - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); -} diff --git a/apps/drivers/ets_airspeed/Makefile b/apps/drivers/ets_airspeed/Makefile new file mode 100644 index 000000000..9089d97af --- /dev/null +++ b/apps/drivers/ets_airspeed/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the Eagle Tree Airspeed V3 driver. +# + +APPNAME = ets_airspeed +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/ets_airspeed/ets_airspeed.cpp b/apps/drivers/ets_airspeed/ets_airspeed.cpp new file mode 100644 index 000000000..790e949e0 --- /dev/null +++ b/apps/drivers/ets_airspeed/ets_airspeed.cpp @@ -0,0 +1,785 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file airspeed.cpp + * @author Simon Wilks + * + * Driver for the Eagle Tree Airspeed V3 connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +#include +#include + +#include +#include + + +/* Configuration Constants */ +#define ETS_AIRSPEED_BUS PX4_I2C_BUS_ESC +#define ETS_AIRSPEED_ADDRESS 0x75 + +/* ETS_AIRSPEED Registers addresses */ + +#define ETS_AIRSPEED_READ_CMD 0x07 /* Read the data */ + +/* Max measurement rate is 100Hz */ +#define ETS_AIRSPEED_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 ETS_AIRSPEED : public device::I2C +{ +public: + ETS_AIRSPEED(int bus = ETS_AIRSPEED_BUS, int address = ETS_AIRSPEED_ADDRESS); + virtual ~ETS_AIRSPEED(); + + 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; + airspeed_report *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + + orb_advert_t _differential_pressure_topic; + + 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); + + +}; + +/* 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[]); + +ETS_AIRSPEED::ETS_AIRSPEED(int bus, int address) : + I2C("ETS_AIRSPEED", 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), + _differential_pressure_topic(-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")) +{ + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +ETS_AIRSPEED::~ETS_AIRSPEED() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +int +ETS_AIRSPEED::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 airspeed_report[_num_reports]; + + 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])); + _differential_pressure_topic = orb_advertise(ORB_ID(sensor_differential_pressure), &_reports[0]); + + if (_differential_pressure_topic < 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 +ETS_AIRSPEED::probe() +{ + return measure(); +} + +int +ETS_AIRSPEED::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(ETS_AIRSPEED_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(ETS_AIRSPEED_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 airspeed_report *buf = new struct airspeed_report[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; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +ETS_AIRSPEED::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct airspeed_report); + 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(ETS_AIRSPEED_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 +ETS_AIRSPEED::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + uint8_t cmd = ETS_AIRSPEED_READ_CMD; + ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) + { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + return ret; + } + ret = OK; + + return ret; +} + +int +ETS_AIRSPEED::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + + perf_begin(_sample_perf); + + ret = transfer(nullptr, 0, &val[0], 2); + + if (ret < 0) + { + log("error reading from sensor: %d", ret); + return ret; + } + + uint16_t distance = val[0] << 8 | val[1]; + float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + _reports[_next_report].timestamp = hrt_absolute_time(); + _reports[_next_report].speed = si_units; + + /* publish it */ + orb_publish(ORB_ID(sensor_differential_pressure), _differential_pressure_topic, &_reports[_next_report]); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, toss it */ + if (_next_report == _oldest_report) { + perf_count(_buffer_overflows); + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + +out: + perf_end(_sample_perf); + return ret; + + return ret; +} + +void +ETS_AIRSPEED::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)&ETS_AIRSPEED::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 +ETS_AIRSPEED::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +ETS_AIRSPEED::cycle_trampoline(void *arg) +{ + ETS_AIRSPEED *dev = (ETS_AIRSPEED *)arg; + + dev->cycle(); +} + +void +ETS_AIRSPEED::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(ETS_AIRSPEED_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&ETS_AIRSPEED::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(ETS_AIRSPEED_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) + log("measure error"); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&ETS_AIRSPEED::cycle_trampoline, + this, + USEC2TICK(ETS_AIRSPEED_CONVERSION_INTERVAL)); +} + +void +ETS_AIRSPEED::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. + */ +namespace ets_airspeed +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +ETS_AIRSPEED *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new ETS_AIRSPEED(ETS_AIRSPEED_BUS); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); + +fail: + + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + else + { + errx(1, "driver not running"); + } + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct airspeed_report report; + ssize_t sz; + int ret; + + int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'ets_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "immediate read failed"); + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.speed); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + errx(1, "failed to set 2Hz poll rate"); + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "periodic read failed"); + + warnx("periodic read %u", i); + warnx("measurement: %0.3f", (double)report.speed); + warnx("time: %lld", report.timestamp); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +ets_airspeed_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + ets_airspeed::start(); + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + ets_airspeed::stop(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + ets_airspeed::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + ets_airspeed::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + ets_airspeed::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index e80a41f15..9a0f0e5c1 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -70,7 +70,7 @@ controls_init(void) unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; - r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 950; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 30; diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 136375140..cd21d556c 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -56,6 +56,9 @@ ORB_DEFINE(sensor_baro, struct baro_report); #include ORB_DEFINE(sensor_range_finder, struct range_finder_report); +#include +ORB_DEFINE(sensor_differential_pressure, struct airspeed_report); + #include ORB_DEFINE(output_pwm, struct pwm_output_values); diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 80cf6f312..b5f35f514 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -127,6 +127,7 @@ CONFIGURED_APPS += drivers/px4fmu CONFIGURED_APPS += drivers/hil CONFIGURED_APPS += drivers/gps CONFIGURED_APPS += drivers/mb12xx +CONFIGURED_APPS += drivers/ets_airspeed # Testing stuff CONFIGURED_APPS += px4/sensors_bringup -- cgit v1.2.3 From c8ac1d0b0a97f5fc926a48e64e0e116387453dcd Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 11 Apr 2013 08:37:25 +0200 Subject: Add in the missing header. --- apps/drivers/drv_airspeed.h | 74 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 74 insertions(+) create mode 100644 apps/drivers/drv_airspeed.h diff --git a/apps/drivers/drv_airspeed.h b/apps/drivers/drv_airspeed.h new file mode 100644 index 000000000..a98f56807 --- /dev/null +++ b/apps/drivers/drv_airspeed.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Airspeed driver interface. + */ + +#ifndef _DRV_AIRSPEED_H +#define _DRV_AIRSPEED_H + +#include +#include + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +#define AIRSPEED_DEVICE_PATH "/dev/airspeed" + +/** + * Airspeed report structure. Reads from the device must be in multiples of this + * structure. + */ +struct airspeed_report { + uint64_t timestamp; + uint8_t speed; /** in meters/sec */ +}; + +/* + * ObjDev tag for raw range finder data. + */ +ORB_DECLARE(sensor_differential_pressure); + +/* + * ioctl() definitions + * + * Airspeed drivers also implement the generic sensor driver + * interfaces from drv_sensor.h + */ + +#define _AIRSPEEDIOCBASE (0x7700) +#define __AIRSPEEDIOC(_n) (_IOC(_AIRSPEEDIOCBASE, _n)) + + +#endif /* _DRV_AIRSPEED_H */ -- cgit v1.2.3 From 42f4a9e8800c4d5d2823b4f477c556547fe0d3d0 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 13 Apr 2013 08:47:12 +0200 Subject: Switch to differential pressure topic --- apps/drivers/drv_airspeed.h | 14 +-- apps/drivers/ets_airspeed/ets_airspeed.cpp | 150 ++++++++++++++++++++--------- apps/systemlib/airspeed.c | 2 +- apps/uORB/objects_common.cpp | 3 - 4 files changed, 110 insertions(+), 59 deletions(-) diff --git a/apps/drivers/drv_airspeed.h b/apps/drivers/drv_airspeed.h index a98f56807..269ee4559 100644 --- a/apps/drivers/drv_airspeed.h +++ b/apps/drivers/drv_airspeed.h @@ -47,18 +47,18 @@ #define AIRSPEED_DEVICE_PATH "/dev/airspeed" /** - * Airspeed report structure. Reads from the device must be in multiples of this + * Airspeed report structure. Reads from the device must be in multiples of this * structure. */ -struct airspeed_report { - uint64_t timestamp; - uint8_t speed; /** in meters/sec */ -}; +//struct airspeed_report { +// uint64_t timestamp; +// uint8_t diff_pressure; /** differential pressure in Pa */ +//}; /* * ObjDev tag for raw range finder data. */ -ORB_DECLARE(sensor_differential_pressure); +//ORB_DECLARE(sensor_differential_pressure); /* * ioctl() definitions @@ -67,7 +67,7 @@ ORB_DECLARE(sensor_differential_pressure); * interfaces from drv_sensor.h */ -#define _AIRSPEEDIOCBASE (0x7700) +#define _AIRSPEEDIOCBASE (0x7700) #define __AIRSPEEDIOC(_n) (_IOC(_AIRSPEEDIOCBASE, _n)) diff --git a/apps/drivers/ets_airspeed/ets_airspeed.cpp b/apps/drivers/ets_airspeed/ets_airspeed.cpp index 790e949e0..0b535b0b5 100644 --- a/apps/drivers/ets_airspeed/ets_airspeed.cpp +++ b/apps/drivers/ets_airspeed/ets_airspeed.cpp @@ -63,24 +63,28 @@ #include #include +#include -#include #include +#include #include +#include +#include /* for baro readings */ #include - /* Configuration Constants */ -#define ETS_AIRSPEED_BUS PX4_I2C_BUS_ESC -#define ETS_AIRSPEED_ADDRESS 0x75 +#define I2C_BUS PX4_I2C_BUS_ESC +#define I2C_ADDRESS 0x75 /* ETS_AIRSPEED Registers addresses */ - -#define ETS_AIRSPEED_READ_CMD 0x07 /* Read the data */ +#define READ_CMD 0x07 /* Read the data */ /* Max measurement rate is 100Hz */ -#define ETS_AIRSPEED_CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ +#define CONVERSION_INTERVAL (1000000 / 10) /* microseconds */ + +#define DIFF_PRESSURE_SCALE 1.0 +#define DIFF_PRESSURE_OFFSET 1673 /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -92,10 +96,13 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif +static int _sensor_sub = -1; + + class ETS_AIRSPEED : public device::I2C { public: - ETS_AIRSPEED(int bus = ETS_AIRSPEED_BUS, int address = ETS_AIRSPEED_ADDRESS); + ETS_AIRSPEED(int bus = I2C_BUS, int address = I2C_ADDRESS); virtual ~ETS_AIRSPEED(); virtual int init(); @@ -112,20 +119,21 @@ protected: virtual int probe(); private: - work_s _work; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - airspeed_report *_reports; - bool _sensor_ok; - int _measure_ticks; - bool _collect_phase; + 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; - orb_advert_t _differential_pressure_topic; + orb_advert_t _airspeed_pub; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; - 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 @@ -184,7 +192,7 @@ ETS_AIRSPEED::ETS_AIRSPEED(int bus, int address) : _sensor_ok(false), _measure_ticks(0), _collect_phase(false), - _differential_pressure_topic(-1), + _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")) @@ -217,7 +225,7 @@ ETS_AIRSPEED::init() /* allocate basic report buffers */ _num_reports = 2; - _reports = new struct airspeed_report[_num_reports]; + _reports = new struct differential_pressure_s[_num_reports]; if (_reports == nullptr) goto out; @@ -226,11 +234,18 @@ ETS_AIRSPEED::init() /* get a publish handle on the airspeed topic */ memset(&_reports[0], 0, sizeof(_reports[0])); - _differential_pressure_topic = orb_advertise(ORB_ID(sensor_differential_pressure), &_reports[0]); + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]); - if (_differential_pressure_topic < 0) + if (_airspeed_pub < 0) debug("failed to create airspeed sensor object. Did you start uOrb?"); + _sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + + if (_sensor_sub < 0) { + debug("failed to subscribe to sensor_combined object."); + return ret; + } + ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; @@ -272,7 +287,7 @@ ETS_AIRSPEED::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(ETS_AIRSPEED_CONVERSION_INTERVAL); + _measure_ticks = USEC2TICK(CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (want_start) @@ -290,7 +305,7 @@ ETS_AIRSPEED::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(ETS_AIRSPEED_CONVERSION_INTERVAL)) + if (ticks < USEC2TICK(CONVERSION_INTERVAL)) return -EINVAL; /* update interval for next measurement */ @@ -320,7 +335,7 @@ ETS_AIRSPEED::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; /* allocate new buffer */ - struct airspeed_report *buf = new struct airspeed_report[arg]; + struct differential_pressure_s *buf = new struct differential_pressure_s[arg]; if (nullptr == buf) return -ENOMEM; @@ -351,7 +366,7 @@ ETS_AIRSPEED::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t ETS_AIRSPEED::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct airspeed_report); + unsigned count = buflen / sizeof(struct differential_pressure_s); int ret = 0; /* buffer must be large enough */ @@ -390,7 +405,7 @@ ETS_AIRSPEED::read(struct file *filp, char *buffer, size_t buflen) } /* wait for it to complete */ - usleep(ETS_AIRSPEED_CONVERSION_INTERVAL); + usleep(CONVERSION_INTERVAL); /* run the collection phase */ if (OK != collect()) { @@ -415,7 +430,7 @@ ETS_AIRSPEED::measure() /* * Send the command to begin a measurement. */ - uint8_t cmd = ETS_AIRSPEED_READ_CMD; + uint8_t cmd = READ_CMD; ret = transfer(&cmd, 1, nullptr, 0); if (OK != ret) @@ -441,20 +456,48 @@ ETS_AIRSPEED::collect() ret = transfer(nullptr, 0, &val[0], 2); - if (ret < 0) - { + if (ret < 0) { log("error reading from sensor: %d", ret); return ret; } - uint16_t distance = val[0] << 8 | val[1]; - float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].speed = si_units; + uint16_t diff_pres_pa = val[1] << 8 | val[0]; + //log("val: %0.3f", (float)(diff_pressure)); + + /* adjust if necessary */ + diff_pres_pa = DIFF_PRESSURE_SCALE * (diff_pres_pa - DIFF_PRESSURE_OFFSET); + //log("measurement: %0.2f m/s", calc_indicated_airspeed((float)_reports[_next_report].diff_pressure)); + + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + + bool updated; + orb_check(_sensor_sub, &updated); + if (updated) { + orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw); + printf("baro temp %3.6f\n", raw.baro_pres_mbar); + } + unlock(); + //if (raw.baro_temp_celcius > 0) + // log("baro temp %3.3f\n", (uint8_t) raw.baro_temp_celcius); + + float airspeed_true = calc_true_airspeed(diff_pres_pa + raw.baro_pres_mbar*1e2f, + raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - 5.0f); //factor 1e2 for conversion from mBar to Pa + // XXX HACK - true temperature is much less than indicated temperature in baro, + // subtract 5 degrees in an attempt to account for the electrical upheating of the PCB + + float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa); - /* publish it */ - orb_publish(ORB_ID(sensor_differential_pressure), _differential_pressure_topic, &_reports[_next_report]); + _reports[_next_report].timestamp = hrt_absolute_time(); + _reports[_next_report].static_pressure_mbar = raw.baro_pres_mbar; + _reports[_next_report].differential_pressure_mbar = diff_pres_pa * 1e-2f; + _reports[_next_report].temperature_celcius = raw.baro_temp_celcius; + _reports[_next_report].indicated_airspeed_m_s = airspeed_indicated; + _reports[_next_report].true_airspeed_m_s = airspeed_true; + _reports[_next_report].voltage = 0; + + /* announce the airspeed if needed, just publish else */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); /* post a report to the ring - note, not locked */ INCREMENT(_next_report, _num_reports); @@ -472,7 +515,6 @@ ETS_AIRSPEED::collect() out: perf_end(_sample_perf); - return ret; return ret; } @@ -536,14 +578,14 @@ ETS_AIRSPEED::cycle() /* * Is there a collect->measure gap? */ - if (_measure_ticks > USEC2TICK(ETS_AIRSPEED_CONVERSION_INTERVAL)) { + if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) { /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, &_work, (worker_t)&ETS_AIRSPEED::cycle_trampoline, this, - _measure_ticks - USEC2TICK(ETS_AIRSPEED_CONVERSION_INTERVAL)); + _measure_ticks - USEC2TICK(CONVERSION_INTERVAL)); return; } @@ -561,7 +603,7 @@ ETS_AIRSPEED::cycle() &_work, (worker_t)&ETS_AIRSPEED::cycle_trampoline, this, - USEC2TICK(ETS_AIRSPEED_CONVERSION_INTERVAL)); + USEC2TICK(CONVERSION_INTERVAL)); } void @@ -607,7 +649,7 @@ start() errx(1, "already started"); /* create the driver */ - g_dev = new ETS_AIRSPEED(ETS_AIRSPEED_BUS); + g_dev = new ETS_AIRSPEED(I2C_BUS); if (g_dev == nullptr) goto fail; @@ -662,7 +704,7 @@ void stop() void test() { - struct airspeed_report report; + struct differential_pressure_s report; ssize_t sz; int ret; @@ -678,7 +720,18 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("measurement: %0.2f m", (double)report.speed); + warnx("diff pressure: %0.3f mbar", report.differential_pressure_mbar); + warnx("indicated airspeed: %0.1f m/s", report.indicated_airspeed_m_s); + warnx("true airspeed: %0.1f m/s", report.true_airspeed_m_s); + + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + + //if (raw.baro_temp_celcius > 0) + // log("baro temp %3.3f\n", (uint8_t) raw.baro_temp_celcius); + warnx("temp: %3.5f", raw.baro_temp_celcius); warnx("time: %lld", report.timestamp); /* start the sensor polling at 2Hz */ @@ -704,8 +757,9 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.speed); - warnx("time: %lld", report.timestamp); + warnx("diff pressure: %0.3f mbar", report.differential_pressure_mbar); + warnx("indicated airspeed: %0.1f m/s", report.indicated_airspeed_m_s); + warnx("true airspeed: %0.1f m/s", report.true_airspeed_m_s); warnx("time: %lld", report.timestamp); } errx(0, "PASS"); diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c index 264287b10..15bb833a9 100644 --- a/apps/systemlib/airspeed.c +++ b/apps/systemlib/airspeed.c @@ -97,7 +97,7 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp float density = get_air_density(static_pressure, temperature_celsius); if (density < 0.0001f || !isfinite(density)) { density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; - printf("[airspeed] Invalid air density, using density at sea level\n"); +// printf("[airspeed] Invalid air density, using density at sea level\n"); } float pressure_difference = total_pressure - static_pressure; diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index cd21d556c..136375140 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -56,9 +56,6 @@ ORB_DEFINE(sensor_baro, struct baro_report); #include ORB_DEFINE(sensor_range_finder, struct range_finder_report); -#include -ORB_DEFINE(sensor_differential_pressure, struct airspeed_report); - #include ORB_DEFINE(output_pwm, struct pwm_output_values); -- cgit v1.2.3 From c5a453cd18949d2d4673c0b343e22c22a8d2854d Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 13 Apr 2013 09:09:21 +0200 Subject: Remove some testing code. --- apps/drivers/ets_airspeed/ets_airspeed.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/apps/drivers/ets_airspeed/ets_airspeed.cpp b/apps/drivers/ets_airspeed/ets_airspeed.cpp index 0b535b0b5..58d016a30 100644 --- a/apps/drivers/ets_airspeed/ets_airspeed.cpp +++ b/apps/drivers/ets_airspeed/ets_airspeed.cpp @@ -477,7 +477,6 @@ ETS_AIRSPEED::collect() orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw); printf("baro temp %3.6f\n", raw.baro_pres_mbar); } - unlock(); //if (raw.baro_temp_celcius > 0) // log("baro temp %3.3f\n", (uint8_t) raw.baro_temp_celcius); -- cgit v1.2.3 From df6976c8d640b395220d46f5b1fd7ecfc8ae3e04 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 19 Apr 2013 16:20:40 +0200 Subject: Split diff pressure and airspeed. --- apps/commander/commander.c | 10 ++--- apps/drivers/ets_airspeed/ets_airspeed.cpp | 71 +++++------------------------- apps/sdlog/sdlog.c | 17 +++++-- apps/sensors/sensors.cpp | 59 +++++++++++++++++-------- apps/uORB/objects_common.cpp | 3 ++ apps/uORB/topics/differential_pressure.h | 11 ++--- apps/uORB/topics/sensor_combined.h | 2 + 7 files changed, 80 insertions(+), 93 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 7c0a25f86..fcfffcfef 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -801,7 +801,7 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) struct differential_pressure_s differential_pressure; int calibration_counter = 0; - float airspeed_offset = 0.0f; + float diff_pres_offset = 0.0f; while (calibration_counter < calibration_count) { @@ -812,7 +812,7 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure); - airspeed_offset += differential_pressure.voltage; + diff_pres_offset += differential_pressure.differential_pressure_pa; calibration_counter++; } else if (poll_ret == 0) { @@ -822,11 +822,11 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) } } - airspeed_offset = airspeed_offset / calibration_count; + diff_pres_offset = diff_pres_offset / calibration_count; - if (isfinite(airspeed_offset)) { + if (isfinite(diff_pres_offset)) { - if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) { + if (param_set(param_find("SENS_VAIR_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, "Setting offs failed!"); } diff --git a/apps/drivers/ets_airspeed/ets_airspeed.cpp b/apps/drivers/ets_airspeed/ets_airspeed.cpp index 58d016a30..860baa760 100644 --- a/apps/drivers/ets_airspeed/ets_airspeed.cpp +++ b/apps/drivers/ets_airspeed/ets_airspeed.cpp @@ -61,16 +61,16 @@ #include -#include -#include #include +#include +#include +#include #include #include #include #include -#include /* for baro readings */ #include /* Configuration Constants */ @@ -83,9 +83,6 @@ /* Max measurement rate is 100Hz */ #define CONVERSION_INTERVAL (1000000 / 10) /* microseconds */ -#define DIFF_PRESSURE_SCALE 1.0 -#define DIFF_PRESSURE_OFFSET 1673 - /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -96,9 +93,6 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -static int _sensor_sub = -1; - - class ETS_AIRSPEED : public device::I2C { public: @@ -127,6 +121,7 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; + int _differential_pressure_offset; orb_advert_t _airspeed_pub; @@ -195,7 +190,8 @@ ETS_AIRSPEED::ETS_AIRSPEED(int bus, int address) : _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")) + _buffer_overflows(perf_alloc(PC_COUNT, "ETS_AIRSPEED_buffer_overflows")), + _differential_pressure_offset(0) { // enable debug() calls _debug_enabled = true; @@ -239,12 +235,7 @@ ETS_AIRSPEED::init() if (_airspeed_pub < 0) debug("failed to create airspeed sensor object. Did you start uOrb?"); - _sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - - if (_sensor_sub < 0) { - debug("failed to subscribe to sensor_combined object."); - return ret; - } + param_get(param_find("SENS_VAIR_OFF"), &_differential_pressure_offset); ret = OK; /* sensor is ok, but we don't really know if it is within range */ @@ -462,38 +453,13 @@ ETS_AIRSPEED::collect() } uint16_t diff_pres_pa = val[1] << 8 | val[0]; - //log("val: %0.3f", (float)(diff_pressure)); /* adjust if necessary */ - diff_pres_pa = DIFF_PRESSURE_SCALE * (diff_pres_pa - DIFF_PRESSURE_OFFSET); + diff_pres_pa -= _differential_pressure_offset; //log("measurement: %0.2f m/s", calc_indicated_airspeed((float)_reports[_next_report].diff_pressure)); - - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - - bool updated; - orb_check(_sensor_sub, &updated); - if (updated) { - orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw); - printf("baro temp %3.6f\n", raw.baro_pres_mbar); - } - //if (raw.baro_temp_celcius > 0) - // log("baro temp %3.3f\n", (uint8_t) raw.baro_temp_celcius); - - float airspeed_true = calc_true_airspeed(diff_pres_pa + raw.baro_pres_mbar*1e2f, - raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - 5.0f); //factor 1e2 for conversion from mBar to Pa - // XXX HACK - true temperature is much less than indicated temperature in baro, - // subtract 5 degrees in an attempt to account for the electrical upheating of the PCB - - float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa); _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].static_pressure_mbar = raw.baro_pres_mbar; - _reports[_next_report].differential_pressure_mbar = diff_pres_pa * 1e-2f; - _reports[_next_report].temperature_celcius = raw.baro_temp_celcius; - _reports[_next_report].indicated_airspeed_m_s = airspeed_indicated; - _reports[_next_report].true_airspeed_m_s = airspeed_true; - _reports[_next_report].voltage = 0; + _reports[_next_report].differential_pressure_pa = diff_pres_pa; /* announce the airspeed if needed, just publish else */ orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); @@ -512,7 +478,6 @@ ETS_AIRSPEED::collect() ret = OK; -out: perf_end(_sample_perf); return ret; @@ -719,19 +684,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %0.3f mbar", report.differential_pressure_mbar); - warnx("indicated airspeed: %0.1f m/s", report.indicated_airspeed_m_s); - warnx("true airspeed: %0.1f m/s", report.true_airspeed_m_s); - - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - - //if (raw.baro_temp_celcius > 0) - // log("baro temp %3.3f\n", (uint8_t) raw.baro_temp_celcius); - warnx("temp: %3.5f", raw.baro_temp_celcius); - warnx("time: %lld", report.timestamp); + warnx("diff pressure: %0.3f pa", (double) report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) @@ -756,9 +709,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("diff pressure: %0.3f mbar", report.differential_pressure_mbar); - warnx("indicated airspeed: %0.1f m/s", report.indicated_airspeed_m_s); - warnx("true airspeed: %0.1f m/s", report.true_airspeed_m_s); warnx("time: %lld", report.timestamp); + warnx("diff pressure: %0.3f pa", (double) report.differential_pressure_pa); } errx(0, "PASS"); diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index df8745d9f..46b232c34 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -71,6 +71,7 @@ #include #include #include +#include #include @@ -444,6 +445,7 @@ int sdlog_thread_main(int argc, char *argv[]) struct optical_flow_s flow; struct battery_status_s batt; struct differential_pressure_s diff_pressure; + struct airspeed_s airspeed; } buf; memset(&buf, 0, sizeof(buf)); @@ -462,6 +464,7 @@ int sdlog_thread_main(int argc, char *argv[]) int flow_sub; int batt_sub; int diff_pressure_sub; + int airspeed_sub; } subs; /* --- MANAGEMENT - LOGGING COMMAND --- */ @@ -563,6 +566,13 @@ int sdlog_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- AIRSPEED --- */ + /* subscribe to ORB for airspeed */ + subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + fds[fdsc_count].fd = subs.airspeed_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -655,6 +665,7 @@ int sdlog_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); orb_copy(ORB_ID(differential_pressure), subs.diff_pressure_sub, &buf.diff_pressure); + orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed); orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt); /* if skipping is on or logging is disabled, ignore */ @@ -691,9 +702,9 @@ int sdlog_thread_main(int argc, char *argv[]) .vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw}, .control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]}, .flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}, - .diff_pressure = buf.diff_pressure.differential_pressure_mbar, - .ind_airspeed = buf.diff_pressure.indicated_airspeed_m_s, - .true_airspeed = buf.diff_pressure.true_airspeed_m_s + .diff_pressure = buf.diff_pressure.differential_pressure_pa, + .ind_airspeed = buf.airspeed.indicated_airspeed_m_s, + .true_airspeed = buf.airspeed.true_airspeed_m_s }; /* put into buffer for later IO */ diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index d725c7727..2cf3b92ef 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -77,6 +77,7 @@ #include #include #include +#include #define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ #define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ @@ -156,6 +157,8 @@ private: int _mag_sub; /**< raw mag data subscription */ int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _differential_pressure_sub; /**< raw differential pressure subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ @@ -165,6 +168,7 @@ private: orb_advert_t _rc_pub; /**< raw r/c control topic */ orb_advert_t _battery_pub; /**< battery status */ orb_advert_t _airspeed_pub; /**< airspeed */ + orb_advert_t _differential_pressure_pub; /**< differential_pressure */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -172,6 +176,7 @@ private: struct battery_status_s _battery_status; /**< battery status */ struct baro_report _barometer; /**< barometer data */ struct differential_pressure_s _differential_pressure; + struct airspeed_s _airspeed; struct { float min[_rc_max_chan_count]; @@ -330,6 +335,14 @@ private: */ void baro_poll(struct sensor_combined_s &raw); + /** + * Poll the differential pressure sensor for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void differential_pressure_poll(struct sensor_combined_s &raw); + /** * Check for changes in vehicle status. */ @@ -398,6 +411,7 @@ Sensors::Sensors() : _rc_pub(-1), _battery_pub(-1), _airspeed_pub(-1), + _differential_pressure_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) @@ -887,6 +901,27 @@ Sensors::baro_poll(struct sensor_combined_s &raw) } } +void +Sensors::differential_pressure_poll(struct sensor_combined_s &raw) +{ + bool updated; + orb_check(_differential_pressure_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(differential_pressure), _differential_pressure_sub, &_differential_pressure); + + float airspeed_true = calc_true_airspeed(_differential_pressure.differential_pressure_pa + raw.baro_pres_mbar*1e2f, + raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - 5.0f); //factor 1e2 for conversion from mBar to Pa + // XXX HACK - true temperature is much less than indicated temperature in baro, + // subtract 5 degrees in an attempt to account for the electrical upheating of the PCB + + float airspeed_indicated = calc_indicated_airspeed(_differential_pressure.differential_pressure_pa); + + raw.differential_pressure_pa = _differential_pressure.differential_pressure_pa; + raw.differential_pressure_counter++; + } +} + void Sensors::vehicle_status_poll() { @@ -1045,31 +1080,18 @@ Sensors::adc_poll(struct sensor_combined_s &raw) */ if (voltage > 0.4f) { - float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP sensor - - float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f, - _barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa - // XXX HACK - true temperature is much less than indicated temperature in baro, - // subtract 5 degrees in an attempt to account for the electrical upheating of the PCB - - float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa); - - //printf("voltage: %.4f, diff_pres_pa %.4f, baro press %.4f Pa, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)_barometer.pressure*1e2f, (double)airspeed_indicated, (double)airspeed_true); + float diff_pres_pa = voltage * 1000.0f - _parameters.airspeed_offset; //for MPXV7002DP sensor _differential_pressure.timestamp = hrt_absolute_time(); - _differential_pressure.static_pressure_mbar = _barometer.pressure; - _differential_pressure.differential_pressure_mbar = diff_pres_pa*1e-2f; - _differential_pressure.temperature_celcius = _barometer.temperature; - _differential_pressure.indicated_airspeed_m_s = airspeed_indicated; - _differential_pressure.true_airspeed_m_s = airspeed_true; + _differential_pressure.differential_pressure_pa = diff_pres_pa; _differential_pressure.voltage = voltage; /* announce the airspeed if needed, just publish else */ - if (_airspeed_pub > 0) { - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_differential_pressure); + if (_differential_pressure_pub > 0) { + orb_publish(ORB_ID(differential_pressure), _differential_pressure_pub, &_differential_pressure); } else { - _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_differential_pressure); + _differential_pressure_pub = orb_advertise(ORB_ID(differential_pressure), &_differential_pressure); } } } @@ -1334,6 +1356,7 @@ Sensors::task_main() gyro_poll(raw); mag_poll(raw); baro_poll(raw); + differential_pressure_poll(raw); parameter_update_poll(true /* forced */); diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 136375140..4197f6fb2 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -122,6 +122,9 @@ ORB_DEFINE(optical_flow, struct optical_flow_s); #include "topics/omnidirectional_flow.h" ORB_DEFINE(omnidirectional_flow, struct omnidirectional_flow_s); +#include "topics/airspeed.h" +ORB_DEFINE(airspeed, struct airspeed_s); + #include "topics/differential_pressure.h" ORB_DEFINE(differential_pressure, struct differential_pressure_s); diff --git a/apps/uORB/topics/differential_pressure.h b/apps/uORB/topics/differential_pressure.h index d5e4bf37e..ac5220619 100644 --- a/apps/uORB/topics/differential_pressure.h +++ b/apps/uORB/topics/differential_pressure.h @@ -49,16 +49,13 @@ */ /** - * Differential pressure and airspeed + * Differential pressure. */ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ - float static_pressure_mbar; /**< Static / environment pressure */ - float differential_pressure_mbar; /**< Differential pressure reading */ - float temperature_celcius; /**< ambient temperature in celcius, -1 if unknown */ - float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */ - float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ - float voltage; /**< Voltage from the airspeed sensor (voltage divider already compensated) */ + float differential_pressure_pa; /**< Differential pressure reading */ + float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ + }; /** diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h index 961ee8b4a..ad88e4b6e 100644 --- a/apps/uORB/topics/sensor_combined.h +++ b/apps/uORB/topics/sensor_combined.h @@ -103,6 +103,8 @@ struct sensor_combined_s { float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ uint32_t baro_counter; /**< Number of raw baro measurements taken */ + float differential_pressure_pa; /**< Airspeed sensor differential pressure */ + uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */ }; /** -- cgit v1.2.3 From 696e48fbf38de9d0ac12494cb2749ba3b04f852f Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 19 Apr 2013 18:28:06 +0200 Subject: Cleanup variable names and such --- apps/commander/commander.c | 30 +++++++++++----------- apps/drivers/ets_airspeed/ets_airspeed.cpp | 8 +++--- apps/sdlog/sdlog.c | 12 ++++----- apps/sensors/sensor_params.c | 2 +- apps/sensors/sensors.cpp | 40 +++++++++++++++--------------- apps/uORB/topics/sensor_combined.h | 2 +- 6 files changed, 47 insertions(+), 47 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index fcfffcfef..2980ab118 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -797,8 +797,8 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) const int calibration_count = 2500; - int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s differential_pressure; + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; int calibration_counter = 0; float diff_pres_offset = 0.0f; @@ -806,13 +806,13 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) while (calibration_counter < calibration_count) { /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } }; + struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; int poll_ret = poll(fds, 1, 1000); if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure); - diff_pres_offset += differential_pressure.differential_pressure_pa; + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + diff_pres_offset += diff_pres.differential_pressure_pa; calibration_counter++; } else if (poll_ret == 0) { @@ -826,7 +826,7 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) if (isfinite(diff_pres_offset)) { - if (param_set(param_find("SENS_VAIR_OFF"), &(diff_pres_offset))) { + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, "Setting offs failed!"); } @@ -856,7 +856,7 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) status->flag_preflight_airspeed_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); - close(sub_differential_pressure); + close(diff_pres_sub); } @@ -1477,10 +1477,10 @@ int commander_thread_main(int argc, char *argv[]) struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); - int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s differential_pressure; - memset(&differential_pressure, 0, sizeof(differential_pressure)); - uint64_t last_differential_pressure_time = 0; + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + memset(&diff_pres, 0, sizeof(diff_pres)); + uint64_t last_diff_pres_time = 0; /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1535,11 +1535,11 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); } - orb_check(differential_pressure_sub, &new_data); + orb_check(diff_pres_sub, &new_data); if (new_data) { - orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure); - last_differential_pressure_time = differential_pressure.timestamp; + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + last_diff_pres_time = diff_pres.timestamp; } orb_check(cmd_sub, &new_data); @@ -1754,7 +1754,7 @@ int commander_thread_main(int argc, char *argv[]) } /* Check for valid airspeed/differential pressure measurements */ - if (hrt_absolute_time() - last_differential_pressure_time < 2000000) { + if (hrt_absolute_time() - last_diff_pres_time < 2000000) { current_status.flag_airspeed_valid = true; } else { diff --git a/apps/drivers/ets_airspeed/ets_airspeed.cpp b/apps/drivers/ets_airspeed/ets_airspeed.cpp index 860baa760..943848d43 100644 --- a/apps/drivers/ets_airspeed/ets_airspeed.cpp +++ b/apps/drivers/ets_airspeed/ets_airspeed.cpp @@ -121,7 +121,7 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; - int _differential_pressure_offset; + int _diff_pres_offset; orb_advert_t _airspeed_pub; @@ -191,7 +191,7 @@ ETS_AIRSPEED::ETS_AIRSPEED(int bus, int address) : _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")), - _differential_pressure_offset(0) + _diff_pres_offset(0) { // enable debug() calls _debug_enabled = true; @@ -235,7 +235,7 @@ ETS_AIRSPEED::init() if (_airspeed_pub < 0) debug("failed to create airspeed sensor object. Did you start uOrb?"); - param_get(param_find("SENS_VAIR_OFF"), &_differential_pressure_offset); + param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset); ret = OK; /* sensor is ok, but we don't really know if it is within range */ @@ -455,7 +455,7 @@ ETS_AIRSPEED::collect() uint16_t diff_pres_pa = val[1] << 8 | val[0]; /* adjust if necessary */ - diff_pres_pa -= _differential_pressure_offset; + diff_pres_pa -= _diff_pres_offset; //log("measurement: %0.2f m/s", calc_indicated_airspeed((float)_reports[_next_report].diff_pressure)); _reports[_next_report].timestamp = hrt_absolute_time(); diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 46b232c34..84a9eb6ac 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -444,7 +444,7 @@ int sdlog_thread_main(int argc, char *argv[]) struct vehicle_vicon_position_s vicon_pos; struct optical_flow_s flow; struct battery_status_s batt; - struct differential_pressure_s diff_pressure; + struct differential_pressure_s diff_pres; struct airspeed_s airspeed; } buf; memset(&buf, 0, sizeof(buf)); @@ -463,7 +463,7 @@ int sdlog_thread_main(int argc, char *argv[]) int vicon_pos_sub; int flow_sub; int batt_sub; - int diff_pressure_sub; + int diff_pres_sub; int airspeed_sub; } subs; @@ -561,8 +561,8 @@ int sdlog_thread_main(int argc, char *argv[]) /* --- DIFFERENTIAL PRESSURE --- */ /* subscribe to ORB for flow measurements */ - subs.diff_pressure_sub = orb_subscribe(ORB_ID(differential_pressure)); - fds[fdsc_count].fd = subs.diff_pressure_sub; + subs.diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + fds[fdsc_count].fd = subs.diff_pres_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -664,7 +664,7 @@ int sdlog_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); - orb_copy(ORB_ID(differential_pressure), subs.diff_pressure_sub, &buf.diff_pressure); + orb_copy(ORB_ID(differential_pressure), subs.diff_pres_sub, &buf.diff_pres); orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed); orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt); @@ -702,7 +702,7 @@ int sdlog_thread_main(int argc, char *argv[]) .vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw}, .control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]}, .flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}, - .diff_pressure = buf.diff_pressure.differential_pressure_pa, + .diff_pressure = buf.diff_pres.differential_pressure_pa, .ind_airspeed = buf.airspeed.indicated_airspeed_m_s, .true_airspeed = buf.airspeed.true_airspeed_m_s }; diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index a1ef9d136..da2dfcca6 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -64,7 +64,7 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); -PARAM_DEFINE_FLOAT(SENS_VAIR_OFF, 2.5f); +PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 2.5f); PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 2cf3b92ef..ab8818b40 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -158,7 +158,7 @@ private: int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ int _airspeed_sub; /**< airspeed subscription */ - int _differential_pressure_sub; /**< raw differential pressure subscription */ + int _diff_pres_sub; /**< raw differential pressure subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ @@ -168,14 +168,14 @@ private: orb_advert_t _rc_pub; /**< raw r/c control topic */ orb_advert_t _battery_pub; /**< battery status */ orb_advert_t _airspeed_pub; /**< airspeed */ - orb_advert_t _differential_pressure_pub; /**< differential_pressure */ + orb_advert_t _diff_pres_pub; /**< differential_pressure */ perf_counter_t _loop_perf; /**< loop performance counter */ struct rc_channels_s _rc; /**< r/c channel data */ struct battery_status_s _battery_status; /**< battery status */ struct baro_report _barometer; /**< barometer data */ - struct differential_pressure_s _differential_pressure; + struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; struct { @@ -341,7 +341,7 @@ private: * @param raw Combined sensor data structure into which * data should be returned. */ - void differential_pressure_poll(struct sensor_combined_s &raw); + void diff_pres_poll(struct sensor_combined_s &raw); /** * Check for changes in vehicle status. @@ -411,7 +411,7 @@ Sensors::Sensors() : _rc_pub(-1), _battery_pub(-1), _airspeed_pub(-1), - _differential_pressure_pub(-1), + _diff_pres_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) @@ -496,8 +496,8 @@ Sensors::Sensors() : _parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE"); _parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE"); - /*Airspeed offset */ - _parameter_handles.airspeed_offset = param_find("SENS_VAIR_OFF"); + /* Differential pressure offset */ + _parameter_handles.airspeed_offset = param_find("SENS_DPRES_OFF"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); @@ -902,22 +902,22 @@ Sensors::baro_poll(struct sensor_combined_s &raw) } void -Sensors::differential_pressure_poll(struct sensor_combined_s &raw) +Sensors::diff_pres_poll(struct sensor_combined_s &raw) { bool updated; - orb_check(_differential_pressure_sub, &updated); + orb_check(_diff_pres_sub, &updated); if (updated) { - orb_copy(ORB_ID(differential_pressure), _differential_pressure_sub, &_differential_pressure); + orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); - float airspeed_true = calc_true_airspeed(_differential_pressure.differential_pressure_pa + raw.baro_pres_mbar*1e2f, + float airspeed_true = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar*1e2f, raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - 5.0f); //factor 1e2 for conversion from mBar to Pa // XXX HACK - true temperature is much less than indicated temperature in baro, // subtract 5 degrees in an attempt to account for the electrical upheating of the PCB - float airspeed_indicated = calc_indicated_airspeed(_differential_pressure.differential_pressure_pa); + float airspeed_indicated = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); - raw.differential_pressure_pa = _differential_pressure.differential_pressure_pa; + raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; raw.differential_pressure_counter++; } } @@ -1082,16 +1082,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw) float diff_pres_pa = voltage * 1000.0f - _parameters.airspeed_offset; //for MPXV7002DP sensor - _differential_pressure.timestamp = hrt_absolute_time(); - _differential_pressure.differential_pressure_pa = diff_pres_pa; - _differential_pressure.voltage = voltage; + _diff_pres.timestamp = hrt_absolute_time(); + _diff_pres.differential_pressure_pa = diff_pres_pa; + _diff_pres.voltage = voltage; /* announce the airspeed if needed, just publish else */ - if (_differential_pressure_pub > 0) { - orb_publish(ORB_ID(differential_pressure), _differential_pressure_pub, &_differential_pressure); + if (_diff_pres_pub > 0) { + orb_publish(ORB_ID(differential_pressure), _diff_pres_pub, &_diff_pres); } else { - _differential_pressure_pub = orb_advertise(ORB_ID(differential_pressure), &_differential_pressure); + _diff_pres_pub = orb_advertise(ORB_ID(differential_pressure), &_diff_pres); } } } @@ -1356,7 +1356,7 @@ Sensors::task_main() gyro_poll(raw); mag_poll(raw); baro_poll(raw); - differential_pressure_poll(raw); + diff_pres_poll(raw); parameter_update_poll(true /* forced */); diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h index ad88e4b6e..9a76b5182 100644 --- a/apps/uORB/topics/sensor_combined.h +++ b/apps/uORB/topics/sensor_combined.h @@ -103,7 +103,7 @@ struct sensor_combined_s { float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ uint32_t baro_counter; /**< Number of raw baro measurements taken */ - float differential_pressure_pa; /**< Airspeed sensor differential pressure */ + float differential_pressure_pa; /**< Airspeed sensor differential pressure */ uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */ }; -- cgit v1.2.3 From 853ba612b132f0a8f41fae1dbadc68ef3960f0d0 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 19 Apr 2013 18:28:47 +0200 Subject: Add missing uORB topic --- apps/uORB/topics/airspeed.h | 67 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 67 insertions(+) create mode 100644 apps/uORB/topics/airspeed.h diff --git a/apps/uORB/topics/airspeed.h b/apps/uORB/topics/airspeed.h new file mode 100644 index 000000000..a3da3758f --- /dev/null +++ b/apps/uORB/topics/airspeed.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file airspeed.h + * + * Definition of airspeed topic + */ + +#ifndef TOPIC_AIRSPEED_H_ +#define TOPIC_AIRSPEED_H_ + +#include "../uORB.h" +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * Airspeed + */ +struct airspeed_s { + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */ + float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(airspeed); + +#endif -- cgit v1.2.3 From 48f815860b5900f3770486d88aea9084c75441e0 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 21 Apr 2013 01:29:07 +0200 Subject: Debugging, cleanup and added airspeed to HoTT telemetry. --- apps/drivers/ets_airspeed/ets_airspeed.cpp | 31 ++++++++++++++++++++++-------- apps/hott_telemetry/messages.c | 12 ++++++++++++ apps/sensors/sensor_params.c | 2 +- apps/sensors/sensors.cpp | 30 +++++++++++++++++++++-------- apps/uORB/topics/differential_pressure.h | 7 ++++--- 5 files changed, 62 insertions(+), 20 deletions(-) diff --git a/apps/drivers/ets_airspeed/ets_airspeed.cpp b/apps/drivers/ets_airspeed/ets_airspeed.cpp index 943848d43..276f4bf59 100644 --- a/apps/drivers/ets_airspeed/ets_airspeed.cpp +++ b/apps/drivers/ets_airspeed/ets_airspeed.cpp @@ -81,7 +81,11 @@ #define READ_CMD 0x07 /* Read the data */ /* Max measurement rate is 100Hz */ -#define CONVERSION_INTERVAL (1000000 / 10) /* microseconds */ +#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ + +/* The Eagle Tree Airspeed V3 can only provide accurate readings + for speeds from 15km/h upwards. */ +#define MIN_ACCURATE_DIFF_PRES_PA 12 /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -222,6 +226,8 @@ ETS_AIRSPEED::init() /* allocate basic report buffers */ _num_reports = 2; _reports = new struct differential_pressure_s[_num_reports]; + for (int i = 0; i < _num_reports; i++) + _reports[i].max_differential_pressure_pa = 0; if (_reports == nullptr) goto out; @@ -235,8 +241,6 @@ ETS_AIRSPEED::init() if (_airspeed_pub < 0) debug("failed to create airspeed sensor object. Did you start uOrb?"); - param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset); - ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; @@ -454,13 +458,24 @@ ETS_AIRSPEED::collect() uint16_t diff_pres_pa = val[1] << 8 | val[0]; - /* adjust if necessary */ - diff_pres_pa -= _diff_pres_offset; - //log("measurement: %0.2f m/s", calc_indicated_airspeed((float)_reports[_next_report].diff_pressure)); + param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset); + if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { + diff_pres_pa = 0; + } else { + diff_pres_pa -= _diff_pres_offset; + } + + // XXX we may want to smooth out the readings to remove noise. + _reports[_next_report].timestamp = hrt_absolute_time(); _reports[_next_report].differential_pressure_pa = diff_pres_pa; + // Track maximum differential pressure measured (so we can work out top speed). + if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) { + _reports[_next_report].max_differential_pressure_pa = diff_pres_pa; + } + /* announce the airspeed if needed, just publish else */ orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); @@ -684,7 +699,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %0.3f pa", (double) report.differential_pressure_pa); + warnx("diff pressure: %d pa", report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) @@ -709,7 +724,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("diff pressure: %0.3f pa", (double) report.differential_pressure_pa); + warnx("diff pressure: %d pa", report.differential_pressure_pa); } errx(0, "PASS"); diff --git a/apps/hott_telemetry/messages.c b/apps/hott_telemetry/messages.c index 8bfb99773..0e466e820 100644 --- a/apps/hott_telemetry/messages.c +++ b/apps/hott_telemetry/messages.c @@ -42,9 +42,11 @@ #include #include #include +#include #include #include +static int airspeed_sub = -1; static int battery_sub = -1; static int sensor_sub = -1; @@ -52,6 +54,7 @@ void messages_init(void) { battery_sub = orb_subscribe(ORB_ID(battery_status)); sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + airspeed_sub = orb_subscribe(ORB_ID(airspeed)); } void build_eam_response(uint8_t *buffer, int *size) @@ -81,6 +84,15 @@ void build_eam_response(uint8_t *buffer, int *size) msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; + /* get a local copy of the current sensor values */ + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); + orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed); + + uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6); + msg.speed_L = (uint8_t)speed & 0xff; + msg.speed_H = (uint8_t)(speed >> 8) & 0xff; + msg.stop = STOP_BYTE; memcpy(buffer, &msg, *size); diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index da2dfcca6..0bab992a7 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -64,7 +64,7 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); -PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 2.5f); +PARAM_DEFINE_INT32(SENS_DPRES_OFF, 1667); PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index ab8818b40..fcd1d869f 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -192,7 +192,7 @@ private: float mag_scale[3]; float accel_offset[3]; float accel_scale[3]; - float airspeed_offset; + int diff_pres_offset_pa; int rc_type; @@ -241,7 +241,7 @@ private: param_t accel_scale[3]; param_t mag_offset[3]; param_t mag_scale[3]; - param_t airspeed_offset; + param_t diff_pres_offset_pa; param_t rc_map_roll; param_t rc_map_pitch; @@ -497,7 +497,7 @@ Sensors::Sensors() : _parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE"); /* Differential pressure offset */ - _parameter_handles.airspeed_offset = param_find("SENS_DPRES_OFF"); + _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); @@ -707,7 +707,7 @@ Sensors::parameters_update() param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2])); /* Airspeed offset */ - param_get(_parameter_handles.airspeed_offset, &(_parameters.airspeed_offset)); + param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { @@ -910,15 +910,26 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) if (updated) { orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); + raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; + raw.differential_pressure_counter++; + float airspeed_true = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar*1e2f, raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - 5.0f); //factor 1e2 for conversion from mBar to Pa // XXX HACK - true temperature is much less than indicated temperature in baro, // subtract 5 degrees in an attempt to account for the electrical upheating of the PCB float airspeed_indicated = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); - - raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; - raw.differential_pressure_counter++; + + _airspeed.indicated_airspeed_m_s = airspeed_indicated; + _airspeed.true_airspeed_m_s = airspeed_true; + + /* announce the airspeed if needed, just publish else */ + if (_airspeed_pub > 0) { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed); + + } else { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed); + } } } @@ -1080,7 +1091,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) */ if (voltage > 0.4f) { - float diff_pres_pa = voltage * 1000.0f - _parameters.airspeed_offset; //for MPXV7002DP sensor + float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor _diff_pres.timestamp = hrt_absolute_time(); _diff_pres.differential_pressure_pa = diff_pres_pa; @@ -1330,6 +1341,7 @@ Sensors::task_main() _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); + _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -1405,6 +1417,8 @@ Sensors::task_main() /* check battery voltage */ adc_poll(raw); + diff_pres_poll(raw); + /* Inform other processes that new data is available to copy */ if (_publishing) orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); diff --git a/apps/uORB/topics/differential_pressure.h b/apps/uORB/topics/differential_pressure.h index ac5220619..8ce85213b 100644 --- a/apps/uORB/topics/differential_pressure.h +++ b/apps/uORB/topics/differential_pressure.h @@ -52,9 +52,10 @@ * Differential pressure. */ struct differential_pressure_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ - float differential_pressure_pa; /**< Differential pressure reading */ - float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint16_t differential_pressure_pa; /**< Differential pressure reading */ + uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */ + float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ }; -- cgit v1.2.3 From 715e3e2ebe0546edfa8c053ff90f4f1fdc521da7 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 22 Apr 2013 08:51:49 +0200 Subject: Cleanup --- apps/drivers/drv_airspeed.h | 14 -------------- apps/drivers/ets_airspeed/ets_airspeed.cpp | 17 ++++++++++------- apps/sensors/sensors.cpp | 18 +++++++++--------- 3 files changed, 19 insertions(+), 30 deletions(-) diff --git a/apps/drivers/drv_airspeed.h b/apps/drivers/drv_airspeed.h index 269ee4559..54213c075 100644 --- a/apps/drivers/drv_airspeed.h +++ b/apps/drivers/drv_airspeed.h @@ -46,20 +46,6 @@ #define AIRSPEED_DEVICE_PATH "/dev/airspeed" -/** - * Airspeed report structure. Reads from the device must be in multiples of this - * structure. - */ -//struct airspeed_report { -// uint64_t timestamp; -// uint8_t diff_pressure; /** differential pressure in Pa */ -//}; - -/* - * ObjDev tag for raw range finder data. - */ -//ORB_DECLARE(sensor_differential_pressure); - /* * ioctl() definitions * diff --git a/apps/drivers/ets_airspeed/ets_airspeed.cpp b/apps/drivers/ets_airspeed/ets_airspeed.cpp index 276f4bf59..88e0fbb13 100644 --- a/apps/drivers/ets_airspeed/ets_airspeed.cpp +++ b/apps/drivers/ets_airspeed/ets_airspeed.cpp @@ -74,20 +74,22 @@ #include /* Configuration Constants */ -#define I2C_BUS PX4_I2C_BUS_ESC +#define I2C_BUS PX4_I2C_BUS_ESC // XXX Replace with PX4_I2C_BUS_EXPANSION before submitting. #define I2C_ADDRESS 0x75 /* ETS_AIRSPEED Registers addresses */ #define READ_CMD 0x07 /* Read the data */ -/* Max measurement rate is 100Hz */ +/* Measurement rate is 100Hz */ #define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ -/* The Eagle Tree Airspeed V3 can only provide accurate readings - for speeds from 15km/h upwards. */ +/** + * The Eagle Tree Airspeed V3 can only provide accurate readings + * for speeds from 15km/h upwards. + */ #define MIN_ACCURATE_DIFF_PRES_PA 12 -/* oddly, ERROR is not defined for c++ */ +/* Oddly, ERROR is not defined for C++ */ #ifdef ERROR # undef ERROR #endif @@ -109,8 +111,8 @@ public: virtual int ioctl(struct file *filp, int cmd, unsigned long arg); /** - * Diagnostics - print some basic information about the driver. - */ + * Diagnostics - print some basic information about the driver. + */ void print_info(); protected: @@ -163,6 +165,7 @@ private: void cycle(); int measure(); int collect(); + /** * Static trampoline from the workq context; because we don't have a * generic workq wrapper yet. diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index fcd1d869f..8b6f18473 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -99,6 +99,12 @@ #define BAT_VOL_LOWPASS_2 0.01f #define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f +/** + * HACK - true temperature is much less than indicated temperature in baro, + * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB + */ +#define PCB_TEMP_ESTIMATE_DEG 5.0f + #define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */ #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) @@ -913,15 +919,9 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; raw.differential_pressure_counter++; - float airspeed_true = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar*1e2f, - raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - 5.0f); //factor 1e2 for conversion from mBar to Pa - // XXX HACK - true temperature is much less than indicated temperature in baro, - // subtract 5 degrees in an attempt to account for the electrical upheating of the PCB - - float airspeed_indicated = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); - - _airspeed.indicated_airspeed_m_s = airspeed_indicated; - _airspeed.true_airspeed_m_s = airspeed_true; + _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); + _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar*1e2f, + raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { -- cgit v1.2.3 From ad212ee628cd08c5c063757e4a5a2edc82392f3b Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 22 Apr 2013 15:00:15 +0200 Subject: More cleanups --- apps/drivers/ets_airspeed/ets_airspeed.cpp | 65 +++++++++++++++++++++--------- 1 file changed, 45 insertions(+), 20 deletions(-) diff --git a/apps/drivers/ets_airspeed/ets_airspeed.cpp b/apps/drivers/ets_airspeed/ets_airspeed.cpp index 88e0fbb13..cede0534d 100644 --- a/apps/drivers/ets_airspeed/ets_airspeed.cpp +++ b/apps/drivers/ets_airspeed/ets_airspeed.cpp @@ -73,22 +73,23 @@ #include #include -/* Configuration Constants */ -#define I2C_BUS PX4_I2C_BUS_ESC // XXX Replace with PX4_I2C_BUS_EXPANSION before submitting. -#define I2C_ADDRESS 0x75 +/* Default I2C bus */ +#define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION -/* ETS_AIRSPEED Registers addresses */ -#define READ_CMD 0x07 /* Read the data */ - -/* Measurement rate is 100Hz */ -#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ +/* I2C bus address */ +#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ +/* Register address */ +#define READ_CMD 0x07 /* Read the data */ + /** - * The Eagle Tree Airspeed V3 can only provide accurate readings - * for speeds from 15km/h upwards. + * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. */ #define MIN_ACCURATE_DIFF_PRES_PA 12 +/* Measurement rate is 100Hz */ +#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ + /* Oddly, ERROR is not defined for C++ */ #ifdef ERROR # undef ERROR @@ -102,7 +103,7 @@ static const int ERROR = -1; class ETS_AIRSPEED : public device::I2C { public: - ETS_AIRSPEED(int bus = I2C_BUS, int address = I2C_ADDRESS); + ETS_AIRSPEED(int bus, int address = I2C_ADDRESS); virtual ~ETS_AIRSPEED(); virtual int init(); @@ -194,11 +195,11 @@ ETS_AIRSPEED::ETS_AIRSPEED(int bus, int address) : _sensor_ok(false), _measure_ticks(0), _collect_phase(false), + _diff_pres_offset(0), _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")), - _diff_pres_offset(0) + _buffer_overflows(perf_alloc(PC_COUNT, "ETS_AIRSPEED_buffer_overflows")) { // enable debug() calls _debug_enabled = true; @@ -229,7 +230,7 @@ ETS_AIRSPEED::init() /* allocate basic report buffers */ _num_reports = 2; _reports = new struct differential_pressure_s[_num_reports]; - for (int i = 0; i < _num_reports; i++) + for (unsigned i = 0; i < _num_reports; i++) _reports[i].max_differential_pressure_pa = 0; if (_reports == nullptr) @@ -613,7 +614,7 @@ const int ERROR = -1; ETS_AIRSPEED *g_dev; -void start(); +void start(int i2c_bus); void stop(); void test(); void reset(); @@ -623,7 +624,7 @@ void info(); * Start the driver. */ void -start() +start(int i2c_bus) { int fd; @@ -631,7 +632,7 @@ start() errx(1, "already started"); /* create the driver */ - g_dev = new ETS_AIRSPEED(I2C_BUS); + g_dev = new ETS_AIRSPEED(i2c_bus); if (g_dev == nullptr) goto fail; @@ -664,7 +665,8 @@ fail: /** * Stop the driver */ -void stop() +void +stop() { if (g_dev != nullptr) { @@ -770,14 +772,36 @@ info() } // namespace + +static void +ets_airspeed_usage() +{ + fprintf(stderr, "usage: ets_airspeed [options] command\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"); +} + int ets_airspeed_main(int argc, char *argv[]) { + int i2c_bus = PX4_I2C_BUS_DEFAULT; + + int i; + for (i = 1; i < argc; i++) { + if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { + if (argc > i + 1) { + i2c_bus = atoi(argv[i + 1]); + } + } + } + /* * Start/load the driver. */ if (!strcmp(argv[1], "start")) - ets_airspeed::start(); + ets_airspeed::start(i2c_bus); /* * Stop the driver @@ -803,5 +827,6 @@ ets_airspeed_main(int argc, char *argv[]) if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) ets_airspeed::info(); - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); + ets_airspeed_usage(); + exit(0); } -- cgit v1.2.3 From 24630f15b6d0b465bd62f1105def4e96ffc92e10 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 22 Apr 2013 21:30:20 +0200 Subject: Yet more cleanups. --- apps/drivers/drv_airspeed.h | 1 + apps/drivers/ets_airspeed/Makefile | 2 +- apps/drivers/ets_airspeed/ets_airspeed.cpp | 8 ++++---- apps/px4io/controls.c | 2 +- 4 files changed, 7 insertions(+), 6 deletions(-) diff --git a/apps/drivers/drv_airspeed.h b/apps/drivers/drv_airspeed.h index 54213c075..bffc35c62 100644 --- a/apps/drivers/drv_airspeed.h +++ b/apps/drivers/drv_airspeed.h @@ -33,6 +33,7 @@ /** * @file Airspeed driver interface. + * @author Simon Wilks */ #ifndef _DRV_AIRSPEED_H diff --git a/apps/drivers/ets_airspeed/Makefile b/apps/drivers/ets_airspeed/Makefile index 9089d97af..f6639b470 100644 --- a/apps/drivers/ets_airspeed/Makefile +++ b/apps/drivers/ets_airspeed/Makefile @@ -37,6 +37,6 @@ APPNAME = ets_airspeed PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 +STACKSIZE = 1024 include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/ets_airspeed/ets_airspeed.cpp b/apps/drivers/ets_airspeed/ets_airspeed.cpp index cede0534d..5bff4c720 100644 --- a/apps/drivers/ets_airspeed/ets_airspeed.cpp +++ b/apps/drivers/ets_airspeed/ets_airspeed.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file airspeed.cpp + * @file ets_airspeed.cpp * @author Simon Wilks * * Driver for the Eagle Tree Airspeed V3 connected via I2C. @@ -197,9 +197,9 @@ ETS_AIRSPEED::ETS_AIRSPEED(int bus, int address) : _collect_phase(false), _diff_pres_offset(0), _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")) + _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")) { // enable debug() calls _debug_enabled = true; diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index e51983071..dc36f6c93 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -70,7 +70,7 @@ controls_init(void) unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; - r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 950; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 30; -- cgit v1.2.3 From 5b60991c63b0c6b87632369fde73236263670448 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 23 Apr 2013 08:49:33 +0200 Subject: Style fix: ETS_AIRSPEED > ETSAirspeed --- apps/drivers/ets_airspeed/ets_airspeed.cpp | 46 +++++++++++++++--------------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/apps/drivers/ets_airspeed/ets_airspeed.cpp b/apps/drivers/ets_airspeed/ets_airspeed.cpp index 5bff4c720..e50395e47 100644 --- a/apps/drivers/ets_airspeed/ets_airspeed.cpp +++ b/apps/drivers/ets_airspeed/ets_airspeed.cpp @@ -100,11 +100,11 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -class ETS_AIRSPEED : public device::I2C +class ETSAirspeed : public device::I2C { public: - ETS_AIRSPEED(int bus, int address = I2C_ADDRESS); - virtual ~ETS_AIRSPEED(); + ETSAirspeed(int bus, int address = I2C_ADDRESS); + virtual ~ETSAirspeed(); virtual int init(); @@ -186,8 +186,8 @@ private: */ extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); -ETS_AIRSPEED::ETS_AIRSPEED(int bus, int address) : - I2C("ETS_AIRSPEED", AIRSPEED_DEVICE_PATH, bus, address, 100000), +ETSAirspeed::ETSAirspeed(int bus, int address) : + I2C("ETSAirspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), _num_reports(0), _next_report(0), _oldest_report(0), @@ -208,7 +208,7 @@ ETS_AIRSPEED::ETS_AIRSPEED(int bus, int address) : memset(&_work, 0, sizeof(_work)); } -ETS_AIRSPEED::~ETS_AIRSPEED() +ETSAirspeed::~ETSAirspeed() { /* make sure we are truly inactive */ stop(); @@ -219,7 +219,7 @@ ETS_AIRSPEED::~ETS_AIRSPEED() } int -ETS_AIRSPEED::init() +ETSAirspeed::init() { int ret = ERROR; @@ -253,13 +253,13 @@ out: } int -ETS_AIRSPEED::probe() +ETSAirspeed::probe() { return measure(); } int -ETS_AIRSPEED::ioctl(struct file *filp, int cmd, unsigned long arg) +ETSAirspeed::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -363,7 +363,7 @@ ETS_AIRSPEED::ioctl(struct file *filp, int cmd, unsigned long arg) } ssize_t -ETS_AIRSPEED::read(struct file *filp, char *buffer, size_t buflen) +ETSAirspeed::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct differential_pressure_s); int ret = 0; @@ -422,7 +422,7 @@ ETS_AIRSPEED::read(struct file *filp, char *buffer, size_t buflen) } int -ETS_AIRSPEED::measure() +ETSAirspeed::measure() { int ret; @@ -444,7 +444,7 @@ ETS_AIRSPEED::measure() } int -ETS_AIRSPEED::collect() +ETSAirspeed::collect() { int ret = -EIO; @@ -503,14 +503,14 @@ ETS_AIRSPEED::collect() } void -ETS_AIRSPEED::start() +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)&ETS_AIRSPEED::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&ETSAirspeed::cycle_trampoline, this, 1); /* notify about state change */ struct subsystem_info_s info = { @@ -528,21 +528,21 @@ ETS_AIRSPEED::start() } void -ETS_AIRSPEED::stop() +ETSAirspeed::stop() { work_cancel(HPWORK, &_work); } void -ETS_AIRSPEED::cycle_trampoline(void *arg) +ETSAirspeed::cycle_trampoline(void *arg) { - ETS_AIRSPEED *dev = (ETS_AIRSPEED *)arg; + ETSAirspeed *dev = (ETSAirspeed *)arg; dev->cycle(); } void -ETS_AIRSPEED::cycle() +ETSAirspeed::cycle() { /* collection phase? */ if (_collect_phase) { @@ -566,7 +566,7 @@ ETS_AIRSPEED::cycle() /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, &_work, - (worker_t)&ETS_AIRSPEED::cycle_trampoline, + (worker_t)&ETSAirspeed::cycle_trampoline, this, _measure_ticks - USEC2TICK(CONVERSION_INTERVAL)); @@ -584,13 +584,13 @@ ETS_AIRSPEED::cycle() /* schedule a fresh cycle call when the measurement is done */ work_queue(HPWORK, &_work, - (worker_t)&ETS_AIRSPEED::cycle_trampoline, + (worker_t)&ETSAirspeed::cycle_trampoline, this, USEC2TICK(CONVERSION_INTERVAL)); } void -ETS_AIRSPEED::print_info() +ETSAirspeed::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -612,7 +612,7 @@ namespace ets_airspeed #endif const int ERROR = -1; -ETS_AIRSPEED *g_dev; +ETSAirspeed *g_dev; void start(int i2c_bus); void stop(); @@ -632,7 +632,7 @@ start(int i2c_bus) errx(1, "already started"); /* create the driver */ - g_dev = new ETS_AIRSPEED(i2c_bus); + g_dev = new ETSAirspeed(i2c_bus); if (g_dev == nullptr) goto fail; -- cgit v1.2.3