From 801697c5402dad7b2949105e7a4ee02a21f7846a Mon Sep 17 00:00:00 2001 From: pixhawk Date: Mon, 20 Aug 2012 16:08:27 +0200 Subject: [PATCH 01/10] fixed sensors error printing --- apps/sensors/sensors.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index 6a06a786f1..ef8ff798f1 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -514,6 +514,7 @@ int sensors_thread_main(int argc, char *argv[]) mag_offset[1] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_YOFFSET]; mag_offset[2] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_ZOFFSET]; + paramcounter = 0; } @@ -565,7 +566,7 @@ int sensors_thread_main(int argc, char *argv[]) if (ret_accelerometer != sizeof(buf_accelerometer)) { acc_fail_count++; - if (acc_fail_count & 0b1000 || (acc_fail_count > 20 && acc_fail_count < 100)) { + if (acc_fail_count & 0b111 || (acc_fail_count > 20 && acc_fail_count < 100)) { fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } @@ -618,7 +619,7 @@ int sensors_thread_main(int argc, char *argv[]) if (ret_magnetometer != sizeof(buf_magnetometer)) { mag_fail_count++; - if (mag_fail_count & 0b1000 || (mag_fail_count > 20 && mag_fail_count < 100)) { + if (mag_fail_count & 0b111 || (mag_fail_count > 20 && mag_fail_count < 100)) { fprintf(stderr, "[sensors] HMC5883L ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } From 821306bc4dccf9bd58112680f499602d384db917 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Aug 2012 17:48:31 +0200 Subject: [PATCH 02/10] Allow to disable USB interface (but leave it enabled as default), give uORB more stack space --- ROMFS/scripts/rcS | 13 +++++++++---- apps/uORB/Makefile | 2 +- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS index 33f63ca873..7b8d513386 100755 --- a/ROMFS/scripts/rcS +++ b/ROMFS/scripts/rcS @@ -18,7 +18,7 @@ # can change this to prevent automatic startup of the flight script. # set MODE autostart -set USB no +set USB autoconnect # # Try to mount the microSD card. @@ -46,11 +46,16 @@ fi # # Check for USB host # -if sercon +if [ $USB != autoconnect ] then - echo "[init] USB interface connected" + echo "[init] not connecting USB" else - echo "[init] No USB connected" + if sercon + then + echo "[init] USB interface connected" + else + echo "[init] No USB connected" + fi fi # diff --git a/apps/uORB/Makefile b/apps/uORB/Makefile index 0766a66eb0..64ba52e9c5 100644 --- a/apps/uORB/Makefile +++ b/apps/uORB/Makefile @@ -37,6 +37,6 @@ APPNAME = uorb PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 2048 +STACKSIZE = 4096 include $(APPDIR)/mk/app.mk From 3246568c82b8882e6b54f15d56523fc35f606089 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 20 Aug 2012 17:59:12 -0700 Subject: [PATCH 03/10] sscanf field widths seem to be broken, don't use them. --- apps/systemlib/mixer/mixer_group.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index bc944ccd9b..44a33bf8b2 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -217,7 +217,7 @@ mixer_load_multirotor(Mixer::ControlCallback control_cb, uintptr_t cb_handle, co char geomname[8]; int s[4]; - if (sscanf(buf, "R: %7s %d %d %d %d", geomname, &s[0], &s[1], &s[2], &s[3]) != 5) { + if (sscanf(buf, "R: %s %d %d %d %d", geomname, &s[0], &s[1], &s[2], &s[3]) != 5) { debug("multirotor parse failed on '%s'", buf); return nullptr; } From 228fbb975aa2d97395dd9b34937b1348ea6b3230 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 21 Aug 2012 01:09:03 -0700 Subject: [PATCH 04/10] New-style driver for the HMC5883 on I2C. --- apps/drivers/drv_mag.h | 2 +- apps/drivers/hmc5883/Makefile | 42 ++ apps/drivers/hmc5883/hmc5883.cpp | 868 +++++++++++++++++++++++++++++ nuttx/configs/px4fmu/nsh/appconfig | 1 + 4 files changed, 912 insertions(+), 1 deletion(-) create mode 100644 apps/drivers/hmc5883/Makefile create mode 100644 apps/drivers/hmc5883/hmc5883.cpp diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index 673a3988fd..fe1addaebe 100644 --- a/apps/drivers/drv_mag.h +++ b/apps/drivers/drv_mag.h @@ -76,7 +76,7 @@ ORB_DECLARE(sensor_mag); */ #define _MAGIOCBASE (0x2300) -#define _MAGIOC(_n) (_IOC(_MAGIOBASE, _n)) +#define _MAGIOC(_n) (_IOC(_MAGIOCBASE, _n)) /** set the driver polling rate to (arg) Hz, or one of the MAG_POLLRATE constants */ #define MAGIOCSPOLLRATE _MAGIOC(0) diff --git a/apps/drivers/hmc5883/Makefile b/apps/drivers/hmc5883/Makefile new file mode 100644 index 0000000000..aa4a397fb3 --- /dev/null +++ b/apps/drivers/hmc5883/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# HMC5883 driver +# + +APPNAME = hmc5883 +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp new file mode 100644 index 0000000000..64a0a932e7 --- /dev/null +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -0,0 +1,868 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 hmc5883.cpp + * + * Driver for the HMC5883 magnetometer connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include + +#include + + +/* + * HMC5883 internal constants and data structures. + */ + +/* Max measurement rate is 160Hz */ +#define HMC5883_CONVERSION_INTERVAL (1000000 / 160) /* microseconds */ + +#define ADDR_CONF_A 0x00 +#define ADDR_CONF_B 0x01 +#define ADDR_MODE 0x02 +#define ADDR_DATA_OUT_X_MSB 0x03 +#define ADDR_DATA_OUT_X_LSB 0x04 +#define ADDR_DATA_OUT_Z_MSB 0x05 +#define ADDR_DATA_OUT_Z_LSB 0x06 +#define ADDR_DATA_OUT_Y_MSB 0x07 +#define ADDR_DATA_OUT_Y_LSB 0x08 +#define ADDR_STATUS 0x09 +#define ADDR_ID_A 0x0a +#define ADDR_ID_B 0x0b +#define ADDR_ID_C 0x0c + +#define HMC5883L_ADDRESS 0x1E + +/* modes not changeable outside of driver */ +#define HMC5883L_MODE_NORMAL (0 << 0) /* default */ +#define HMC5883L_MODE_POSITIVE_BIAS (1 << 0) /* positive bias */ +#define HMC5883L_MODE_NEGATIVE_BIAS (1 << 1) /* negative bias */ + +#define HMC5883L_AVERAGING_1 (0 << 5) /* conf a register */ +#define HMC5883L_AVERAGING_2 (1 << 5) +#define HMC5883L_AVERAGING_4 (2 << 5) +#define HMC5883L_AVERAGING_8 (3 << 5) + +#define MODE_REG_CONTINOUS_MODE (0 << 0) +#define MODE_REG_SINGLE_MODE (1 << 0) /* default */ + +#define STATUS_REG_DATA_OUT_LOCK (1 << 1) /* page 16: set if data is only partially read, read device to reset */ +#define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */ + +#define ID_A_WHO_AM_I 'H' +#define ID_B_WHO_AM_I '4' +#define ID_C_WHO_AM_I '3' + + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +class HMC5883 : public device::I2C +{ +public: + HMC5883(int bus); + ~HMC5883(); + + 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); + + virtual int open_first(struct file *filp); + virtual int close_last(struct file *filp); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + struct work_s _work; + unsigned _measure_ticks; + + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + struct mag_report *_reports; + + bool _collect_phase; + + int _mag_topic; + + unsigned _reads; + unsigned _measure_errors; + unsigned _read_errors; + unsigned _buf_overflows; + + perf_counter_t _sample_perf; + + /** + * 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. + * + * This is the heart of the measurement state machine. This function + * alternately starts a measurement, or collects the data from the + * previous measurement. + * + * When the interval between measurements is greater than the minimum + * measurement interval, a gap is inserted between collection + * and measurement to provide the most recent measurement possible + * at the next interval. + */ + void cycle(); + + /** + * 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); + + /** + * Write a register. + * + * @param reg The register to write. + * @param val The value to write. + * @return OK on write success. + */ + int write_reg(uint8_t reg, uint8_t val); + + /** + * Read a register. + * + * @param reg The register to read. + * @param val The value read. + * @return OK on read success. + */ + int read_reg(uint8_t reg, uint8_t &val); + + /** + * Issue a measurement command. + * + * @return OK if the measurement command was successful. + */ + int measure(); + + /** + * Collect the result of the most recent measurement. + */ + int collect(); + + /** + * Convert a big-endian signed 16-bit value to a float. + * + * @param in A signed 16-bit big-endian value. + * @return The floating-point representation of the value. + */ + float meas_to_float(uint8_t in[2]); + +}; + +/* 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 hmc5883_main(int argc, char *argv[]); + + +HMC5883::HMC5883(int bus) : + I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), + _measure_ticks(0), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _reads(0), + _measure_errors(0), + _read_errors(0), + _buf_overflows(0), + _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")) +{ + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + _work.worker = nullptr; +} + +HMC5883::~HMC5883() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +int +HMC5883::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + ret = I2C::init(); + if (ret != OK) + goto out; + + /* assuming we're good, advertise the object */ + struct mag_report m; + + /* if this fails (e.g. no object in the system) that's OK */ + memset(&m, 0, sizeof(m)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &m); + + if (_mag_topic < 0) { + debug("failed to create sensor_baro object"); + } else { + ret = 0; + } +out: + return ret; +} + +int +HMC5883::open_first(struct file *filp) +{ + /* reset to manual-poll mode */ + _measure_ticks = 0; + + /* allocate basic report buffers */ + _num_reports = 2; + _reports = new struct mag_report[_num_reports]; + _oldest_report = _next_report = 0; + + return OK; +} + +int +HMC5883::close_last(struct file *filp) +{ + /* stop measurement */ + stop(); + + /* free report buffers */ + if (_reports != nullptr) { + delete[] _reports; + _num_reports = 0; + } + + _measure_ticks = 0; + + return OK; +} + +int +HMC5883::probe() +{ + uint8_t cmd[] = { ADDR_STATUS }; + uint8_t data[3]; + + if (read_reg(ADDR_ID_A, data[0]) || + read_reg(ADDR_ID_B, data[1]) || + read_reg(ADDR_ID_C, data[2])) + debug("read_reg fail"); + + if ((data[0] != ID_A_WHO_AM_I) || + (data[1] != ID_B_WHO_AM_I) || + (data[2] != ID_C_WHO_AM_I)) { + debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); + return -EIO; + } + + return OK; +} + +ssize_t +HMC5883::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct mag_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); + } + } + + _reads++; + + /* 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(HMC5883_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); + _reads++; + + } while (0); + + return ret; +} + +int +HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case MAGIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case MAG_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling not supported */ + case MAG_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* 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(HMC5883_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 MAGIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct mag_report *buf = new struct mag_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 MAGIOCSREPORTFORMAT: + return -EINVAL; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +void +HMC5883::start() +{ + /* make sure we are stopped first */ + stop(); + + /* reset the report ring and state machine */ + _collect_phase = false; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(&_work, (worker_t)&HMC5883::cycle_trampoline, this, 1); +} + +void +HMC5883::stop() +{ + work_cancel(&_work); +} + +void +HMC5883::cycle_trampoline(void *arg) +{ + HMC5883 *dev = (HMC5883 *)arg; + + dev->cycle(); +} + +void +HMC5883::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("FATAL collection error - restarting\n"); + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(HMC5883_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(&_work, + (worker_t)&HMC5883::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(HMC5883_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) { + log("FATAL measure error - restarting\n"); + start(); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(&_work, + (worker_t)&HMC5883::cycle_trampoline, + this, + USEC2TICK(HMC5883_CONVERSION_INTERVAL)); +} + +int +HMC5883::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE); + if (OK != ret) + _measure_errors++; + + return ret; +} + +int +HMC5883::collect() +{ +#pragma pack(push, 1) + struct { /* status register and data as read back from the device */ + uint8_t x[2]; + uint8_t z[2]; + uint8_t y[2]; + uint8_t status; + } hmc_report; +#pragma pack(pop) + int ret = -EIO; + uint8_t cmd[1]; + + perf_begin(_sample_perf); + + /* 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(); + + /* get measurements from the device */ + cmd[0] = ADDR_DATA_OUT_X_MSB; + ret = transfer(&cmd[0], 1, &hmc_report.x[0], sizeof(hmc_report)); + if (ret != OK) { + debug("data/status read error"); + goto out; + } + + /* XXX check status? */ + + /* XXX scaling */ + _reports[_next_report].x = meas_to_float(hmc_report.x); + _reports[_next_report].y = meas_to_float(hmc_report.y); + _reports[_next_report].z = meas_to_float(hmc_report.z); + + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag_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) { + _buf_overflows++; + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + +out: + perf_end(_sample_perf); + return ret; +} + +int +HMC5883::write_reg(uint8_t reg, uint8_t val) +{ + uint8_t cmd[] = { reg, val }; + + return transfer(&cmd[0], 2, nullptr, 0); +} + +int +HMC5883::read_reg(uint8_t reg, uint8_t &val) +{ + return transfer(®, 1, &val, 1); +} + +float +HMC5883::meas_to_float(uint8_t in[2]) +{ + union { + uint8_t b[2]; + int16_t w; + } u; + + u.b[0] = in[1]; + u.b[1] = in[0]; + + return (float) u.w; +} + +void +HMC5883::print_info() +{ + printf("reads: %u\n", _reads); + printf("measure errors: %u\n", _measure_errors); + printf("read errors: %u\n", _read_errors); + printf("read overflows: %u\n", _buf_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 +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +HMC5883 *g_dev; + +/* + * XXX this should just be part of the generic sensors test... + */ + + +int +test_fail(const char *fmt, ...) +{ + va_list ap; + + fprintf(stderr, "FAIL: "); + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + fprintf(stderr, "\n"); + fflush(stderr); + return ERROR; +} + +int +test_note(const char *fmt, ...) +{ + va_list ap; + + fprintf(stderr, "note: "); + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + fprintf(stderr, "\n"); + fflush(stderr); + return OK; +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + * + * @param fd An open file descriptor on the driver. + */ +int +test(int fd) +{ + struct mag_report report; + ssize_t sz; + int ret; + + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + return test_fail("immediate read failed: %d", errno); + + test_note("single read"); + test_note("measurement: %.6f %.6f %.6f", report.x, report.y, report.z); + test_note("time: %lld", report.timestamp); + usleep(1000000); + + /* set the queue depth to 10 */ + if (OK != ioctl(fd, MAGIOCSQUEUEDEPTH, 10)) + return test_fail("failed to set queue depth"); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, MAGIOCSPOLLRATE, 2)) + return test_fail("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) + return test_fail("timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + return test_fail("periodic read failed: %d", errno); + + test_note("periodic read %u", i); + test_note("measurement: %.6f %.6f %.6f", report.x, report.y, report.z); + test_note("time: %lld", report.timestamp); + } + + return test_note("PASS"); + return OK; +} + +int +info() +{ + if (g_dev == nullptr) { + fprintf(stderr, "HMC5883: driver not running\n"); + return -ENOENT; + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + return OK; +} + + +} // namespace + +int +hmc5883_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + * + * XXX it would be nice to have a wrapper for this... + */ + if (!strcmp(argv[1], "start")) { + + if (g_dev != nullptr) { + fprintf(stderr, "HMC5883: already loaded\n"); + return -EBUSY; + } + + /* create the driver */ + /* XXX HORRIBLE hack - the bus number should not come from here */ + g_dev = new HMC5883(2); + + if (g_dev == nullptr) { + fprintf(stderr, "HMC5883: driver alloc failed\n"); + return -ENOMEM; + } + + if (OK != g_dev->init()) { + fprintf(stderr, "HMC5883: driver init failed\n"); + usleep(100000); + delete g_dev; + g_dev = nullptr; + return -EIO; + } + + return OK; + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + int fd, ret; + + fd = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + return test_fail("driver open failed: %d", errno); + + ret = test(fd); + close(fd); + return ret; + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info")) + return info(); + + fprintf(stderr, "unrecognised command, try 'start', 'test' or 'info'\n"); + return -EINVAL; +} diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 5032437964..3323d19986 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -85,6 +85,7 @@ CONFIGURED_APPS += attitude_estimator_ekf # Communication and Drivers CONFIGURED_APPS += drivers/device CONFIGURED_APPS += drivers/ms5611 +CONFIGURED_APPS += drivers/hmc5883 CONFIGURED_APPS += drivers/mpu6000 CONFIGURED_APPS += px4/px4io/driver CONFIGURED_APPS += px4/fmu From f3c1a7475d0526501a340b99aee5c5d4d91300a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 21 Aug 2012 17:42:24 +0200 Subject: [PATCH 05/10] Improved tuning for current attitude estimation hack, needs to be removed ASAP --- .../attitude_estimator_bm.c | 23 ++++++++++++++----- 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index feaed66954..e56ff1c3a8 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -87,14 +87,25 @@ int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *eul gyro_values.z = raw->gyro_rad_s[2]; float_vect3 accel_values; - accel_values.x = raw->accelerometer_m_s2[0] * 9.81f * 9.0f; - accel_values.y = raw->accelerometer_m_s2[1] * 9.81f * 9.0f; - accel_values.z = raw->accelerometer_m_s2[2] * 9.81f * 9.0f; + accel_values.x = (raw->accelerometer_m_s2[0] / 9.81f) * 100; + accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 100; + accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 100; float_vect3 mag_values; - mag_values.x = raw->magnetometer_ga[0]*510.0f; - mag_values.y = raw->magnetometer_ga[1]*510.0f; - mag_values.z = raw->magnetometer_ga[2]*510.0f; + mag_values.x = raw->magnetometer_ga[0]*456.0f; + mag_values.y = raw->magnetometer_ga[1]*456.0f; + mag_values.z = raw->magnetometer_ga[2]*456.0f; + + static int i = 0; + + if (i == 500) { + printf("gyro: %8.4f\t%8.4f\t%8.4f\t accel: %8.4f\t%8.4f\t%8.4f\t mag: %8.4f\t%8.4f\t%8.4f\t\n", + gyro_values.x, gyro_values.y, gyro_values.z, + accel_values.x, accel_values.y, accel_values.z, + mag_values.x, mag_values.y, mag_values.z); + i = 0; + } + i++; attitude_blackmagic(&accel_values, &mag_values, &gyro_values); From a0ae2cb175b8f83ee7731335d452cc5ca30e820b Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 21 Aug 2012 20:14:43 -0700 Subject: [PATCH 06/10] Add a set of ORB topics for advertising actuator outputs. This is part of \#7 --- apps/px4/fmu/fmu.cpp | 52 +++++++++++----------- apps/uORB/objects_common.cpp | 6 +++ apps/uORB/topics/actuator_outputs.h | 68 +++++++++++++++++++++++++++++ 3 files changed, 101 insertions(+), 25 deletions(-) create mode 100644 apps/uORB/topics/actuator_outputs.h diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 9d88472462..83bc625099 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -64,6 +64,7 @@ #include #include +#include class FMUServo : public device::CDev @@ -88,6 +89,7 @@ private: int _task; int _t_actuators; int _t_armed; + int _t_outputs; unsigned _num_outputs; volatile bool _task_should_exit; @@ -98,15 +100,12 @@ private: actuator_controls_s _controls; static void task_main_trampoline(int argc, char *argv[]); - void task_main(); + void task_main() __attribute__((noreturn)); - static int control_callback_trampoline(uintptr_t handle, + static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); - int control_callback(uint8_t control_group, - uint8_t control_index, - float &input); }; namespace @@ -212,6 +211,11 @@ FMUServo::task_main() _t_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_armed, 100); /* 10Hz update rate */ + /* advertise the mixed control outputs */ + struct actuator_output_s outputs; + memset(&outputs, 0, sizeof(outputs)); + _t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs); + struct pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; @@ -237,7 +241,6 @@ FMUServo::task_main() /* do we have a control update? */ if (fds[0].revents & POLLIN) { - float outputs[num_outputs]; /* get controls - must always do this to avoid spinning */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); @@ -246,14 +249,20 @@ FMUServo::task_main() if (_mixers != nullptr) { /* do mixing */ - _mixers->mix(&outputs[0], num_outputs); + _mixers->mix(&outputs.output[0], num_outputs); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { /* scale for PWM output 900 - 2100us */ - up_pwm_servo_set(i, 1500 + (600 * outputs[i])); + outputs.output[i] = 1500 + (600 * outputs.output[i]); + + /* output to the servo */ + up_pwm_servo_set(i, outputs.output[i]); } + + /* and publish for anyone that cares to see */ + orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs); } } @@ -264,13 +273,14 @@ FMUServo::task_main() /* get new value */ orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); - /* update PMW servo armed status */ + /* update PWM servo armed status */ up_pwm_servo_arm(aa.armed); } } ::close(_t_actuators); ::close(_t_armed); + ::close(_t_outputs); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -285,24 +295,14 @@ FMUServo::task_main() } int -FMUServo::control_callback_trampoline(uintptr_t handle, +FMUServo::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input) { - return ((FMUServo *)handle)->control_callback(control_group, control_index, input); -} + const actuator_controls_s *controls = (actuator_controls_s *)handle; -int -FMUServo::control_callback(uint8_t control_group, - uint8_t control_index, - float &input) -{ - /* XXX currently only supporting group zero */ - if ((control_group != 0) || (control_index > NUM_ACTUATOR_CONTROLS)) - return -1; - - input = _controls.control[control_index]; + input = controls->control[control_index]; return 0; } @@ -377,7 +377,8 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) case MIXERIOCADDSIMPLE: { mixer_simple_s *mixinfo = (mixer_simple_s *)arg; - SimpleMixer *mixer = new SimpleMixer(control_callback_trampoline, (uintptr_t)this, mixinfo); + SimpleMixer *mixer = new SimpleMixer(control_callback, + (uintptr_t)&_controls, mixinfo); if (mixer->check()) { delete mixer; @@ -385,7 +386,8 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) } else { if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this); + _mixers = new MixerGroup(control_callback, + (uintptr_t)&_controls); _mixers->add_mixer(mixer); } @@ -406,7 +408,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) _mixers = nullptr; } - _mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this); + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); if (_mixers->load_from_file(path) != 0) { delete _mixers; diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index d6b566b6a8..6ff66fceb7 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -114,3 +114,9 @@ ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); ORB_DEFINE(actuator_armed, struct actuator_armed_s); + +#include "topics/actuator_outputs.h" +ORB_DEFINE(actuator_outputs_0, struct actuator_output_s); +ORB_DEFINE(actuator_outputs_1, struct actuator_output_s); +ORB_DEFINE(actuator_outputs_2, struct actuator_output_s); +ORB_DEFINE(actuator_outputs_3, struct actuator_output_s); diff --git a/apps/uORB/topics/actuator_outputs.h b/apps/uORB/topics/actuator_outputs.h new file mode 100644 index 0000000000..63441a0591 --- /dev/null +++ b/apps/uORB/topics/actuator_outputs.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 actuator_outputs.h + * + * Actuator output values. + * + * Values published to these topics are the outputs of the control mixing + * system as sent to the actuators (servos, motors, etc.) that operate + * the vehicle. + * + * Each topic can be published by a single output driver. + */ + +#ifndef TOPIC_ACTUATOR_OUTPUTS_H +#define TOPIC_ACTUATOR_OUTPUTS_H + +#include +#include "../uORB.h" + +#define NUM_ACTUATOR_OUTPUTS 16 +#define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ + +struct actuator_output_s { + float output[NUM_ACTUATOR_OUTPUTS]; +}; + +/* actuator output sets; this list can be expanded as more drivers emerge */ +ORB_DECLARE(actuator_outputs_0); +ORB_DECLARE(actuator_outputs_1); +ORB_DECLARE(actuator_outputs_2); +ORB_DECLARE(actuator_outputs_3); + +/* output sets with pre-defined applications */ +#define ORB_ID_VEHICLE_CONTROLS ORB_ID(actuator_outputs_0) + +#endif \ No newline at end of file From 63831fa908f9deb848c20433c0235554b261bec3 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 21 Aug 2012 22:04:38 -0700 Subject: [PATCH 07/10] Fix an issue where the baro ORB topic was advertised from the wrong context and thus could not be published to. --- apps/drivers/ms5611/ms5611.cpp | 32 ++++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 6c6951d9b5..187de178b5 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -246,6 +246,7 @@ MS5611::MS5611(int bus) : _measure_phase(0), _dT(0), _temp64(0), + _baro_topic(-1), _reads(0), _measure_errors(0), _read_errors(0), @@ -277,18 +278,6 @@ MS5611::init() /* do I2C init (and probe) first */ ret = I2C::init(); - /* assuming we're good, advertise the object */ - if (ret == OK) { - struct baro_report b; - - /* if this fails (e.g. no object in the system) that's OK */ - memset(&b, 0, sizeof(b)); - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &b); - - if (_baro_topic < 0) - debug("failed to create sensor_baro object"); - } - return ret; } @@ -538,6 +527,25 @@ MS5611::cycle_trampoline(void *arg) void MS5611::cycle() { + /* + * We have to publish the baro topic in the context of the workq + * in order to ensure that the descriptor is valid when we go to publish. + * + * @bug We can't really ever be torn down and restarted, since this + * descriptor will never be closed and on the restart we will be + * unable to re-advertise. + */ + if (_baro_topic == -1) { + struct baro_report b; + + /* if this fails (e.g. no object in the system) we will cope */ + memset(&b, 0, sizeof(b)); + _baro_topic = orb_advertise(ORB_ID(sensor_baro), &b); + + if (_baro_topic < 0) + debug("failed to create sensor_baro object"); + } + /* collection phase? */ if (_collect_phase) { From 5f77561ed4fb6b8347a7c848f0d02d7aeaa21c3f Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 21 Aug 2012 22:05:42 -0700 Subject: [PATCH 08/10] Fix output scaling for the hmc5883 driver. Add data checking, and fix an issue where the ORB topic could not be published due to being advertised in the wrong context. --- apps/drivers/drv_mag.h | 8 +++ apps/drivers/hmc5883/hmc5883.cpp | 116 +++++++++++++++++++++++-------- 2 files changed, 96 insertions(+), 28 deletions(-) diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index fe1addaebe..a592917787 100644 --- a/apps/drivers/drv_mag.h +++ b/apps/drivers/drv_mag.h @@ -48,6 +48,8 @@ /** * mag report structure. Reads from the device must be in multiples of this * structure. + * + * Output values are in gauss. */ struct mag_report { float x; @@ -99,4 +101,10 @@ ORB_DECLARE(sensor_mag); /** set the mag scaling constants to the structure pointed to by (arg) */ #define MAGIOCSSCALE _MAGIOC(5) +/** copy the mag scaling constants to the structure pointed to by (arg) */ +#define MAGIOCGSCALE _MAGIOC(6) + +/** perform self-calibration, update scale factors to canonical units */ +#define MAGIOCCALIBRATE _MAGIOC(7) + #endif /* _DRV_MAG_H */ diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 64a0a932e7..6ef0716cd9 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -43,6 +43,7 @@ #include #include +#include #include #include #include @@ -63,7 +64,6 @@ #include - /* * HMC5883 internal constants and data structures. */ @@ -137,14 +137,14 @@ protected: virtual int probe(); private: - struct work_s _work; + work_s _work; unsigned _measure_ticks; unsigned _num_reports; volatile unsigned _next_report; volatile unsigned _oldest_report; - struct mag_report *_reports; - + mag_report *_reports; + mag_scale _scale; bool _collect_phase; int _mag_topic; @@ -257,6 +257,7 @@ HMC5883::HMC5883(int bus) : _next_report(0), _oldest_report(0), _reports(nullptr), + _mag_topic(-1), _reads(0), _measure_errors(0), _read_errors(0), @@ -266,6 +267,14 @@ HMC5883::HMC5883(int bus) : // enable debug() calls _debug_enabled = true; + // default scaling + _scale.x_offset = 0; + _scale.x_scale = 1.0f / 1090.0f; /* default range scale from counts to gauss */ + _scale.y_offset = 0; + _scale.y_scale = 1.0f / 1090.0f; /* default range scale from counts to gauss */ + _scale.z_offset = 0; + _scale.z_scale = 1.0f / 1090.0f; /* default range scale from counts to gauss */ + // work_cancel in the dtor will explode if we don't do this... _work.worker = nullptr; } @@ -290,18 +299,6 @@ HMC5883::init() if (ret != OK) goto out; - /* assuming we're good, advertise the object */ - struct mag_report m; - - /* if this fails (e.g. no object in the system) that's OK */ - memset(&m, 0, sizeof(m)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &m); - - if (_mag_topic < 0) { - debug("failed to create sensor_baro object"); - } else { - ret = 0; - } out: return ret; } @@ -340,7 +337,6 @@ HMC5883::close_last(struct file *filp) int HMC5883::probe() { - uint8_t cmd[] = { ADDR_STATUS }; uint8_t data[3]; if (read_reg(ADDR_ID_A, data[0]) || @@ -434,7 +430,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) _measure_ticks = 0; return OK; - /* external signalling not supported */ + /* external signalling (DRDY) not supported */ case MAG_POLLRATE_EXTERNAL: /* zero would be bad */ @@ -486,7 +482,30 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; } + case MAGIOCSSCALE: + /* set new scale factors */ + memcpy(&_scale, (mag_scale *)arg, sizeof(_scale)); + return 0; + + case MAGIOCGSCALE: + /* copy out scale factors */ + memcpy((mag_scale *)arg, &_scale, sizeof(_scale)); + return 0; + + case MAGIOCCALIBRATE: + /* XXX perform auto-calibration */ + return -EINVAL; + + case MAGIOCSSAMPLERATE: + /* not supported, always 1 sample per poll */ + return -EINVAL; + + case MAGIOCSLOWPASS: + /* not supported, no internal filtering */ + return -EINVAL; + case MAGIOCSREPORTFORMAT: + /* not supported, no custom report format */ return -EINVAL; default: @@ -526,6 +545,25 @@ HMC5883::cycle_trampoline(void *arg) void HMC5883::cycle() { + /* + * We have to publish the mag topic in the context of the workq + * in order to ensure that the descriptor is valid when we go to publish. + * + * @bug We can't really ever be torn down and restarted, since this + * descriptor will never be closed and on the restart we will be + * unable to re-advertise. + */ + if (_mag_topic == -1) { + struct mag_report m; + + /* if this fails (e.g. no object in the system) we will cope */ + memset(&m, 0, sizeof(m)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &m); + + if (_mag_topic < 0) + debug("failed to create sensor_mag object"); + } + /* collection phase? */ if (_collect_phase) { @@ -591,33 +629,55 @@ HMC5883::collect() #pragma pack(push, 1) struct { /* status register and data as read back from the device */ uint8_t x[2]; - uint8_t z[2]; uint8_t y[2]; - uint8_t status; + uint8_t z[2]; } hmc_report; #pragma pack(pop) + struct { + int16_t x, y, z; + } report; int ret = -EIO; - uint8_t cmd[1]; + uint8_t cmd; + perf_begin(_sample_perf); /* 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(); + /* + * @note We could read the status register here, which could tell us that + * we were too early and that the output registers are still being + * written. In the common case that would just slow us down, and + * we're better off just never being early. + */ + /* get measurements from the device */ - cmd[0] = ADDR_DATA_OUT_X_MSB; - ret = transfer(&cmd[0], 1, &hmc_report.x[0], sizeof(hmc_report)); + cmd = ADDR_DATA_OUT_X_MSB; + ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report)); if (ret != OK) { debug("data/status read error"); goto out; } - /* XXX check status? */ + /* swap the data we just received */ + report.x = ((int16_t)hmc_report.x[0] << 8) + hmc_report.x[1]; + report.y = ((int16_t)hmc_report.y[0] << 8) + hmc_report.y[1]; + report.z = ((int16_t)hmc_report.z[0] << 8) + hmc_report.z[1]; - /* XXX scaling */ - _reports[_next_report].x = meas_to_float(hmc_report.x); - _reports[_next_report].y = meas_to_float(hmc_report.y); - _reports[_next_report].z = meas_to_float(hmc_report.z); + /* + * If any of the values are -4096, there was an internal math error in the sensor. + * Generalise this to a simple range check that will also catch some bit errors. + */ + if ((abs(report.x) > 2048) || + (abs(report.y) > 2048) || + (abs(report.z) > 2048)) + goto out; + + /* scale values for output */ + _reports[_next_report].x = report.x * _scale.x_scale + _scale.x_offset; + _reports[_next_report].y = report.y * _scale.y_scale + _scale.y_offset; + _reports[_next_report].z = report.z * _scale.z_scale + _scale.z_offset; /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]); From 8c22e2a092a5527e5e4af2a54ecdeac586ffbfa7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 21 Aug 2012 22:30:04 -0700 Subject: [PATCH 09/10] Whitespace --- apps/drivers/hmc5883/hmc5883.cpp | 15 +++++++++------ apps/drivers/ms5611/ms5611.cpp | 4 ++-- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 6ef0716cd9..a98b30efc9 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -296,6 +296,7 @@ HMC5883::init() /* do I2C init (and probe) first */ ret = I2C::init(); + if (ret != OK) goto out; @@ -344,7 +345,7 @@ HMC5883::probe() read_reg(ADDR_ID_C, data[2])) debug("read_reg fail"); - if ((data[0] != ID_A_WHO_AM_I) || + if ((data[0] != ID_A_WHO_AM_I) || (data[1] != ID_B_WHO_AM_I) || (data[2] != ID_C_WHO_AM_I)) { debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); @@ -545,8 +546,8 @@ HMC5883::cycle_trampoline(void *arg) void HMC5883::cycle() { - /* - * We have to publish the mag topic in the context of the workq + /* + * We have to publish the mag topic in the context of the workq * in order to ensure that the descriptor is valid when we go to publish. * * @bug We can't really ever be torn down and restarted, since this @@ -617,6 +618,7 @@ HMC5883::measure() * Send the command to begin a measurement. */ ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE); + if (OK != ret) _measure_errors++; @@ -648,13 +650,14 @@ HMC5883::collect() /* * @note We could read the status register here, which could tell us that * we were too early and that the output registers are still being - * written. In the common case that would just slow us down, and + * written. In the common case that would just slow us down, and * we're better off just never being early. */ /* get measurements from the device */ cmd = ADDR_DATA_OUT_X_MSB; ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report)); + if (ret != OK) { debug("data/status read error"); goto out; @@ -665,8 +668,8 @@ HMC5883::collect() report.y = ((int16_t)hmc_report.y[0] << 8) + hmc_report.y[1]; report.z = ((int16_t)hmc_report.z[0] << 8) + hmc_report.z[1]; - /* - * If any of the values are -4096, there was an internal math error in the sensor. + /* + * If any of the values are -4096, there was an internal math error in the sensor. * Generalise this to a simple range check that will also catch some bit errors. */ if ((abs(report.x) > 2048) || diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 187de178b5..ee366f6688 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -527,8 +527,8 @@ MS5611::cycle_trampoline(void *arg) void MS5611::cycle() { - /* - * We have to publish the baro topic in the context of the workq + /* + * We have to publish the baro topic in the context of the workq * in order to ensure that the descriptor is valid when we go to publish. * * @bug We can't really ever be torn down and restarted, since this From 88f0080a0ffb299006950c0453eabddb7d17f078 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 21 Aug 2012 23:44:22 -0700 Subject: [PATCH 10/10] Fix an architectural issue with the ORB that prevented publication from interrupt context. ORB topic advertisements are now global handles that can be used in any context. It is still possible to open a topic node as a publisher, but it's not the default. As a consequence, the type of the handle returned from orb_advertise has changed; all other API remains the same. --- .../attitude_estimator_ekf_main.c | 2 +- apps/commander/commander.c | 2 +- apps/drivers/drv_orb_dev.h | 3 + apps/drivers/hmc5883/hmc5883.cpp | 2 +- apps/drivers/mpu6000/mpu6000.cpp | 4 +- apps/drivers/ms5611/ms5611.cpp | 2 +- apps/examples/px4_simple_app/px4_simple_app.c | 4 +- apps/fixedwing_control/fixedwing_control.c | 2 +- apps/gps/mtk.c | 2 +- apps/gps/nmea_helper.c | 2 +- apps/gps/ubx.c | 2 +- apps/mavlink/mavlink.c | 14 +-- .../multirotor_att_control_main.c | 4 +- .../multirotor_pos_control.c | 2 +- .../position_estimator_main.c | 2 +- .../attitude_estimator_bm.c | 2 +- apps/px4/fmu/fmu.cpp | 6 +- apps/px4/px4io/driver/px4io.cpp | 2 +- apps/sensors/sensors.c | 9 +- apps/uORB/uORB.cpp | 88 ++++++++++++------- apps/uORB/uORB.h | 21 ++++- 21 files changed, 110 insertions(+), 67 deletions(-) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index fdf6c9d91d..09cbdd4e94 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -138,7 +138,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* advertise attitude */ - int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 149a662fd4..55d3b82e71 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -96,7 +96,7 @@ static int buzzer; static int mavlink_fd; static bool commander_initialized = false; static struct vehicle_status_s current_status; /**< Main state machine */ -static int stat_pub; +static orb_advert_t stat_pub; static uint16_t nofix_counter = 0; static uint16_t gotfix_counter = 0; diff --git a/apps/drivers/drv_orb_dev.h b/apps/drivers/drv_orb_dev.h index b3fc01a5fa..50288f6901 100644 --- a/apps/drivers/drv_orb_dev.h +++ b/apps/drivers/drv_orb_dev.h @@ -81,4 +81,7 @@ /** Set the minimum interval at which the topic can be seen to be updated for this subscription */ #define ORBIOCSETINTERVAL _ORBIOC(12) +/** Get the global advertiser handle for the topic */ +#define ORBIOCGADVERTISER _ORBIOC(13) + #endif /* _DRV_UORB_H */ diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index a98b30efc9..0fa1f365dc 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -147,7 +147,7 @@ private: mag_scale _scale; bool _collect_phase; - int _mag_topic; + orb_advert_t _mag_topic; unsigned _reads; unsigned _measure_errors; diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 7576fc1d96..951e1a0621 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -181,12 +181,12 @@ private: struct accel_report _accel_report; struct accel_scale _accel_scale; float _accel_range_scale; - int _accel_topic; + orb_advert_t _accel_topic; struct gyro_report _gyro_report; struct gyro_scale _gyro_scale; float _gyro_range_scale; - int _gyro_topic; + orb_advert_t _gyro_topic; unsigned _reads; perf_counter_t _sample_perf; diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index ee366f6688..8f8f96217f 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -125,7 +125,7 @@ private: int32_t _dT; int64_t _temp64; - int _baro_topic; + orb_advert_t _baro_topic; unsigned _reads; unsigned _measure_errors; diff --git a/apps/examples/px4_simple_app/px4_simple_app.c b/apps/examples/px4_simple_app/px4_simple_app.c index fd34a5dacb..7f655964d9 100644 --- a/apps/examples/px4_simple_app/px4_simple_app.c +++ b/apps/examples/px4_simple_app/px4_simple_app.c @@ -59,7 +59,7 @@ int px4_simple_app_main(int argc, char *argv[]) /* advertise attitude topic */ struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - int att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &att); + orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); /* one could wait for multiple topics with this technique, just using one here */ struct pollfd fds[] = { @@ -103,7 +103,7 @@ int px4_simple_app_main(int argc, char *argv[]) att.roll = raw.accelerometer_m_s2[0]; att.pitch = raw.accelerometer_m_s2[1]; att.yaw = raw.accelerometer_m_s2[2]; - orb_publish(ORB_ID(vehicle_attitude), att_pub_fd, &att); + orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); } /* there could be more file descriptors here, in the form like: * if (fds[1..n].revents & POLLIN) {} diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index bad62edcfc..7ea4101abf 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -675,7 +675,7 @@ int fixedwing_control_main(int argc, char *argv[]) /* Set up to publish fixed wing control messages */ struct fixedwing_control_s control; - int fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control); + orb_advert_t fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control); /* Subscribe to global position, attitude and rc */ struct vehicle_global_position_s global_pos; diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c index cef70601b3..0333c71008 100644 --- a/apps/gps/mtk.c +++ b/apps/gps/mtk.c @@ -311,7 +311,7 @@ void *mtk_loop(void *arg) /* advertise GPS topic */ struct vehicle_gps_position_s mtk_gps_d; mtk_gps = &mtk_gps_d; - int gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), &mtk_gps); + orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), &mtk_gps); while (1) { /* Parse a message from the gps receiver */ diff --git a/apps/gps/nmea_helper.c b/apps/gps/nmea_helper.c index 4b520d4036..54912b6d37 100644 --- a/apps/gps/nmea_helper.c +++ b/apps/gps/nmea_helper.c @@ -176,7 +176,7 @@ void *nmea_loop(void *arg) /* advertise GPS topic */ struct vehicle_gps_position_s nmea_gps_d = {0}; nmea_gps = &nmea_gps_d; - int gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps); + orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps); while (1) { /* Parse a message from the gps receiver */ diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index 771dfbd6c0..a629e904d8 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -842,7 +842,7 @@ void *ubx_loop(void *arg) ubx_gps = &ubx_gps_d; - int gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps); + orb_advert_t gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps); while (1) { /* Parse a message from the gps receiver */ diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 7fba65cb73..97822510cc 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -113,7 +113,7 @@ static struct vehicle_status_s v_status; static struct rc_channels_s rc; /* HIL publishers */ -static int pub_hil_attitude = -1; +static orb_advert_t pub_hil_attitude = -1; /** HIL attitude */ static struct vehicle_attitude_s hil_attitude; @@ -126,16 +126,16 @@ static struct ardrone_motors_setpoint_s ardrone_motors; static struct vehicle_command_s vcmd; -static int pub_hil_global_pos = -1; -static int ardrone_motors_pub = -1; -static int cmd_pub = -1; +static orb_advert_t pub_hil_global_pos = -1; +static orb_advert_t ardrone_motors_pub = -1; +static orb_advert_t cmd_pub = -1; static int sensor_sub = -1; static int att_sub = -1; static int global_pos_sub = -1; static int local_pos_sub = -1; -static int flow_pub = -1; -static int global_position_setpoint_pub = -1; -static int local_position_setpoint_pub = -1; +static orb_advert_t flow_pub = -1; +static orb_advert_t global_position_setpoint_pub = -1; +static orb_advert_t local_position_setpoint_pub = -1; static bool mavlink_hil_enabled = false; static char mavlink_message_string[51] = {0}; diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index baee507b2f..df08ca75fe 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -119,9 +119,9 @@ mc_thread_main(int argc, char *argv[]) /* publish actuator controls */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) actuators.control[i] = 0.0f; - int actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); armed.armed = true; - int armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control"); diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index ff32fb4607..3f9c725177 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -89,7 +89,7 @@ mpc_thread_main(int argc, char *argv[]) int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); /* publish attitude setpoint */ - int att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); while (1) { /* get a local copy of the vehicle state */ diff --git a/apps/position_estimator/position_estimator_main.c b/apps/position_estimator/position_estimator_main.c index 773cd87ff3..17482dc0a8 100644 --- a/apps/position_estimator/position_estimator_main.c +++ b/apps/position_estimator/position_estimator_main.c @@ -297,7 +297,7 @@ int position_estimator_main(int argc, char *argv[]) .lon = lon_current * 1e7, .alt = gps.alt }; - int global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + orb_advert_t global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); printf("[multirotor position estimator] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index e56ff1c3a8..fd3ed11375 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -154,7 +154,7 @@ int attitude_estimator_bm_main(int argc, char *argv[]) bool publishing = false; /* advertise attitude */ - int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); publishing = true; struct pollfd fds[] = { diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 83bc625099..3016cb01eb 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -89,7 +89,7 @@ private: int _task; int _t_actuators; int _t_armed; - int _t_outputs; + orb_advert_t _t_outputs; unsigned _num_outputs; volatile bool _task_should_exit; @@ -566,15 +566,13 @@ fake(int argc, char *argv[]) ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; - int handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); + orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); if (handle < 0) { puts("advertise failed"); exit(1); } - close(handle); - exit(0); } diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/px4/px4io/driver/px4io.cpp index a5def874d8..c3371c138b 100644 --- a/apps/px4/px4io/driver/px4io.cpp +++ b/apps/px4/px4io/driver/px4io.cpp @@ -99,7 +99,7 @@ protected: void set_channels(unsigned count, const servo_position_t *data); private: - int _publication; + orb_advert_t _publication; struct rc_input_values _input; }; diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index ef8ff798f1..891167d1c7 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -124,8 +124,8 @@ extern unsigned ppm_decoded_channels; extern uint64_t ppm_last_valid_decode; #endif -/* file handle that will be used for subscribing */ -static int sensor_pub; +/* ORB topic publishing our results */ +static orb_advert_t sensor_pub; /** * Sensor readout and publishing. @@ -404,7 +404,7 @@ int sensors_thread_main(int argc, char *argv[]) .yaw = 0.0f, .throttle = 0.0f }; - int manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); + orb_advert_t manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); if (manual_control_pub < 0) { fprintf(stderr, "[sensors] ERROR: orb_advertise for topic manual_control_setpoint failed.\n"); @@ -413,7 +413,7 @@ int sensors_thread_main(int argc, char *argv[]) /* advertise the rc topic */ struct rc_channels_s rc; memset(&rc, 0, sizeof(rc)); - int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); + orb_advert_t rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); if (rc_pub < 0) { fprintf(stderr, "[sensors] ERROR: orb_advertise for topic rc_channels failed.\n"); @@ -466,7 +466,6 @@ int sensors_thread_main(int argc, char *argv[]) if (vstatus.flag_hil_enabled && !hil_enabled) { hil_enabled = true; publishing = false; - int ret = close(sensor_pub); printf("[sensors] Closing sensor pub: %i \n", ret); /* switching from HIL to non-HIL mode */ diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp index 1e7cdc8db1..c4434e4edd 100644 --- a/apps/uORB/uORB.cpp +++ b/apps/uORB/uORB.cpp @@ -112,6 +112,8 @@ public: virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); + protected: virtual pollevent_t poll_state(struct file *filp); virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); @@ -298,6 +300,8 @@ ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen) * * Writes outside interrupt context will allocate the object * if it has not yet been allocated. + * + * Note that filp will usually be NULL. */ if (nullptr == _data) { if (!up_interrupt_context()) { @@ -353,12 +357,42 @@ ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg) sd->update_interval = arg; return OK; + case ORBIOCGADVERTISER: + *(uintptr_t *)arg = (uintptr_t)this; + return OK; + default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); } } +ssize_t +ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) +{ + ORBDevNode *devnode = (ORBDevNode *)handle; + int ret; + + /* this is a bit risky, since we are trusting the handle in order to deref it */ + if (devnode->_meta != meta) { + errno = EINVAL; + return ERROR; + } + + /* call the devnode write method with no file pointer */ + ret = devnode->write(nullptr, (const char *)data, meta->o_size); + + if (ret < 0) + return ERROR; + + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } + + return OK; +} + pollevent_t ORBDevNode::poll_state(struct file *filp) { @@ -614,7 +648,7 @@ test() if (pfd < 0) return test_fail("advertise failed: %d", errno); - test_note("publish fd %d", pfd); + test_note("publish handle 0x%08x", pfd); sfd = orb_subscribe(ORB_ID(orb_test)); if (sfd < 0) @@ -877,29 +911,35 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve return ERROR; } - /* the advertiser must perform an initial publish to initialise the object */ - if (advertiser) { - ret = orb_publish(meta, fd, data); - - if (ret != OK) { - /* save errno across the close */ - ret = errno; - close(fd); - errno = ret; - return ERROR; - } - } - /* everything has been OK, we can return the handle now */ return fd; } } // namespace -int +orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { - return node_open(PUBSUB, meta, data, true); + int result, fd; + orb_advert_t advertiser; + + /* open the node as an advertiser */ + fd = node_open(PUBSUB, meta, data, true); + if (fd == ERROR) + return ERROR; + + /* get the advertiser handle and close the node */ + result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + close(fd); + if (result == ERROR) + return ERROR; + + /* the advertiser must perform an initial publish to initialise the object */ + result= orb_publish(meta, advertiser, data); + if (result == ERROR) + return ERROR; + + return advertiser; } int @@ -915,21 +955,9 @@ orb_unsubscribe(int handle) } int -orb_publish(const struct orb_metadata *meta, int handle, const void *data) +orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) { - int ret; - - ret = write(handle, data, meta->o_size); - - if (ret < 0) - return ERROR; - - if (ret != (int)meta->o_size) { - errno = EIO; - return ERROR; - } - - return OK; + return ORBDevNode::publish(meta, handle, data); } int diff --git a/apps/uORB/uORB.h b/apps/uORB/uORB.h index c36d0044f2..eb068d2b78 100644 --- a/apps/uORB/uORB.h +++ b/apps/uORB/uORB.h @@ -105,12 +105,27 @@ struct orb_metadata { __BEGIN_DECLS +/** + * ORB topic advertiser handle. + * + * Advertiser handles are global; once obtained they can be shared freely + * and do not need to be closed or released. + * + * This permits publication from interrupt context and other contexts where + * a file-descriptor-based handle would not otherwise be in scope for the + * publisher. + */ +typedef intptr_t orb_advert_t; + /** * Advertise as the publisher of a topic. * * This performs the initial advertisement of a topic; it creates the topic * node in /obj if required and publishes the initial data. * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. * @param data A pointer to the initial data to be published. @@ -122,7 +137,7 @@ __BEGIN_DECLS * ORB_DEFINE with no corresponding ORB_DECLARE) * this function will return -1 and set errno to ENOENT. */ -extern int orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT; +extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT; /** * Publish new data to a topic. @@ -131,13 +146,13 @@ extern int orb_advertise(const struct orb_metadata *meta, const void *data) __EX * will be notified. Subscribers that are not waiting can check the topic * for updates using orb_check and/or orb_stat. * - * @handle The handle returned from orb_advertise. * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. + * @handle The handle returned from orb_advertise. * @param data A pointer to the data to be published. * @return OK on success, ERROR otherwise with errno set accordingly. */ -extern int orb_publish(const struct orb_metadata *meta, int handle, const void *data) __EXPORT; +extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT; /** * Subscribe to a topic.