diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-03-09 11:03:38 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-03-09 11:03:38 +0100 |
commit | c9775d745578f53e62362070414573b2b655ade8 (patch) | |
tree | b4627cb377c714c6fc42488c879db701062fa968 /apps | |
parent | cc628fbc27794fee52e3a6f33d091758ca1cb51a (diff) | |
parent | 35790e673b3ab11eba6c9c5640b3ce60a2e6c70a (diff) | |
download | px4-firmware-c9775d745578f53e62362070414573b2b655ade8.tar.gz px4-firmware-c9775d745578f53e62362070414573b2b655ade8.tar.bz2 px4-firmware-c9775d745578f53e62362070414573b2b655ade8.zip |
Merge branch 'px4io-i2c' of github.com:PX4/Firmware into px4io-i2c
Diffstat (limited to 'apps')
-rw-r--r-- | apps/drivers/drv_range_finder.h | 81 | ||||
-rw-r--r-- | apps/drivers/mb12xx/Makefile | 42 | ||||
-rw-r--r-- | apps/drivers/mb12xx/mb12xx.cpp | 842 | ||||
-rw-r--r-- | apps/drivers/px4io/px4io.cpp | 70 | ||||
-rw-r--r-- | apps/mavlink/mavlink_log.h | 12 | ||||
-rw-r--r-- | apps/px4io/controls.c | 55 | ||||
-rw-r--r-- | apps/px4io/mixer.cpp | 24 | ||||
-rw-r--r-- | apps/px4io/protocol.h | 1 | ||||
-rw-r--r-- | apps/px4io/px4io.c | 8 | ||||
-rw-r--r-- | apps/px4io/registers.c | 59 | ||||
-rw-r--r-- | apps/px4io/safety.c | 7 | ||||
-rw-r--r-- | apps/sensors/sensors.cpp | 42 | ||||
-rw-r--r-- | apps/systemcmds/mixer/mixer.c | 18 | ||||
-rw-r--r-- | apps/systemlib/mixer/mixer.cpp | 1 | ||||
-rw-r--r-- | apps/systemlib/mixer/mixer_group.cpp | 1 | ||||
-rw-r--r-- | apps/uORB/objects_common.cpp | 3 | ||||
-rw-r--r-- | apps/uORB/topics/subsystem_info.h | 3 |
17 files changed, 1191 insertions, 78 deletions
diff --git a/apps/drivers/drv_range_finder.h b/apps/drivers/drv_range_finder.h new file mode 100644 index 000000000..03a82ec5d --- /dev/null +++ b/apps/drivers/drv_range_finder.h @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Rangefinder driver interface. + */ + +#ifndef _DRV_RANGEFINDER_H +#define _DRV_RANGEFINDER_H + +#include <stdint.h> +#include <sys/ioctl.h> + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +#define RANGE_FINDER_DEVICE_PATH "/dev/range_finder" + +/** + * range finder report structure. Reads from the device must be in multiples of this + * structure. + */ +struct range_finder_report { + uint64_t timestamp; + float distance; /** in meters */ + uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */ +}; + +/* + * ObjDev tag for raw range finder data. + */ +ORB_DECLARE(sensor_range_finder); + +/* + * ioctl() definitions + * + * Rangefinder drivers also implement the generic sensor driver + * interfaces from drv_sensor.h + */ + +#define _RANGEFINDERIOCBASE (0x7900) +#define __RANGEFINDERIOC(_n) (_IOC(_RANGEFINDERIOCBASE, _n)) + +/** set the minimum effective distance of the device */ +#define RANGEFINDERIOCSETMINIUMDISTANCE __RANGEFINDERIOC(1) + +/** set the maximum effective distance of the device */ +#define RANGEFINDERIOCSETMAXIUMDISTANCE __RANGEFINDERIOC(2) + + +#endif /* _DRV_RANGEFINDER_H */ diff --git a/apps/drivers/mb12xx/Makefile b/apps/drivers/mb12xx/Makefile new file mode 100644 index 000000000..0d2405787 --- /dev/null +++ b/apps/drivers/mb12xx/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the Maxbotix Sonar driver. +# + +APPNAME = mb12xx +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/mb12xx/mb12xx.cpp b/apps/drivers/mb12xx/mb12xx.cpp new file mode 100644 index 000000000..0e4712f5a --- /dev/null +++ b/apps/drivers/mb12xx/mb12xx.cpp @@ -0,0 +1,842 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mb12xx.cpp + * @author Greg Hulands + * + * Driver for the Maxbotix sonar range finders 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/board.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> + +#include <drivers/drv_hrt.h> +#include <drivers/drv_range_finder.h> + +#include <uORB/uORB.h> +#include <uORB/topics/subsystem_info.h> + +/* Configuration Constants */ +#define MB12XX_BUS PX4_I2C_BUS_EXPANSION +#define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ + +/* MB12xx Registers addresses */ + +#define MB12XX_TAKE_RANGE_REG 0x51 /* Measure range Register */ +#define MB12XX_SET_ADDRESS_1 0xAA /* Change address 1 Register */ +#define MB12XX_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ + +/* Device limits */ +#define MB12XX_MIN_DISTANCE (0.20f) +#define MB12XX_MAX_DISTANCE (7.65f) + +#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class MB12XX : public device::I2C +{ +public: + MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR); + ~MB12XX(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + range_finder_report *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE + * and MB12XX_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]); + +MB12XX::MB12XX(int bus, int address) : + I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000), + _min_distance(MB12XX_MIN_DISTANCE), + _max_distance(MB12XX_MAX_DISTANCE), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _range_finder_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), + _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) +{ + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +MB12XX::~MB12XX() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +int +MB12XX::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_reports = 2; + _reports = new struct range_finder_report[_num_reports]; + + if (_reports == nullptr) + goto out; + + _oldest_report = _next_report = 0; + + /* get a publish handle on the range finder topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]); + + if (_range_finder_topic < 0) + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +MB12XX::probe() +{ + // TODO: take a range reading and see if it is between the min and max + + return OK; +} + +void +MB12XX::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +MB12XX::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +MB12XX::get_minimum_distance() +{ + return _min_distance; +} + +float +MB12XX::get_maximum_distance() +{ + return _max_distance; +} + +int +MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* add one to account for the sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct range_finder_report *buf = new struct range_finder_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: + { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + case RANGEFINDERIOCSETMAXIUMDISTANCE: + { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +MB12XX::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + /* XXX really it'd be nice to lock against other readers here */ + do { + _oldest_report = _next_report = 0; + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(MB12XX_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + } while (0); + + return ret; +} + +int +MB12XX::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + uint8_t cmd = MB12XX_TAKE_RANGE_REG; + ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) + { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + return ret; + } + ret = OK; + + return ret; +} + +int +MB12XX::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + + perf_begin(_sample_perf); + + ret = transfer(nullptr, 0, &val[0], 2); + + if (ret < 0) + { + log("error reading from sensor: %d", ret); + return ret; + } + + uint16_t distance = val[0] << 8 | val[1]; + float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + _reports[_next_report].timestamp = hrt_absolute_time(); + _reports[_next_report].distance = si_units; + _reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + + /* publish it */ + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, toss it */ + if (_next_report == _oldest_report) { + perf_count(_buffer_overflows); + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + +out: + perf_end(_sample_perf); + return ret; + + return ret; +} + +void +MB12XX::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER}; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +MB12XX::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +MB12XX::cycle_trampoline(void *arg) +{ + MB12XX *dev = (MB12XX *)arg; + + dev->cycle(); +} + +void +MB12XX::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MB12XX::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) + log("measure error"); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MB12XX::cycle_trampoline, + this, + USEC2TICK(MB12XX_CONVERSION_INTERVAL)); +} + +void +MB12XX::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); +} + +/** + * Local functions in support of the shell command. + */ +namespace mb12xx +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +MB12XX *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new MB12XX(MB12XX_BUS); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); + +fail: + + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + else + { + errx(1, "driver not running"); + } + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + int ret; + + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "immediate read failed"); + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + errx(1, "failed to set 2Hz poll rate"); + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "periodic read failed"); + + warnx("periodic read %u", i); + warnx("measurement: %0.3f", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +mb12xx_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + mb12xx::start(); + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + mb12xx::stop(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + mb12xx::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + mb12xx::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + mb12xx::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2611c4e9c..f9ffa6bcd 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -82,7 +82,9 @@ #include <uORB/topics/parameter_update.h> #include <px4io/protocol.h> +#include <mavlink/mavlink_log.h> #include "uploader.h" +#include <debug.h> class PX4IO : public device::I2C @@ -111,6 +113,8 @@ private: volatile int _task; ///< worker task volatile bool _task_should_exit; + int _mavlink_fd; + perf_counter_t _perf_update; /* cached IO state */ @@ -285,6 +289,7 @@ PX4IO::PX4IO() : _update_interval(0), _task(-1), _task_should_exit(false), + _mavlink_fd(-1), _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), _status(0), _alarms(0), @@ -301,6 +306,9 @@ PX4IO::PX4IO() : /* we need this potentially before it could be set in task_main */ g_dev = this; + /* open MAVLink text channel */ + _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + _debug_enabled = true; } @@ -354,6 +362,7 @@ PX4IO::init() (_max_rc_input < 1) || (_max_rc_input > 255)) { log("failed getting parameters from PX4IO"); + mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort."); return -1; } if (_max_rc_input > RC_INPUT_MAX_CHANNELS) @@ -380,6 +389,8 @@ PX4IO::init() if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) { + mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); + /* WARNING: COMMANDER app/vehicle status must be initialized. * If this fails (or the app is not started), worst-case IO * remains untouched (so manual override is still available). @@ -469,6 +480,7 @@ PX4IO::init() ret = io_set_rc_config(); if (ret != OK) { log("failed to update RC input config"); + mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail"); return ret; } @@ -490,6 +502,8 @@ PX4IO::init() return -errno; } + mavlink_log_info(_mavlink_fd, "[IO] init ok"); + return OK; } @@ -771,9 +785,16 @@ PX4IO::io_set_rc_config() /* send channel config to IO */ ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); if (ret != OK) { - log("RC config update failed"); + log("rc config upload failed"); + break; + } + + /* check the IO initialisation flag */ + if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) { + log("config for RC%d rejected by IO", i + 1); break; } + offset += PX4IO_P_RC_CONFIG_STRIDE; } @@ -1157,9 +1178,11 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) /* check for the mixer-OK flag */ if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { debug("mixer upload OK"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok"); return 0; } else { debug("mixer rejected by IO"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); } /* load must have failed for some reason */ @@ -1186,7 +1209,7 @@ PX4IO::print_status() printf("%u bytes free\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); - printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s\n", + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), @@ -1197,7 +1220,8 @@ PX4IO::print_status() ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""), ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC")); + ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), + ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL")); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); printf("alarms 0x%04x%s%s%s%s%s%s\n", alarms, @@ -1475,7 +1499,7 @@ test(void) servos[i] = pwm_value; ret = write(fd, servos, sizeof(servos)); - if (ret != sizeof(servos)) + if (ret != (int)sizeof(servos)) err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); if (direction > 0) { @@ -1547,7 +1571,7 @@ px4io_main(int argc, char *argv[]) errx(1, "already loaded"); /* create the driver - it will set g_dev */ - (void)new PX4IO; + (void)new PX4IO(); if (g_dev == nullptr) errx(1, "driver alloc failed"); @@ -1558,7 +1582,7 @@ px4io_main(int argc, char *argv[]) } /* look for the optional pwm update rate for the supported modes */ - if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { + if ((argc > 2) && (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0)) { if (argc > 2 + 1) { #warning implement this } else { @@ -1570,16 +1594,31 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "recovery")) { + + if (g_dev != nullptr) { + /* + * Enable in-air restart support. + * We can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + g_dev->ioctl(NULL, PWM_SERVO_INAIR_RESTART_ENABLE, 0); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "stop")) { - if (g_dev != nullptr) { - /* stop the driver */ - delete g_dev; - } else { - errx(1, "not loaded"); - } - exit(0); + if (g_dev != nullptr) { + /* stop the driver */ + delete g_dev; + } else { + errx(1, "not loaded"); } + exit(0); + } if (!strcmp(argv[1], "status")) { @@ -1604,8 +1643,9 @@ px4io_main(int argc, char *argv[]) exit(1); } uint8_t level = atoi(argv[2]); - // we can cheat and call the driver directly, as it - // doesn't reference filp in ioctl() + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_DEBUG, level); if (ret != 0) { printf("SET_DEBUG failed - %d\n", ret); diff --git a/apps/mavlink/mavlink_log.h b/apps/mavlink/mavlink_log.h index 62e6f7ca0..233a76cb3 100644 --- a/apps/mavlink/mavlink_log.h +++ b/apps/mavlink/mavlink_log.h @@ -63,7 +63,11 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ +#ifdef __cplusplus +#define mavlink_log_emergency(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); +#else #define mavlink_log_emergency(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); +#endif /** * Send a mavlink critical message. @@ -71,7 +75,11 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ +#ifdef __cplusplus +#define mavlink_log_critical(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); +#else #define mavlink_log_critical(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); +#endif /** * Send a mavlink info message. @@ -79,7 +87,11 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ +#ifdef __cplusplus +#define mavlink_log_info(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); +#else #define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); +#endif struct mavlink_logmessage { char text[51]; diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index dabdde0af..2d99116f8 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -147,32 +147,44 @@ controls_tick() { uint16_t raw = r_raw_rc_values[i]; - /* implement the deadzone */ - if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) { - raw += conf[PX4IO_P_RC_CONFIG_DEADZONE]; - if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) - raw = conf[PX4IO_P_RC_CONFIG_CENTER]; - } - if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) { - raw -= conf[PX4IO_P_RC_CONFIG_DEADZONE]; - if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) - raw = conf[PX4IO_P_RC_CONFIG_CENTER]; - } + int16_t scaled; - /* constrain to min/max values */ + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) raw = conf[PX4IO_P_RC_CONFIG_MIN]; if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) raw = conf[PX4IO_P_RC_CONFIG_MAX]; - int16_t scaled = raw; - - /* adjust to zero-relative around center (nominal -500..500) */ - scaled -= conf[PX4IO_P_RC_CONFIG_CENTER]; - - /* scale to fixed-point representation (-10000..10000) */ - scaled *= 20; + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } + /* invert channel if requested */ if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) scaled = -scaled; @@ -256,10 +268,11 @@ controls_tick() { bool override = false; /* - * Check mapped channel 5; if the value is 'high' then the pilot has + * Check mapped channel 5 (can be any remote channel, + * depends on RC_MAP_OVER parameter); + * If the value is 'high' then the pilot has * requested override. * - * XXX This should be configurable. */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH)) override = true; diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 65de864ec..092a35a44 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -109,9 +109,11 @@ mixer_tick(void) /* * Decide which set of controls we're using. */ - if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) { + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) || + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { - /* don't actually mix anything - we already have raw PWM values */ + /* don't actually mix anything - we already have raw PWM values or + not a valid mixer. */ source = MIX_NONE; } else { @@ -172,9 +174,11 @@ mixer_tick(void) * XXX correct behaviour for failsafe may require an additional case * here. */ - bool should_arm = (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && - /* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK))); + bool should_arm = ( + /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + /* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ @@ -239,6 +243,11 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { + /* do not allow a mixer change while fully armed */ + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + return; + } px4io_mixdata *msg = (px4io_mixdata *)buffer; @@ -252,9 +261,12 @@ mixer_handle_text(const void *buffer, size_t length) switch (msg->action) { case F2I_MIXER_ACTION_RESET: isr_debug(2, "reset"); + + /* FIRST mark the mixer as invalid */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + /* THEN actually delete it */ mixer_group.reset(); mixer_text_length = 0; - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 0ee6d2c4b..14d221b5b 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -103,6 +103,7 @@ #define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */ #define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ #define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ +#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 589264661..0c4838523 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -215,12 +215,16 @@ user_start(int argc, char *argv[]) /* post debug state at ~1Hz */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u", + + struct mallinfo minfo = mallinfo(); + + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", (unsigned)debug_level, (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, - (unsigned)i2c_loop_resets); + (unsigned)i2c_loop_resets, + (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } } diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 5ec2f7258..511a47f8d 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -361,6 +361,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_PAGE_RC_CONFIG: { + + /* do not allow a RC config change while fully armed */ + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + break; + } + unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; @@ -383,6 +390,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_RC_CONFIG_OPTIONS: value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; /* set all options except the enabled option */ conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; @@ -390,27 +398,44 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* should the channel be enabled? */ /* this option is normally set last */ if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint8_t count = 0; + /* assert min..center..max ordering */ - if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) - break; - if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) - break; - if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) - break; - if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) - break; + if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { + count++; + } + /* assert deadzone is sane */ - if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) - break; - if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) - break; - if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) - break; - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) - break; + if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { + count++; + } + // The following check isn't needed as constraint checks in controls.c will catch this. + //if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + // count++; + //} + //if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + // count++; + //} + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + count++; + } /* sanity checks pass, enable channel */ - conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + if (count) { + isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; + } else { + conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } } break; /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 540636e19..5495d5ae1 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -176,12 +176,17 @@ heartbeat_blink(void *arg) static void failsafe_blink(void *arg) { + /* indicate that a serious initialisation error occured */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { + LED_AMBER(true); + return; + } + static bool failsafe = false; /* blink the failsafe LED if we don't have FMU input */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { failsafe = !failsafe; - } else { failsafe = false; } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 89fd71dc6..d725c7727 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1129,31 +1129,45 @@ Sensors::ppm_poll() /* Read out values from raw message */ for (unsigned int i = 0; i < channel_limit; i++) { - /* scale around the mid point differently for lower and upper range */ + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (rc_input.values[i] < _parameters.min[i]) + rc_input.values[i] = _parameters.min[i]; + if (rc_input.values[i] > _parameters.max[i]) + rc_input.values[i] = _parameters.max[i]; + + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * the total range is 2 (-1..1). + * If center (trim) == min, scale to 0..1, if center (trim) == max, + * scale to -1..0. + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { - _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); + _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { - /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ - _rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); + _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); } else { /* in the configured dead zone, output zero */ _rc.chan[i].scaled = 0.0f; } - /* reverse channel if required */ - if (i == (int)_rc.function[THROTTLE]) { - if ((int)_parameters.rev[i] == -1) { - _rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled; - } - - } else { - _rc.chan[i].scaled *= _parameters.rev[i]; - } + _rc.chan[i].scaled *= _parameters.rev[i]; /* handle any parameter-induced blowups */ - if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled)) + if (!isfinite(_rc.chan[i].scaled)) _rc.chan[i].scaled = 0.0f; } diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c index e2f7b5bd5..55c4f0836 100644 --- a/apps/systemcmds/mixer/mixer.c +++ b/apps/systemcmds/mixer/mixer.c @@ -117,7 +117,23 @@ load(const char *devname, const char *fname) if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) continue; - /* XXX an optimisation here would be to strip extra whitespace */ + /* compact whitespace in the buffer */ + char *t, *f; + for (f = buf; *f != '\0'; f++) { + /* scan for space characters */ + if (*f == ' ') { + /* look for additional spaces */ + t = f + 1; + while (*t == ' ') + t++; + if (*t == '\0') { + /* strip trailing whitespace */ + *f = '\0'; + } else if (t > (f + 1)) { + memmove(f + 1, t, strlen(t) + 1); + } + } + } /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp index 72f765d90..df0dfe838 100644 --- a/apps/systemlib/mixer/mixer.cpp +++ b/apps/systemlib/mixer/mixer.cpp @@ -54,6 +54,7 @@ #include "mixer.h" Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) : + _next(nullptr), _control_cb(control_cb), _cb_handle(cb_handle) { diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 60eeff225..1dbc512cd 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -93,6 +93,7 @@ MixerGroup::reset() mixer = _first; _first = mixer->_next; delete mixer; + mixer = nullptr; } } diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 3f3476f62..136375140 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -53,6 +53,9 @@ ORB_DEFINE(sensor_gyro, struct gyro_report); #include <drivers/drv_baro.h> ORB_DEFINE(sensor_baro, struct baro_report); +#include <drivers/drv_range_finder.h> +ORB_DEFINE(sensor_range_finder, struct range_finder_report); + #include <drivers/drv_pwm_output.h> ORB_DEFINE(output_pwm, struct pwm_output_values); diff --git a/apps/uORB/topics/subsystem_info.h b/apps/uORB/topics/subsystem_info.h index c3e039d66..c415e832e 100644 --- a/apps/uORB/topics/subsystem_info.h +++ b/apps/uORB/topics/subsystem_info.h @@ -71,7 +71,8 @@ enum SUBSYSTEM_TYPE SUBSYSTEM_TYPE_YAWPOSITION = 4096, SUBSYSTEM_TYPE_ALTITUDECONTROL = 16384, SUBSYSTEM_TYPE_POSITIONCONTROL = 32768, - SUBSYSTEM_TYPE_MOTORCONTROL = 65536 + SUBSYSTEM_TYPE_MOTORCONTROL = 65536, + SUBSYSTEM_TYPE_RANGEFINDER = 131072 }; /** |