From fc82ed0c693635ccfaeee4f060b93b1a1c083acb Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 20 Mar 2013 13:21:00 +0100 Subject: [PATCH 01/13] 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 0000000000..9089d97af2 --- /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 0000000000..4ac707c3cb --- /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'"); +} From 4f99200b0ff102c01f415a57e9f173a863483d2a Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 11 Apr 2013 08:36:54 +0200 Subject: [PATCH 02/13] Initial implementation that can read values from the ETS. --- apps/drivers/{ets => ets_airspeed}/Makefile | 0 .../{ets => ets_airspeed}/ets_airspeed.cpp | 123 +++++++++--------- apps/px4io/controls.c | 2 +- apps/uORB/objects_common.cpp | 3 + nuttx/configs/px4fmu/nsh/appconfig | 1 + 5 files changed, 67 insertions(+), 62 deletions(-) rename apps/drivers/{ets => ets_airspeed}/Makefile (100%) rename apps/drivers/{ets => ets_airspeed}/ets_airspeed.cpp (82%) diff --git a/apps/drivers/ets/Makefile b/apps/drivers/ets_airspeed/Makefile similarity index 100% rename from apps/drivers/ets/Makefile rename to apps/drivers/ets_airspeed/Makefile diff --git a/apps/drivers/ets/ets_airspeed.cpp b/apps/drivers/ets_airspeed/ets_airspeed.cpp similarity index 82% rename from apps/drivers/ets/ets_airspeed.cpp rename to apps/drivers/ets_airspeed/ets_airspeed.cpp index 4ac707c3cb..790e949e0f 100644 --- a/apps/drivers/ets/ets_airspeed.cpp +++ b/apps/drivers/ets_airspeed/ets_airspeed.cpp @@ -65,20 +65,22 @@ #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 */ +#define ETS_AIRSPEED_BUS PX4_I2C_BUS_ESC +#define ETS_AIRSPEED_ADDRESS 0x75 -/* ETS Registers addresses */ +/* ETS_AIRSPEED Registers addresses */ -#define ETS_READ_CMD 0x07 /* Read the data */ +#define ETS_AIRSPEED_READ_CMD 0x07 /* Read the data */ -#define ETS_CONVERSION_INTERVAL 60000 /* 60ms */ +/* Max measurement rate is 100Hz */ +#define ETS_AIRSPEED_CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -90,11 +92,11 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -class ETS : public device::I2C +class ETS_AIRSPEED : public device::I2C { public: - ETS(int bus = ETS_BUS, int address = ETS_BASEADDR); - virtual ~ETS(); + ETS_AIRSPEED(int bus = ETS_AIRSPEED_BUS, int address = ETS_AIRSPEED_ADDRESS); + virtual ~ETS_AIRSPEED(); virtual int init(); @@ -114,7 +116,7 @@ private: unsigned _num_reports; volatile unsigned _next_report; volatile unsigned _oldest_report; - range_finder_report *_reports; + airspeed_report *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -171,10 +173,10 @@ private: /* * Driver 'main' command. */ -extern "C" __EXPORT int ETS_main(int argc, char *argv[]); +extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); -ETS::ETS(int bus, int address) : - I2C("ETS", AIRSPEED_SENSOR_DEVICE_PATH, bus, address, 100000), +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), @@ -183,9 +185,9 @@ ETS::ETS(int bus, int address) : _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")) + _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; @@ -194,7 +196,7 @@ ETS::ETS(int bus, int address) : memset(&_work, 0, sizeof(_work)); } -ETS::~ETS() +ETS_AIRSPEED::~ETS_AIRSPEED() { /* make sure we are truly inactive */ stop(); @@ -205,7 +207,7 @@ ETS::~ETS() } int -ETS::init() +ETS_AIRSPEED::init() { int ret = ERROR; @@ -215,7 +217,7 @@ ETS::init() /* allocate basic report buffers */ _num_reports = 2; - _reports = new struct range_finder_report[_num_reports]; + _reports = new struct airspeed_report[_num_reports]; if (_reports == nullptr) goto out; @@ -224,7 +226,7 @@ ETS::init() /* 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]); + _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?"); @@ -237,13 +239,13 @@ out: } int -ETS::probe() +ETS_AIRSPEED::probe() { return measure(); } int -ETS::ioctl(struct file *filp, int cmd, unsigned long arg) +ETS_AIRSPEED::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -270,7 +272,7 @@ ETS::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_CONVERSION_INTERVAL); + _measure_ticks = USEC2TICK(ETS_AIRSPEED_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ if (want_start) @@ -288,7 +290,7 @@ ETS::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(ETS_CONVERSION_INTERVAL)) + if (ticks < USEC2TICK(ETS_AIRSPEED_CONVERSION_INTERVAL)) return -EINVAL; /* update interval for next measurement */ @@ -318,7 +320,7 @@ ETS::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; /* allocate new buffer */ - struct range_finder_report *buf = new struct range_finder_report[arg]; + struct airspeed_report *buf = new struct airspeed_report[arg]; if (nullptr == buf) return -ENOMEM; @@ -347,9 +349,9 @@ ETS::ioctl(struct file *filp, int cmd, unsigned long arg) } ssize_t -ETS::read(struct file *filp, char *buffer, size_t buflen) +ETS_AIRSPEED::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); + unsigned count = buflen / sizeof(struct airspeed_report); int ret = 0; /* buffer must be large enough */ @@ -388,7 +390,7 @@ ETS::read(struct file *filp, char *buffer, size_t buflen) } /* wait for it to complete */ - usleep(ETS_CONVERSION_INTERVAL); + usleep(ETS_AIRSPEED_CONVERSION_INTERVAL); /* run the collection phase */ if (OK != collect()) { @@ -406,14 +408,14 @@ ETS::read(struct file *filp, char *buffer, size_t buflen) } int -ETS::measure() +ETS_AIRSPEED::measure() { int ret; /* * Send the command to begin a measurement. */ - uint8_t cmd = ETS_READ_CMD; + uint8_t cmd = ETS_AIRSPEED_READ_CMD; ret = transfer(&cmd, 1, nullptr, 0); if (OK != ret) @@ -428,7 +430,7 @@ ETS::measure() } int -ETS::collect() +ETS_AIRSPEED::collect() { int ret = -EIO; @@ -449,11 +451,10 @@ ETS::collect() 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; + _reports[_next_report].speed = si_units; /* publish it */ - orb_publish(ORB_ID(_differential_pressure), _differential_pressure_topic, &_reports[_next_report]); + 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); @@ -477,14 +478,14 @@ out: } void -ETS::start() +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::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&ETS_AIRSPEED::cycle_trampoline, this, 1); /* notify about state change */ struct subsystem_info_s info = { @@ -502,21 +503,21 @@ ETS::start() } void -ETS::stop() +ETS_AIRSPEED::stop() { work_cancel(HPWORK, &_work); } void -ETS::cycle_trampoline(void *arg) +ETS_AIRSPEED::cycle_trampoline(void *arg) { - ETS *dev = (ETS *)arg; + ETS_AIRSPEED *dev = (ETS_AIRSPEED *)arg; dev->cycle(); } void -ETS::cycle() +ETS_AIRSPEED::cycle() { /* collection phase? */ if (_collect_phase) { @@ -535,14 +536,14 @@ ETS::cycle() /* * Is there a collect->measure gap? */ - if (_measure_ticks > USEC2TICK(ETS_CONVERSION_INTERVAL)) { + 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::cycle_trampoline, + (worker_t)&ETS_AIRSPEED::cycle_trampoline, this, - _measure_ticks - USEC2TICK(ETS_CONVERSION_INTERVAL)); + _measure_ticks - USEC2TICK(ETS_AIRSPEED_CONVERSION_INTERVAL)); return; } @@ -558,13 +559,13 @@ ETS::cycle() /* schedule a fresh cycle call when the measurement is done */ work_queue(HPWORK, &_work, - (worker_t)&ETS::cycle_trampoline, + (worker_t)&ETS_AIRSPEED::cycle_trampoline, this, - USEC2TICK(ETS_CONVERSION_INTERVAL)); + USEC2TICK(ETS_AIRSPEED_CONVERSION_INTERVAL)); } void -ETS::print_info() +ETS_AIRSPEED::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -577,7 +578,7 @@ ETS::print_info() /** * Local functions in support of the shell command. */ -namespace ETS +namespace ets_airspeed { /* oddly, ERROR is not defined for c++ */ @@ -586,7 +587,7 @@ namespace ETS #endif const int ERROR = -1; -ETS *g_dev; +ETS_AIRSPEED *g_dev; void start(); void stop(); @@ -606,7 +607,7 @@ start() errx(1, "already started"); /* create the driver */ - g_dev = new ETS(ETS_BUS); + g_dev = new ETS_AIRSPEED(ETS_AIRSPEED_BUS); if (g_dev == nullptr) goto fail; @@ -615,7 +616,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(AIRSPEED_SENSOR_DEVICE_PATH, O_RDONLY); + fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); if (fd < 0) goto fail; @@ -661,14 +662,14 @@ void stop() void test() { - struct range_finder_report report; + struct airspeed_report report; ssize_t sz; int ret; - int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + int fd = open(AIRSPEED_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); + 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)); @@ -677,7 +678,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); + warnx("measurement: %0.2f m", (double)report.speed); warnx("time: %lld", report.timestamp); /* start the sensor polling at 2Hz */ @@ -703,7 +704,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); + warnx("measurement: %0.3f", (double)report.speed); warnx("time: %lld", report.timestamp); } @@ -716,7 +717,7 @@ test() void reset() { - int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -748,37 +749,37 @@ info() } // namespace int -ETS_main(int argc, char *argv[]) +ets_airspeed_main(int argc, char *argv[]) { /* * Start/load the driver. */ if (!strcmp(argv[1], "start")) - ETS::start(); + ets_airspeed::start(); /* * Stop the driver */ if (!strcmp(argv[1], "stop")) - ETS::stop(); + ets_airspeed::stop(); /* * Test the driver/device. */ if (!strcmp(argv[1], "test")) - ETS::test(); + ets_airspeed::test(); /* * Reset the driver. */ if (!strcmp(argv[1], "reset")) - ETS::reset(); + ets_airspeed::reset(); /* * Print driver information. */ if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) - ETS::info(); + 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 e80a41f15c..9a0f0e5c1b 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 1363751401..cd21d556c5 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 80cf6f312c..b5f35f5140 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 From c8ac1d0b0a97f5fc926a48e64e0e116387453dcd Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 11 Apr 2013 08:37:25 +0200 Subject: [PATCH 03/13] 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 0000000000..a98f568070 --- /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 */ From 42f4a9e8800c4d5d2823b4f477c556547fe0d3d0 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 13 Apr 2013 08:47:12 +0200 Subject: [PATCH 04/13] 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 a98f568070..269ee45591 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 790e949e0f..0b535b0b5f 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 264287b10f..15bb833a99 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 cd21d556c5..1363751401 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); From c5a453cd18949d2d4673c0b343e22c22a8d2854d Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 13 Apr 2013 09:09:21 +0200 Subject: [PATCH 05/13] 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 0b535b0b5f..58d016a307 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); From df6976c8d640b395220d46f5b1fd7ecfc8ae3e04 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 19 Apr 2013 16:20:40 +0200 Subject: [PATCH 06/13] 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 7c0a25f862..fcfffcfef7 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 58d016a307..860baa760c 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 df8745d9f5..46b232c34d 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 d725c7727f..2cf3b92ef7 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 1363751401..4197f6fb2d 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 d5e4bf37ef..ac52206192 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 961ee8b4a6..ad88e4b6e1 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 */ }; /** From 696e48fbf38de9d0ac12494cb2749ba3b04f852f Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 19 Apr 2013 18:28:06 +0200 Subject: [PATCH 07/13] 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 fcfffcfef7..2980ab118e 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 860baa760c..943848d438 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 46b232c34d..84a9eb6ac5 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 a1ef9d136e..da2dfcca67 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 2cf3b92ef7..ab8818b40c 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 ad88e4b6e1..9a76b51829 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 */ }; From 853ba612b132f0a8f41fae1dbadc68ef3960f0d0 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 19 Apr 2013 18:28:47 +0200 Subject: [PATCH 08/13] 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 0000000000..a3da3758fd --- /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 From 48f815860b5900f3770486d88aea9084c75441e0 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 21 Apr 2013 01:29:07 +0200 Subject: [PATCH 09/13] 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 943848d438..276f4bf593 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 8bfb997737..0e466e8209 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 da2dfcca67..0bab992a78 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 ab8818b40c..fcd1d869f9 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 ac52206192..8ce85213be 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) */ }; From 715e3e2ebe0546edfa8c053ff90f4f1fdc521da7 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 22 Apr 2013 08:51:49 +0200 Subject: [PATCH 10/13] 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 269ee45591..54213c0754 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 276f4bf593..88e0fbb13f 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 fcd1d869f9..8b6f184737 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) { From ad212ee628cd08c5c063757e4a5a2edc82392f3b Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 22 Apr 2013 15:00:15 +0200 Subject: [PATCH 11/13] 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 88e0fbb13f..cede0534d1 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 */ +/* I2C bus address */ +#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ + +/* Register address */ +#define READ_CMD 0x07 /* Read the data */ -/* 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 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); } From 24630f15b6d0b465bd62f1105def4e96ffc92e10 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 22 Apr 2013 21:30:20 +0200 Subject: [PATCH 12/13] 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 54213c0754..bffc35c62c 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 9089d97af2..f6639b470c 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 cede0534d1..5bff4c7203 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 e519830710..dc36f6c934 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; From 5b60991c63b0c6b87632369fde73236263670448 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 23 Apr 2013 08:49:33 +0200 Subject: [PATCH 13/13] 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 5bff4c7203..e50395e479 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;