diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-22 08:56:33 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-22 08:56:33 +0200 |
commit | 72979032e9bfef200809e97663c613b7b530b011 (patch) | |
tree | dc47df0ecf27a101f520d6fc422a6d84f011f1ab | |
parent | d17bbc7a0bdc30302df0001ddad91733064d3d11 (diff) | |
parent | 88f0080a0ffb299006950c0453eabddb7d17f078 (diff) | |
download | px4-firmware-72979032e9bfef200809e97663c613b7b530b011.tar.gz px4-firmware-72979032e9bfef200809e97663c613b7b530b011.tar.bz2 px4-firmware-72979032e9bfef200809e97663c613b7b530b011.zip |
Merge branch 'master' into px4dev_new_param
29 files changed, 1243 insertions, 117 deletions
diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS index 33f63ca87..7b8d51338 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/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index fdf6c9d91..09cbdd4e9 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 a81e102e9..a642ac093 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_mag.h b/apps/drivers/drv_mag.h index 673a3988f..a59291778 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; @@ -76,7 +78,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) @@ -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/drv_orb_dev.h b/apps/drivers/drv_orb_dev.h index b3fc01a5f..50288f690 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/Makefile b/apps/drivers/hmc5883/Makefile new file mode 100644 index 000000000..aa4a397fb --- /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 000000000..0fa1f365d --- /dev/null +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -0,0 +1,931 @@ +/**************************************************************************** + * + * 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 <nuttx/config.h> + +#include <drivers/device/i2c.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <arch/board/up_hrt.h> + +#include <systemlib/perf_counter.h> + +#include <drivers/drv_mag.h> + +/* + * 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: + work_s _work; + unsigned _measure_ticks; + + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + mag_report *_reports; + mag_scale _scale; + bool _collect_phase; + + orb_advert_t _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), + _mag_topic(-1), + _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; + + // 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; +} + +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; + +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 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 (DRDY) 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 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: + /* 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() +{ + /* + * 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) { + + /* 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 y[2]; + uint8_t z[2]; + } hmc_report; +#pragma pack(pop) + struct { + int16_t x, y, z; + } report; + int ret = -EIO; + 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 = 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; + } + + /* 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]; + + /* + * 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]); + + /* 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/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 7576fc1d9..951e1a062 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 6c6951d9b..8f8f96217 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; @@ -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) { diff --git a/apps/examples/px4_simple_app/px4_simple_app.c b/apps/examples/px4_simple_app/px4_simple_app.c index fd34a5dac..7f655964d 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 bad62edcf..7ea4101ab 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 cef70601b..0333c7100 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 4b520d403..54912b6d3 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 771dfbd6c..a629e904d 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 653a41090..596953789 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 baee507b2..df08ca75f 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 ff32fb460..3f9c72517 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 773cd87ff..17482dc0a 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 feaed6695..fd3ed1137 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); @@ -143,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 9d8847246..3016cb01e 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -64,6 +64,7 @@ #include <arch/board/up_pwm_servo.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_outputs.h> class FMUServo : public device::CDev @@ -88,6 +89,7 @@ private: int _task; int _t_actuators; int _t_armed; + orb_advert_t _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); -} - -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; + const actuator_controls_s *controls = (actuator_controls_s *)handle; - 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; @@ -564,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 a5def874d..c3371c138 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 6a06a786f..891167d1c 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 */ @@ -514,6 +513,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 +565,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 +618,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())); } diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index bc944ccd9..44a33bf8b 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; } diff --git a/apps/uORB/Makefile b/apps/uORB/Makefile index 0766a66eb..64ba52e9c 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 diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index d6b566b6a..6ff66fceb 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 000000000..63441a059 --- /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 <stdint.h> +#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 diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp index 1e7cdc8db..c4434e4ed 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 c36d0044f..eb068d2b7 100644 --- a/apps/uORB/uORB.h +++ b/apps/uORB/uORB.h @@ -106,11 +106,26 @@ 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. diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 040ad0c48..a99b5f6ad 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -86,6 +86,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 |