From d013ac092769f7975e540a24c5756002251248a4 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 13 Mar 2015 23:36:46 -0700 Subject: [PATCH] Support for building more modules with Linux Added more queue support to linux/px4_layer. Use virt char devices for ms5611, and mavlink. Added more HRT functionality. uORB latency test now fails. Likely due to bad HRT impl for Linux. Signed-off-by: Mark Charlebois --- makefiles/linux/config_linux_default.mk | 8 +- makefiles/toolchain_native.mk | 5 +- .../device/{cdev.cpp => cdev_nuttx.cpp} | 0 src/drivers/device/device.cpp | 159 +- src/drivers/device/device_nuttx.cpp | 257 ++ src/drivers/device/i2c.h | 6 +- src/drivers/device/i2c_linux.cpp | 257 ++ src/drivers/device/{i2c.cpp => i2c_nuttx.cpp} | 0 src/drivers/device/module.mk | 11 +- src/drivers/device/ringbuffer.h | 2 +- src/drivers/device/spi.h | 2 +- src/drivers/device/vdevice.h | 33 + src/drivers/drv_accel.h | 2 +- src/drivers/drv_airspeed.h | 2 +- src/drivers/drv_baro.h | 3 +- src/drivers/drv_device.h | 2 +- src/drivers/drv_gyro.h | 2 +- src/drivers/drv_mag.h | 2 +- src/drivers/drv_sensor.h | 3 +- src/drivers/ms5611/module.mk | 6 +- src/drivers/ms5611/ms5611.h | 6 +- src/drivers/ms5611/ms5611_i2c.cpp | 31 +- src/drivers/ms5611/ms5611_linux.cpp | 1215 +++++++++ .../ms5611/{ms5611.cpp => ms5611_nuttx.cpp} | 0 src/drivers/ms5611/ms5611_spi.cpp | 2 +- src/modules/mavlink/mavlink_ftp.h | 2 +- src/modules/mavlink/mavlink_ftp_linux.cpp | 861 ++++++ src/modules/mavlink/mavlink_main.h | 19 +- src/modules/mavlink/mavlink_main_linux.cpp | 1795 +++++++++++++ src/modules/mavlink/mavlink_main_linux.h | 417 +++ src/modules/mavlink/mavlink_main_nuttx.h | 415 +++ src/modules/mavlink/mavlink_receiver.cpp | 9 +- src/modules/mavlink/module.mk | 14 +- src/modules/sensors/module.mk | 8 +- src/modules/sensors/sensors_linux.cpp | 2308 +++++++++++++++++ .../{sensors.cpp => sensors_nuttx.cpp} | 0 src/platforms/linux/include/queue.h | 8 - src/platforms/linux/main.cpp | 59 +- src/platforms/linux/px4_layer/dq_addlast.c | 74 + src/platforms/linux/px4_layer/dq_remfirst.c | 82 + src/platforms/linux/px4_layer/drv_hrt.c | 197 +- src/platforms/linux/px4_layer/module.mk | 5 +- .../linux/px4_layer/px4_linux_impl.cpp | 21 + .../linux/px4_layer/px4_linux_tasks.c | 34 +- src/platforms/linux/px4_layer/sq_addafter.c | 71 + .../vdevice.cpp => platforms/px4_adc.h} | 91 +- src/platforms/px4_config.h | 6 + src/platforms/px4_defines.h | 3 + src/platforms/px4_i2c.h | 129 + src/platforms/px4_posix.h | 10 +- src/platforms/px4_spi.h | 35 + src/platforms/px4_tasks.h | 4 + src/platforms/px4_workqueue.h | 116 + 53 files changed, 8455 insertions(+), 354 deletions(-) rename src/drivers/device/{cdev.cpp => cdev_nuttx.cpp} (100%) create mode 100644 src/drivers/device/device_nuttx.cpp create mode 100644 src/drivers/device/i2c_linux.cpp rename src/drivers/device/{i2c.cpp => i2c_nuttx.cpp} (100%) create mode 100644 src/drivers/ms5611/ms5611_linux.cpp rename src/drivers/ms5611/{ms5611.cpp => ms5611_nuttx.cpp} (100%) create mode 100644 src/modules/mavlink/mavlink_ftp_linux.cpp create mode 100644 src/modules/mavlink/mavlink_main_linux.cpp create mode 100644 src/modules/mavlink/mavlink_main_linux.h create mode 100644 src/modules/mavlink/mavlink_main_nuttx.h create mode 100644 src/modules/sensors/sensors_linux.cpp rename src/modules/sensors/{sensors.cpp => sensors_nuttx.cpp} (100%) create mode 100644 src/platforms/linux/px4_layer/dq_addlast.c create mode 100644 src/platforms/linux/px4_layer/dq_remfirst.c create mode 100644 src/platforms/linux/px4_layer/sq_addafter.c rename src/{drivers/device/vdevice.cpp => platforms/px4_adc.h} (59%) create mode 100644 src/platforms/px4_i2c.h create mode 100644 src/platforms/px4_spi.h create mode 100644 src/platforms/px4_workqueue.h diff --git a/makefiles/linux/config_linux_default.mk b/makefiles/linux/config_linux_default.mk index e4382cfce5..8286c1c367 100644 --- a/makefiles/linux/config_linux_default.mk +++ b/makefiles/linux/config_linux_default.mk @@ -11,7 +11,8 @@ # Board support modules # MODULES += drivers/device -#MODULES += modules/sensors +MODULES += modules/sensors +MODULES += drivers/ms5611 # # System commands @@ -21,8 +22,7 @@ MODULES += drivers/device # # General system control # -#MODULES += modules/mavlink -#MODULES += modules/mavlink +MODULES += modules/mavlink # # Estimation modules (EKF/ SO3 / other filters) @@ -48,7 +48,7 @@ MODULES += modules/dataman MODULES += lib/mathlib MODULES += lib/geo MODULES += lib/geo_lookup -#MODULES += lib/conversion +MODULES += lib/conversion # # Linux port diff --git a/makefiles/toolchain_native.mk b/makefiles/toolchain_native.mk index db40689acc..a9598faa39 100644 --- a/makefiles/toolchain_native.mk +++ b/makefiles/toolchain_native.mk @@ -81,7 +81,8 @@ CPP = clang$(CLANGVER) -E DEV_VER_SUPPORTED = 4.2.1 endif -LD = ld.gold +#LD = ld.gold +LD = ld AR = ar rcs NM = nm OBJCOPY = objcopy @@ -286,7 +287,7 @@ endef define PRELINK @$(ECHO) "PRELINK: $1" @$(MKDIR) -p $(dir $1) - $(Q) $(LD) -r -o $1 $2 + $(Q) $(LD) -Ur -o $1 $2 endef # $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev_nuttx.cpp similarity index 100% rename from src/drivers/device/cdev.cpp rename to src/drivers/device/cdev_nuttx.cpp diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index d46901683f..b4976250d4 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -34,64 +34,24 @@ /** * @file device.cpp * - * Fundamental driver base class for the device framework. + * Fundamental driver base class for the virtual device framework. */ #include "device.h" -#include +#include +#include #include #include -#include namespace device { -/** - * Interrupt dispatch table entry. - */ -struct irq_entry { - int irq; - Device *owner; -}; - -static const unsigned irq_nentries = 8; /**< size of the interrupt dispatch table */ -static irq_entry irq_entries[irq_nentries]; /**< interrupt dispatch table (XXX should be a vector) */ - -/** - * Register an interrupt to a specific device. - * - * @param irq The interrupt number to register. - * @param owner The device receiving the interrupt. - * @return OK if the interrupt was registered. - */ -static int register_interrupt(int irq, Device *owner); - -/** - * Unregister an interrupt. - * - * @param irq The previously-registered interrupt to be de-registered. - */ -static void unregister_interrupt(int irq); - -/** - * Handle an interrupt. - * - * @param irq The interrupt being invoked. - * @param context The interrupt register context. - * @return Always returns OK. - */ -static int interrupt(int irq, void *context); - -Device::Device(const char *name, - int irq) : +Device::Device(const char *name) : // public // protected _name(name), - _debug_enabled(false), - // private - _irq(irq), - _irq_attached(false) + _debug_enabled(false) { sem_init(&_lock, 0, 1); @@ -107,9 +67,6 @@ Device::Device(const char *name, Device::~Device() { sem_destroy(&_lock); - - if (_irq_attached) - unregister_interrupt(_irq); } int @@ -117,45 +74,9 @@ Device::init() { int ret = OK; - // If assigned an interrupt, connect it - if (_irq) { - /* ensure it's disabled */ - up_disable_irq(_irq); - - /* register */ - ret = register_interrupt(_irq, this); - - if (ret != OK) - goto out; - - _irq_attached = true; - } - -out: return ret; } -void -Device::interrupt_enable() -{ - if (_irq_attached) - up_enable_irq(_irq); -} - -void -Device::interrupt_disable() -{ - if (_irq_attached) - up_disable_irq(_irq); -} - -void -Device::interrupt(void *context) -{ - // default action is to disable the interrupt so we don't get called again - interrupt_disable(); -} - void Device::log(const char *fmt, ...) { @@ -184,74 +105,26 @@ Device::debug(const char *fmt, ...) } } -static int -register_interrupt(int irq, Device *owner) +int +Device::dev_read(unsigned offset, void *data, unsigned count) { - int ret = -ENOMEM; - - // look for a slot where we can register the interrupt - for (unsigned i = 0; i < irq_nentries; i++) { - if (irq_entries[i].irq == 0) { - - // great, we could put it here; try attaching it - ret = irq_attach(irq, &interrupt); - - if (ret == OK) { - irq_entries[i].irq = irq; - irq_entries[i].owner = owner; - } - - break; - } - } - - return ret; -} - -static void -unregister_interrupt(int irq) -{ - for (unsigned i = 0; i < irq_nentries; i++) { - if (irq_entries[i].irq == irq) { - irq_entries[i].irq = 0; - irq_entries[i].owner = nullptr; - } - } -} - -static int -interrupt(int irq, void *context) -{ - for (unsigned i = 0; i < irq_nentries; i++) { - if (irq_entries[i].irq == irq) { - irq_entries[i].owner->interrupt(context); - break; - } - } - - return OK; + return -ENODEV; } int -Device::read(unsigned offset, void *data, unsigned count) +Device::dev_write(unsigned offset, void *data, unsigned count) { - return -ENODEV; + return -ENODEV; } int -Device::write(unsigned offset, void *data, unsigned count) +Device::dev_ioctl(unsigned operation, unsigned &arg) { - return -ENODEV; -} - -int -Device::ioctl(unsigned operation, unsigned &arg) -{ - switch (operation) { - case DEVIOCGDEVICEID: - return (int)_device_id.devid; - } - return -ENODEV; + switch (operation) { + case PX4_DEVIOCGDEVICEID: + return (int)_device_id.devid; + } + return -ENODEV; } } // namespace device diff --git a/src/drivers/device/device_nuttx.cpp b/src/drivers/device/device_nuttx.cpp new file mode 100644 index 0000000000..d46901683f --- /dev/null +++ b/src/drivers/device/device_nuttx.cpp @@ -0,0 +1,257 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 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 device.cpp + * + * Fundamental driver base class for the device framework. + */ + +#include "device.h" + +#include +#include +#include +#include + +namespace device +{ + +/** + * Interrupt dispatch table entry. + */ +struct irq_entry { + int irq; + Device *owner; +}; + +static const unsigned irq_nentries = 8; /**< size of the interrupt dispatch table */ +static irq_entry irq_entries[irq_nentries]; /**< interrupt dispatch table (XXX should be a vector) */ + +/** + * Register an interrupt to a specific device. + * + * @param irq The interrupt number to register. + * @param owner The device receiving the interrupt. + * @return OK if the interrupt was registered. + */ +static int register_interrupt(int irq, Device *owner); + +/** + * Unregister an interrupt. + * + * @param irq The previously-registered interrupt to be de-registered. + */ +static void unregister_interrupt(int irq); + +/** + * Handle an interrupt. + * + * @param irq The interrupt being invoked. + * @param context The interrupt register context. + * @return Always returns OK. + */ +static int interrupt(int irq, void *context); + +Device::Device(const char *name, + int irq) : + // public + // protected + _name(name), + _debug_enabled(false), + // private + _irq(irq), + _irq_attached(false) +{ + sem_init(&_lock, 0, 1); + + /* setup a default device ID. When bus_type is UNKNOWN the + other fields are invalid */ + _device_id.devid = 0; + _device_id.devid_s.bus_type = DeviceBusType_UNKNOWN; + _device_id.devid_s.bus = 0; + _device_id.devid_s.address = 0; + _device_id.devid_s.devtype = 0; +} + +Device::~Device() +{ + sem_destroy(&_lock); + + if (_irq_attached) + unregister_interrupt(_irq); +} + +int +Device::init() +{ + int ret = OK; + + // If assigned an interrupt, connect it + if (_irq) { + /* ensure it's disabled */ + up_disable_irq(_irq); + + /* register */ + ret = register_interrupt(_irq, this); + + if (ret != OK) + goto out; + + _irq_attached = true; + } + +out: + return ret; +} + +void +Device::interrupt_enable() +{ + if (_irq_attached) + up_enable_irq(_irq); +} + +void +Device::interrupt_disable() +{ + if (_irq_attached) + up_disable_irq(_irq); +} + +void +Device::interrupt(void *context) +{ + // default action is to disable the interrupt so we don't get called again + interrupt_disable(); +} + +void +Device::log(const char *fmt, ...) +{ + va_list ap; + + printf("[%s] ", _name); + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); + printf("\n"); + fflush(stdout); +} + +void +Device::debug(const char *fmt, ...) +{ + va_list ap; + + if (_debug_enabled) { + printf("<%s> ", _name); + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); + printf("\n"); + fflush(stdout); + } +} + +static int +register_interrupt(int irq, Device *owner) +{ + int ret = -ENOMEM; + + // look for a slot where we can register the interrupt + for (unsigned i = 0; i < irq_nentries; i++) { + if (irq_entries[i].irq == 0) { + + // great, we could put it here; try attaching it + ret = irq_attach(irq, &interrupt); + + if (ret == OK) { + irq_entries[i].irq = irq; + irq_entries[i].owner = owner; + } + + break; + } + } + + return ret; +} + +static void +unregister_interrupt(int irq) +{ + for (unsigned i = 0; i < irq_nentries; i++) { + if (irq_entries[i].irq == irq) { + irq_entries[i].irq = 0; + irq_entries[i].owner = nullptr; + } + } +} + +static int +interrupt(int irq, void *context) +{ + for (unsigned i = 0; i < irq_nentries; i++) { + if (irq_entries[i].irq == irq) { + irq_entries[i].owner->interrupt(context); + break; + } + } + + return OK; +} + +int +Device::read(unsigned offset, void *data, unsigned count) +{ + return -ENODEV; +} + +int +Device::write(unsigned offset, void *data, unsigned count) +{ + return -ENODEV; +} + +int +Device::ioctl(unsigned operation, unsigned &arg) +{ + switch (operation) { + case DEVIOCGDEVICEID: + return (int)_device_id.devid; + } + return -ENODEV; +} + +} // namespace device diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index cb13fed83c..97ab25672c 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -42,7 +42,7 @@ #include "device.h" -#include +#include namespace device __EXPORT { @@ -124,7 +124,7 @@ protected: * @return OK if the transfer was successful, -errno * otherwise. */ - int transfer(i2c_msg_s *msgv, unsigned msgs); + int transfer(px4_i2c_msg_t *msgv, unsigned msgs); /** * Change the bus address. @@ -142,7 +142,7 @@ protected: private: uint16_t _address; uint32_t _frequency; - struct i2c_dev_s *_dev; + px4_i2c_dev_t *_dev; I2C(const device::I2C &); I2C operator=(const device::I2C &); diff --git a/src/drivers/device/i2c_linux.cpp b/src/drivers/device/i2c_linux.cpp new file mode 100644 index 0000000000..4ab3ff1723 --- /dev/null +++ b/src/drivers/device/i2c_linux.cpp @@ -0,0 +1,257 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 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 i2c.cpp + * + * Base class for devices attached via the I2C bus. + * + * @todo Bus frequency changes; currently we do nothing with the value + * that is supplied. Should we just depend on the bus knowing? + */ + +#include "i2c.h" + +namespace device +{ + +unsigned int I2C::_bus_clocks[3] = { 100000, 100000, 100000 }; + +I2C::I2C(const char *name, + const char *devname, + int bus, + uint16_t address, + uint32_t frequency, + int irq) : + // base class + CDev(name, devname), + // public + // protected + _retries(0), + // private + _bus(bus), + _address(address), + _frequency(frequency), + _dev(nullptr) +{ + // fill in _device_id fields for a I2C device + _device_id.devid_s.bus_type = DeviceBusType_I2C; + _device_id.devid_s.bus = bus; + _device_id.devid_s.address = address; + // devtype needs to be filled in by the driver + _device_id.devid_s.devtype = 0; +} + +I2C::~I2C() +{ + if (_dev) { + px4_i2cuninitialize(_dev); + _dev = nullptr; + } +} + +int +I2C::set_bus_clock(unsigned bus, unsigned clock_hz) +{ + int index = bus - 1; + + if (index < 0 || index >= static_cast(sizeof(_bus_clocks) / sizeof(_bus_clocks[0]))) { + return -EINVAL; + } + + if (_bus_clocks[index] > 0) { + // debug("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz); + } + _bus_clocks[index] = clock_hz; + + return PX4_OK; +} + +int +I2C::init() +{ + int ret = PX4_OK; + unsigned bus_index; + + // attach to the i2c bus + _dev = px4_i2cinitialize(_bus); + + if (_dev == nullptr) { + debug("failed to init I2C"); + ret = -ENOENT; + goto out; + } + + // the above call fails for a non-existing bus index, + // so the index math here is safe. + bus_index = _bus - 1; + + // abort if the max frequency we allow (the frequency we ask) + // is smaller than the bus frequency + if (_bus_clocks[bus_index] > _frequency) { + (void)px4_i2cuninitialize(_dev); + _dev = nullptr; + log("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)", + _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000); + ret = -EINVAL; + goto out; + } + + // set the bus frequency on the first access if it has + // not been set yet + if (_bus_clocks[bus_index] == 0) { + _bus_clocks[bus_index] = _frequency; + } + + // set frequency for this instance once to the bus speed + // the bus speed is the maximum supported by all devices on the bus, + // as we have to prioritize performance over compatibility. + // If a new device requires a lower clock speed, this has to be + // manually set via "fmu i2c " before starting any + // drivers. + // This is necessary as automatically lowering the bus speed + // for maximum compatibility could induce timing issues on + // critical sensors the adopter might be unaware of. + I2C_SETFREQUENCY(_dev, _bus_clocks[bus_index]); + + // call the probe function to check whether the device is present + ret = probe(); + + if (ret != PX4_OK) { + debug("probe failed"); + goto out; + } + + // do base class init, which will create device node, etc + ret = CDev::init(); + + if (ret != PX4_OK) { + debug("cdev init failed"); + goto out; + } + + // tell the world where we are + log("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)", + _bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000); + +out: + if ((ret != PX4_OK) && (_dev != nullptr)) { + px4_i2cuninitialize(_dev); + _dev = nullptr; + } + return ret; +} + +int +I2C::probe() +{ + // Assume the device is too stupid to be discoverable. + return PX4_OK; +} + +int +I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ + px4_i2c_msg_t msgv[2]; + unsigned msgs; + int ret; + unsigned retry_count = 0; + + do { + // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + + msgs = 0; + + if (send_len > 0) { + msgv[msgs].addr = _address; + msgv[msgs].flags = 0; + msgv[msgs].buffer = const_cast(send); + msgv[msgs].length = send_len; + msgs++; + } + + if (recv_len > 0) { + msgv[msgs].addr = _address; + msgv[msgs].flags = I2C_M_READ; + msgv[msgs].buffer = recv; + msgv[msgs].length = recv_len; + msgs++; + } + + if (msgs == 0) + return -EINVAL; + + ret = I2C_TRANSFER(_dev, &msgv[0], msgs); + + /* success */ + if (ret == PX4_OK) + break; + + /* if we have already retried once, or we are going to give up, then reset the bus */ + if ((retry_count >= 1) || (retry_count >= _retries)) + px4_i2creset(_dev); + + } while (retry_count++ < _retries); + + return ret; + +} + +int +I2C::transfer(px4_i2c_msg_t *msgv, unsigned msgs) +{ + int ret; + unsigned retry_count = 0; + + /* force the device address into the message vector */ + for (unsigned i = 0; i < msgs; i++) + msgv[i].addr = _address; + + + do { + ret = I2C_TRANSFER(_dev, msgv, msgs); + + /* success */ + if (ret == PX4_OK) + break; + + /* if we have already retried once, or we are going to give up, then reset the bus */ + if ((retry_count >= 1) || (retry_count >= _retries)) + px4_i2creset(_dev); + + } while (retry_count++ < _retries); + + return ret; +} + +} // namespace device diff --git a/src/drivers/device/i2c.cpp b/src/drivers/device/i2c_nuttx.cpp similarity index 100% rename from src/drivers/device/i2c.cpp rename to src/drivers/device/i2c_nuttx.cpp diff --git a/src/drivers/device/module.mk b/src/drivers/device/module.mk index 5475e01e41..a3660d70a5 100644 --- a/src/drivers/device/module.mk +++ b/src/drivers/device/module.mk @@ -37,14 +37,15 @@ ifeq ($(PX4_TARGET_OS),nuttx) SRCS = \ - device.cpp \ - cdev.cpp \ - i2c.cpp \ + device_nuttx.cpp \ + cdev_nuttx.cpp \ + i2c_nuttx.cpp \ pio.cpp \ spi.cpp endif ifeq ($(PX4_TARGET_OS),linux) SRCS = vcdev.cpp \ - vdevice.cpp \ - vcdev_posix.cpp + device.cpp \ + vcdev_posix.cpp \ + i2c_linux.cpp endif diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index b26e2e7c83..35b38efd7d 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -503,7 +503,7 @@ RingBuffer::resize(unsigned new_size) void RingBuffer::print_info(const char *name) { - printf("%s %u/%u (%u/%u @ %p)\n", + printf("%s %u/%lu (%u/%u @ %p)\n", name, _num_items, _num_items * _item_size, diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 1d98376898..4822880144 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -42,7 +42,7 @@ #include "device.h" -#include +#include namespace device __EXPORT { diff --git a/src/drivers/device/vdevice.h b/src/drivers/device/vdevice.h index f35ac9fc19..bf823f5b5d 100644 --- a/src/drivers/device/vdevice.h +++ b/src/drivers/device/vdevice.h @@ -97,6 +97,39 @@ public: */ virtual int init(); + /** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param offset The device address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read. + * @return The number of items read on success, negative errno otherwise. + */ + int dev_read(unsigned address, void *data, unsigned count); + + /** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param address The device address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of items to write. + * @return The number of items written on success, negative errno otherwise. + */ + int dev_write(unsigned address, void *data, unsigned count); + + /** + * Perform a device-specific operation. + * + * @param operation The operation to perform. + * @param arg An argument to the operation. + * @return Negative errno on error, OK or positive value on success. + */ + int dev_ioctl(unsigned operation, unsigned &arg); + /* device bus types for DEVID */ diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index fccd446ad0..738b28a6e1 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -94,7 +94,7 @@ ORB_DECLARE(sensor_accel); */ #define _ACCELIOCBASE (0x2100) -#define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n)) +#define _ACCELIOC(_n) (_PX4_IOC(_ACCELIOCBASE, _n)) /** set the accel internal sample rate to at least (arg) Hz */ diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h index fddef36263..ff49a60664 100644 --- a/src/drivers/drv_airspeed.h +++ b/src/drivers/drv_airspeed.h @@ -59,7 +59,7 @@ */ #define _AIRSPEEDIOCBASE (0x7700) -#define __AIRSPEEDIOC(_n) (_IOC(_AIRSPEEDIOCBASE, _n)) +#define __AIRSPEEDIOC(_n) (_PX4_IOC(_AIRSPEEDIOCBASE, _n)) #define AIRSPEEDIOCSSCALE __AIRSPEEDIOC(0) #define AIRSPEEDIOCGSCALE __AIRSPEEDIOC(1) diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 65fb3a4cf4..791e3c5cc1 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -40,6 +40,7 @@ #ifndef _DRV_BARO_H #define _DRV_BARO_H +#include #include #include @@ -71,7 +72,7 @@ ORB_DECLARE(sensor_baro); */ #define _BAROIOCBASE (0x2200) -#define _BAROIOC(_n) (_IOC(_BAROIOCBASE, _n)) +#define _BAROIOC(_n) (_PX4_IOC(_BAROIOCBASE, _n)) /** set corrected MSL pressure in pascals */ #define BAROIOCSMSLPRESSURE _BAROIOC(0) diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h index 19d55cef38..650b18bed5 100644 --- a/src/drivers/drv_device.h +++ b/src/drivers/drv_device.h @@ -51,7 +51,7 @@ */ #define _DEVICEIOCBASE (0x100) -#define _DEVICEIOC(_n) (_IOC(_DEVICEIOCBASE, _n)) +#define _DEVICEIOC(_n) (_PX4_IOC(_DEVICEIOCBASE, _n)) /** ask device to stop publishing */ #define DEVIOCSPUBBLOCK _DEVICEIOC(0) diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 122d204159..8e80413ac9 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -91,7 +91,7 @@ ORB_DECLARE(sensor_gyro); */ #define _GYROIOCBASE (0x2300) -#define _GYROIOC(_n) (_IOC(_GYROIOCBASE, _n)) +#define _GYROIOC(_n) (_PX4_IOC(_GYROIOCBASE, _n)) /** set the gyro internal sample rate to at least (arg) Hz */ #define GYROIOCSSAMPLERATE _GYROIOC(0) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 3c12216a62..e2f8493e38 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -91,7 +91,7 @@ ORB_DECLARE(sensor_mag); */ #define _MAGIOCBASE (0x2400) -#define _MAGIOC(_n) (_IOC(_MAGIOCBASE, _n)) +#define _MAGIOC(_n) (_PX4_IOC(_MAGIOCBASE, _n)) /** set the mag internal sample rate to at least (arg) Hz */ #define MAGIOCSSAMPLERATE _MAGIOC(0) diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 467dace082..b1148ce76f 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -40,6 +40,7 @@ #ifndef _DRV_SENSOR_H #define _DRV_SENSOR_H +#include #include #include @@ -69,7 +70,7 @@ */ #define _SENSORIOCBASE (0x2000) -#define _SENSORIOC(_n) (_IOC(_SENSORIOCBASE, _n)) +#define _SENSORIOC(_n) (_PX4_IOC(_SENSORIOCBASE, _n)) /** * Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk index ee74058fc1..ba1ac7efcf 100644 --- a/src/drivers/ms5611/module.mk +++ b/src/drivers/ms5611/module.mk @@ -37,6 +37,10 @@ MODULE_COMMAND = ms5611 -SRCS = ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp +ifeq ($(PX4_TARGET_OS),nuttx) +SRCS = ms5611_nuttx.cpp ms5611_spi.cpp ms5611_i2c.cpp +else +SRCS = ms5611_linux.cpp ms5611_spi.cpp ms5611_i2c.cpp +endif MAXOPTIMIZATION = -Os diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index c946e38cb8..d129891bda 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -80,6 +80,6 @@ extern bool crc4(uint16_t *n_prom); } /* namespace */ /* interface factories */ -extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; -extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; -typedef device::Device *(*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); +extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak)); +extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) __attribute__((weak)); +typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp index 30d5e48881..733fafbd87 100644 --- a/src/drivers/ms5611/ms5611_i2c.cpp +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -39,12 +39,13 @@ /* XXX trim includes */ #include +#include #include #include #include #include -#include +//#include #include #include @@ -70,8 +71,8 @@ public: virtual ~MS5611_I2C(); virtual int init(); - virtual int read(unsigned offset, void *data, unsigned count); - virtual int ioctl(unsigned operation, unsigned &arg); + virtual int dev_read(unsigned offset, void *data, unsigned count); + virtual int dev_ioctl(unsigned operation, unsigned &arg); protected: virtual int probe(); @@ -98,7 +99,7 @@ private: /** * Read the MS5611 PROM * - * @return OK if the PROM reads successfully. + * @return PX4_OK if the PROM reads successfully. */ int _read_prom(); @@ -128,7 +129,7 @@ MS5611_I2C::init() } int -MS5611_I2C::read(unsigned offset, void *data, unsigned count) +MS5611_I2C::dev_read(unsigned offset, void *data, unsigned count) { union _cvt { uint8_t b[4]; @@ -139,7 +140,7 @@ MS5611_I2C::read(unsigned offset, void *data, unsigned count) /* read the most recent measurement */ uint8_t cmd = 0; int ret = transfer(&cmd, 1, &buf[0], 3); - if (ret == OK) { + if (ret == PX4_OK) { /* fetch the raw value */ cvt->b[0] = buf[2]; cvt->b[1] = buf[1]; @@ -151,7 +152,7 @@ MS5611_I2C::read(unsigned offset, void *data, unsigned count) } int -MS5611_I2C::ioctl(unsigned operation, unsigned &arg) +MS5611_I2C::dev_ioctl(unsigned operation, unsigned &arg) { int ret; @@ -176,14 +177,14 @@ MS5611_I2C::probe() { _retries = 10; - if ((OK == _probe_address(MS5611_ADDRESS_1)) || - (OK == _probe_address(MS5611_ADDRESS_2))) { + if ((PX4_OK == _probe_address(MS5611_ADDRESS_1)) || + (PX4_OK == _probe_address(MS5611_ADDRESS_2))) { /* * Disable retries; we may enable them selectively in some cases, * but the device gets confused if we retry some of the commands. */ _retries = 0; - return OK; + return PX4_OK; } return -EIO; @@ -196,14 +197,14 @@ MS5611_I2C::_probe_address(uint8_t address) set_address(address); /* send reset command */ - if (OK != _reset()) + if (PX4_OK != _reset()) return -EIO; /* read PROM */ - if (OK != _read_prom()) + if (PX4_OK != _read_prom()) return -EIO; - return OK; + return PX4_OK; } @@ -254,7 +255,7 @@ MS5611_I2C::_read_prom() for (int i = 0; i < 8; i++) { uint8_t cmd = ADDR_PROM_SETUP + (i * 2); - if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) + if (PX4_OK != transfer(&cmd, 1, &prom_buf[0], 2)) break; /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ @@ -264,5 +265,5 @@ MS5611_I2C::_read_prom() } /* calculate CRC and return success/failure accordingly */ - return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; + return ms5611::crc4(&_prom.c[0]) ? PX4_OK : -EIO; } diff --git a/src/drivers/ms5611/ms5611_linux.cpp b/src/drivers/ms5611/ms5611_linux.cpp new file mode 100644 index 0000000000..043f16407b --- /dev/null +++ b/src/drivers/ms5611/ms5611_linux.cpp @@ -0,0 +1,1215 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 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 ms5611.cpp + * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "ms5611.h" + +enum MS5611_BUS { + MS5611_BUS_ALL = 0, + MS5611_BUS_I2C_INTERNAL, + MS5611_BUS_I2C_EXTERNAL, + MS5611_BUS_SPI_INTERNAL, + MS5611_BUS_SPI_EXTERNAL +}; + +/* 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 + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) + +/* helper macro for arithmetic - returns the square of the argument */ +#define POW2(_x) ((_x) * (_x)) + +/* + * MS5611 internal constants and data structures. + */ + +/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ +#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ +#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ +#define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" +#define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" + +class MS5611 : public device::CDev +{ +public: + MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path); + ~MS5611(); + + virtual int init(); + + virtual ssize_t read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen); + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + Device *_interface; + + ms5611::prom_s _prom; + + struct work_s _work; + unsigned _measure_ticks; + + RingBuffer *_reports; + + bool _collect_phase; + unsigned _measure_phase; + + /* intermediate temperature values per MS5611 datasheet */ + int32_t _TEMP; + int64_t _OFF; + int64_t _SENS; + float _P; + float _T; + + /* altitude conversion calibration */ + unsigned _msl_pressure; /* in Pa */ + + orb_advert_t _baro_topic; + int _orb_class_instance; + int _class_instance; + + perf_counter_t _sample_perf; + perf_counter_t _measure_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Initialize 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_cycle(); + + /** + * Stop the automatic measurement state machine. + */ + void stop_cycle(); + + /** + * 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(); + + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ } + + /** + * 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); + + /** + * Issue a measurement command for the current state. + * + * @return OK if the measurement command was successful. + */ + virtual int measure(); + + /** + * Collect the result of the most recent measurement. + */ + virtual int collect(); +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); + +MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path) : + CDev("MS5611", path), + _interface(interface), + _prom(prom_buf.s), + _measure_ticks(0), + _reports(nullptr), + _collect_phase(false), + _measure_phase(0), + _TEMP(0), + _OFF(0), + _SENS(0), + _msl_pressure(101325), + _baro_topic(-1), + _orb_class_instance(-1), + _class_instance(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), + _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), + _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) +{ + // work_cancel in stop_cycle called from the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +MS5611::~MS5611() +{ + /* make sure we are truly inactive */ + stop_cycle(); + + if (_class_instance != -1) + unregister_class_devname(get_devname(), _class_instance); + + /* free any existing reports */ + if (_reports != nullptr) + delete _reports; + + // free perf counters + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); + + delete _interface; +} + +int +MS5611::init() +{ + int ret; + + ret = CDev::init(); + if (ret != OK) { + debug("CDev init failed"); + goto out; + } + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(baro_report)); + + if (_reports == nullptr) { + debug("can't get memory for reports"); + ret = -ENOMEM; + goto out; + } + + /* register alternate interfaces if we have to */ + _class_instance = register_class_devname(BARO_BASE_DEVICE_PATH); + + struct baro_report brp; + /* do a first measurement cycle to populate reports with valid data */ + _measure_phase = 0; + _reports->flush(); + + /* this do..while is goto without goto */ + do { + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + _reports->get(&brp); + + ret = OK; + + _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, + &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + + + if (_baro_topic < 0) { + warnx("failed to create sensor_baro publication"); + } + + } while (0); + +out: + return ret; +} + +ssize_t +MS5611::read(device::px4_dev_handle_t *handlep, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct baro_report); + struct baro_report *brp = reinterpret_cast(buffer); + 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 (_reports->get(brp)) { + ret += sizeof(*brp); + brp++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _measure_phase = 0; + _reports->flush(); + + /* do temperature first */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + usleep(MS5611_CONVERSION_INTERVAL); + + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(brp)) + ret = sizeof(*brp); + + } while (0); + + return ret; +} + +int +MS5611::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop_cycle(); + _measure_ticks = 0; + return OK; + + /* external signalling 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(MS5611_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start_cycle(); + + 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(MS5611_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_cycle(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + if (!_reports->resize(arg)) { + return -ENOMEM; + } + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* + * Since we are initialized, we do not need to do anything, since the + * PROM is correctly read and the part does not need to be configured. + */ + return OK; + + case BAROIOCSMSLPRESSURE: + + /* range-check for sanity */ + if ((arg < 80000) || (arg > 120000)) + return -EINVAL; + + _msl_pressure = arg; + return OK; + + case BAROIOCGMSLPRESSURE: + return _msl_pressure; + + default: + break; + } + + /* give it to the bus-specific superclass */ + // return bus_ioctl(filp, cmd, arg); + return CDev::ioctl(handlep, cmd, arg); +} + +void +MS5611::start_cycle() +{ + + /* reset the report ring and state machine */ + _collect_phase = false; + _measure_phase = 0; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); +} + +void +MS5611::stop_cycle() +{ + work_cancel(HPWORK, &_work); +} + +void +MS5611::cycle_trampoline(void *arg) +{ + MS5611 *dev = reinterpret_cast(arg); + + dev->cycle(); +} + +void +MS5611::cycle() +{ + int ret; + unsigned dummy; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = collect(); + if (ret != OK) { + if (ret == -6) { + /* + * The ms5611 seems to regularly fail to respond to + * its address; this happens often enough that we'd rather not + * spam the console with a message for this. + */ + } else { + //log("collection error %d", ret); + } + /* issue a reset command to the sensor */ + _interface->dev_ioctl(IOCTL_RESET, dummy); + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + * Don't inject one after temperature measurements, so we can keep + * doing pressure measurements at something close to the desired rate. + */ + if ((_measure_phase != 0) && + (_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + ret = measure(); + if (ret != OK) { + //log("measure error %d", ret); + /* issue a reset command to the sensor */ + _interface->dev_ioctl(IOCTL_RESET, dummy); + /* reset the collection state machine and try again */ + start_cycle(); + return; + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MS5611::cycle_trampoline, + this, + USEC2TICK(MS5611_CONVERSION_INTERVAL)); +} + +int +MS5611::measure() +{ + int ret; + + perf_begin(_measure_perf); + + /* + * In phase zero, request temperature; in other phases, request pressure. + */ + unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + + /* + * Send the command to begin measuring. + */ + ret = _interface->dev_ioctl(IOCTL_MEASURE, addr); + if (OK != ret) + perf_count(_comms_errors); + + perf_end(_measure_perf); + + return ret; +} + +int +MS5611::collect() +{ + int ret; + uint32_t raw; + + perf_begin(_sample_perf); + + struct baro_report report; + /* this should be fairly close to the end of the conversion, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + + /* read the most recent measurement - read offset/size are hardcoded in the interface */ + ret = _interface->dev_read(0, (void *)&raw, 0); + if (ret < 0) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + /* handle a measurement */ + if (_measure_phase == 0) { + + /* temperature offset (in ADC units) */ + int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8); + + /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ + _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23); + + /* base sensor scale/offset values */ + _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8); + _OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7); + + /* temperature compensation */ + if (_TEMP < 2000) { + + int32_t T2 = POW2(dT) >> 31; + + int64_t f = POW2((int64_t)_TEMP - 2000); + int64_t OFF2 = 5 * f >> 1; + int64_t SENS2 = 5 * f >> 2; + + if (_TEMP < -1500) { + int64_t f2 = POW2(_TEMP + 1500); + OFF2 += 7 * f2; + SENS2 += 11 * f2 >> 1; + } + + _TEMP -= T2; + _OFF -= OFF2; + _SENS -= SENS2; + } + + } else { + + /* pressure calculation, result in Pa */ + int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; + _P = P * 0.01f; + _T = _TEMP * 0.01f; + + /* generate a new report */ + report.temperature = _TEMP / 100.0f; + report.pressure = P / 100.0f; /* convert to millibar */ + + /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ + + /* + * PERFORMANCE HINT: + * + * The single precision calculation is 50 microseconds faster than the double + * precision variant. It is however not obvious if double precision is required. + * Pending more inspection and tests, we'll leave the double precision variant active. + * + * Measurements: + * double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us + * single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us + */ + + /* tropospheric properties (0-11km) for standard atmosphere */ + const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ + const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ + const double g = 9.80665; /* gravity constant in m/s/s */ + const double R = 287.05; /* ideal gas constant in J/kg/K */ + + /* current pressure at MSL in kPa */ + double p1 = _msl_pressure / 1000.0; + + /* measured pressure in kPa */ + double p = P / 1000.0; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + + /* publish it */ + if (!(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + } + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + } + + /* update the measurement state machine */ + INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1); + + perf_end(_sample_perf); + + return OK; +} + +void +MS5611::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); + _reports->print_info("report queue"); + printf("TEMP: %d\n", _TEMP); + printf("SENS: %lld\n", (long long)_SENS); + printf("OFF: %lld\n", (long long)_OFF); + printf("P: %.3f\n", (double)_P); + printf("T: %.3f\n", (double)_T); + printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); + + printf("factory_setup %u\n", _prom.factory_setup); + printf("c1_pressure_sens %u\n", _prom.c1_pressure_sens); + printf("c2_pressure_offset %u\n", _prom.c2_pressure_offset); + printf("c3_temp_coeff_pres_sens %u\n", _prom.c3_temp_coeff_pres_sens); + printf("c4_temp_coeff_pres_offset %u\n", _prom.c4_temp_coeff_pres_offset); + printf("c5_reference_temp %u\n", _prom.c5_reference_temp); + printf("c6_temp_coeff_temp %u\n", _prom.c6_temp_coeff_temp); + printf("serial_and_crc %u\n", _prom.serial_and_crc); +} + +/** + * Local functions in support of the shell command. + */ +namespace ms5611 +{ + +/* + list of supported bus configurations + */ +struct ms5611_bus_option { + enum MS5611_BUS busid; + const char *devpath; + MS5611_constructor interface_constructor; + uint8_t busnum; + MS5611 *dev; +} bus_options[] = { +#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT) + { MS5611_BUS_SPI_EXTERNAL, "/dev/ms5611_spi_ext", &MS5611_spi_interface, PX4_SPI_BUS_EXT, NULL }, +#endif +#ifdef PX4_SPIDEV_BARO + { MS5611_BUS_SPI_INTERNAL, "/dev/ms5611_spi_int", &MS5611_spi_interface, PX4_SPI_BUS_SENSORS, NULL }, +#endif +#ifdef PX4_I2C_BUS_ONBOARD + { MS5611_BUS_I2C_INTERNAL, "/dev/ms5611_int", &MS5611_i2c_interface, PX4_I2C_BUS_ONBOARD, NULL }, +#endif +#ifdef PX4_I2C_BUS_EXPANSION + { MS5611_BUS_I2C_EXTERNAL, "/dev/ms5611_ext", &MS5611_i2c_interface, PX4_I2C_BUS_EXPANSION, NULL }, +#endif +}; +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) + +bool start_bus(struct ms5611_bus_option &bus); +struct ms5611_bus_option &find_bus(enum MS5611_BUS busid); +void start(enum MS5611_BUS busid); +void test(enum MS5611_BUS busid); +void reset(enum MS5611_BUS busid); +void info(); +void calibrate(unsigned altitude, enum MS5611_BUS busid); +void usage(); + +/** + * MS5611 crc4 cribbed from the datasheet + */ +bool +crc4(uint16_t *n_prom) +{ + int16_t cnt; + uint16_t n_rem; + uint16_t crc_read; + uint8_t n_bit; + + n_rem = 0x00; + + /* save the read crc */ + crc_read = n_prom[7]; + + /* remove CRC byte */ + n_prom[7] = (0xFF00 & (n_prom[7])); + + for (cnt = 0; cnt < 16; cnt++) { + /* uneven bytes */ + if (cnt & 1) { + n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF); + + } else { + n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8); + } + + for (n_bit = 8; n_bit > 0; n_bit--) { + if (n_rem & 0x8000) { + n_rem = (n_rem << 1) ^ 0x3000; + + } else { + n_rem = (n_rem << 1); + } + } + } + + /* final 4 bit remainder is CRC value */ + n_rem = (0x000F & (n_rem >> 12)); + n_prom[7] = crc_read; + + /* return true if CRCs match */ + return (0x000F & crc_read) == (n_rem ^ 0x00); +} + + +/** + * Start the driver. + */ +bool +start_bus(struct ms5611_bus_option &bus) +{ + if (bus.dev != nullptr) { + warnx("bus option already started"); + return false; + } + + prom_u prom_buf; + device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); + if (interface->init() != OK) { + delete interface; + warnx("no device on bus %u", (unsigned)bus.busid); + return false; + } + + bus.dev = new MS5611(interface, prom_buf, bus.devpath); + if (bus.dev != nullptr && OK != bus.dev->init()) { + delete bus.dev; + bus.dev = NULL; + return false; + } + + int fd = open(bus.devpath, O_RDONLY); + + /* set the poll rate to default, starts automatic data collection */ + if (fd == -1) { + warnx("can't open baro device"); + return false; + } + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + close(fd); + warnx("failed setting default poll rate"); + return false; + } + + close(fd); + return true; +} + + +/** + * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. + */ +void +start(enum MS5611_BUS busid) +{ + uint8_t i; + bool started = false; + + for (i=0; iprint_info(); + } + } +} + +/** + * Calculate actual MSL pressure given current altitude + */ +void +calibrate(unsigned altitude, enum MS5611_BUS busid) +{ + struct ms5611_bus_option &bus = find_bus(busid); + struct baro_report report; + float pressure; + float p1; + + int fd; + + fd = open(bus.devpath, O_RDONLY); + + if (fd < 0) + err(1, "open failed (try 'ms5611 start' if the driver is not running)"); + + /* start the sensor polling at max */ + if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) + errx(1, "failed to set poll rate"); + + /* average a few measurements */ + pressure = 0.0f; + + for (unsigned i = 0; i < 20; i++) { + struct pollfd fds; + int ret; + ssize_t sz; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 1000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = px4_read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "sensor read failed"); + + pressure += report.pressure; + } + + pressure /= 20; /* average */ + pressure /= 10; /* scale from millibar to kPa */ + + /* tropospheric properties (0-11km) for standard atmosphere */ + const float T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ + const float a = -6.5 / 1000; /* temperature gradient in degrees per metre */ + const float g = 9.80665f; /* gravity constant in m/s/s */ + const float R = 287.05f; /* ideal gas constant in J/kg/K */ + + warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude); + + p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R)))); + + warnx("calculated MSL pressure %10.4fkPa", (double)p1); + + /* save as integer Pa */ + p1 *= 1000.0f; + + if (px4_ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) + err(1, "BAROIOCSMSLPRESSURE"); + + close(fd); +} + +void +usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'"); + warnx("options:"); + warnx(" -X (external I2C bus)"); + warnx(" -I (intternal I2C bus)"); + warnx(" -S (external SPI bus)"); + warnx(" -s (internal SPI bus)"); +} + +} // namespace + +int +ms5611_main(int argc, char *argv[]) +{ + enum MS5611_BUS busid = MS5611_BUS_ALL; + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "XISs")) != EOF) { + switch (ch) { + case 'X': + busid = MS5611_BUS_I2C_EXTERNAL; + break; + case 'I': + busid = MS5611_BUS_I2C_INTERNAL; + break; + case 'S': + busid = MS5611_BUS_SPI_EXTERNAL; + break; + case 's': + busid = MS5611_BUS_SPI_INTERNAL; + break; + default: + ms5611::usage(); + return 1; + } + } + + const char *verb = argv[optind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) + ms5611::start(busid); + + /* + * Test the driver/device. + */ + if (!strcmp(verb, "test")) + ms5611::test(busid); + + /* + * Reset the driver. + */ + if (!strcmp(verb, "reset")) + ms5611::reset(busid); + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) + ms5611::info(); + + /* + * Perform MSL pressure calibration given an altitude in metres + */ + if (!strcmp(verb, "calibrate")) { + if (argc < 2) + errx(1, "missing altitude"); + + long altitude = strtol(argv[optind+1], nullptr, 10); + + ms5611::calibrate(altitude, busid); + } + + warnx("unrecognised command, try 'start', 'test', 'reset' or 'info'"); + return 1; +} diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611_nuttx.cpp similarity index 100% rename from src/drivers/ms5611/ms5611.cpp rename to src/drivers/ms5611/ms5611_nuttx.cpp diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 0e68e9cb5d..7ec7718157 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -44,7 +44,7 @@ #include #include #include -#include +//#include #include #include diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 9693a92a97..eca1710174 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -39,7 +39,7 @@ #include #include -#include +#include #include #include "mavlink_messages.h" diff --git a/src/modules/mavlink/mavlink_ftp_linux.cpp b/src/modules/mavlink/mavlink_ftp_linux.cpp new file mode 100644 index 0000000000..1ff7dce735 --- /dev/null +++ b/src/modules/mavlink/mavlink_ftp_linux.cpp @@ -0,0 +1,861 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 mavlink_ftp.cpp +/// @author px4dev, Don Gagne + +#include +#include +#include +#include +#include +#include + +#include "mavlink_ftp.h" +#include "mavlink_tests/mavlink_ftp_test.h" + +// Uncomment the line below to get better debug output. Never commit with this left on. +//#define MAVLINK_FTP_DEBUG + +MavlinkFTP * +MavlinkFTP::get_server(void) +{ + static MavlinkFTP server; + return &server; +} + +MavlinkFTP::MavlinkFTP() : + _request_bufs{}, + _request_queue{}, + _request_queue_sem{}, + _utRcvMsgFunc{}, + _ftp_test{} +{ + // initialise the request freelist + dq_init(&_request_queue); + sem_init(&_request_queue_sem, 0, 1); + + // initialize session list + for (size_t i=0; imsgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { + mavlink_msg_file_transfer_protocol_decode(msg, &req->message); + +#ifdef MAVLINK_FTP_UNIT_TEST + if (!_utRcvMsgFunc) { + warnx("Incorrectly written unit test\n"); + return; + } + // We use fake ids when unit testing + req->serverSystemId = MavlinkFtpTest::serverSystemId; + req->serverComponentId = MavlinkFtpTest::serverComponentId; + req->serverChannel = MavlinkFtpTest::serverChannel; +#else + // Not unit testing, use the real thing + req->serverSystemId = mavlink->get_system_id(); + req->serverComponentId = mavlink->get_component_id(); + req->serverChannel = mavlink->get_channel(); +#endif + + // This is the system id we want to target when sending + req->targetSystemId = msg->sysid; + + if (req->message.target_system == req->serverSystemId) { + req->mavlink = mavlink; +#ifdef MAVLINK_FTP_UNIT_TEST + // We are running in Unit Test mode. Don't queue, just call _worket directly. + _process_request(req); +#else + // We are running in normal mode. Queue the request to the worker + work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0); +#endif + return; + } + } + + _return_request(req); +} + +/// @brief Queued static work queue routine to handle mavlink messages +void +MavlinkFTP::_worker_trampoline(void *arg) +{ + Request* req = reinterpret_cast(arg); + MavlinkFTP* server = MavlinkFTP::get_server(); + + // call the server worker with the work item + server->_process_request(req); +} + +/// @brief Processes an FTP message +void +MavlinkFTP::_process_request(Request *req) +{ + PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); + + ErrorCode errorCode = kErrNone; + + // basic sanity checks; must validate length before use + if (payload->size > kMaxDataLength) { + errorCode = kErrInvalidDataSize; + goto out; + } + +#ifdef MAVLINK_FTP_DEBUG + printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset); +#endif + + switch (payload->opcode) { + case kCmdNone: + break; + + case kCmdTerminateSession: + errorCode = _workTerminate(payload); + break; + + case kCmdResetSessions: + errorCode = _workReset(payload); + break; + + case kCmdListDirectory: + errorCode = _workList(payload); + break; + + case kCmdOpenFileRO: + errorCode = _workOpen(payload, O_RDONLY); + break; + + case kCmdCreateFile: + errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY); + break; + + case kCmdOpenFileWO: + errorCode = _workOpen(payload, O_CREAT | O_WRONLY); + break; + + case kCmdReadFile: + errorCode = _workRead(payload); + break; + + case kCmdWriteFile: + errorCode = _workWrite(payload); + break; + + case kCmdRemoveFile: + errorCode = _workRemoveFile(payload); + break; + + case kCmdRename: + errorCode = _workRename(payload); + break; + + case kCmdTruncateFile: + errorCode = _workTruncateFile(payload); + break; + + case kCmdCreateDirectory: + errorCode = _workCreateDirectory(payload); + break; + + case kCmdRemoveDirectory: + errorCode = _workRemoveDirectory(payload); + break; + + + case kCmdCalcFileCRC32: + errorCode = _workCalcFileCRC32(payload); + break; + + default: + errorCode = kErrUnknownCommand; + break; + } + +out: + // handle success vs. error + if (errorCode == kErrNone) { + payload->req_opcode = payload->opcode; + payload->opcode = kRspAck; +#ifdef MAVLINK_FTP_DEBUG + warnx("FTP: ack\n"); +#endif + } else { + int r_errno = errno; + warnx("FTP: nak %u", errorCode); + payload->req_opcode = payload->opcode; + payload->opcode = kRspNak; + payload->size = 1; + payload->data[0] = errorCode; + if (errorCode == kErrFailErrno) { + payload->size = 2; + payload->data[1] = r_errno; + } + } + + + // respond to the request + _reply(req); + + _return_request(req); +} + +/// @brief Sends the specified FTP reponse message out through mavlink +void +MavlinkFTP::_reply(Request *req) +{ + PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); + + payload->seqNumber = payload->seqNumber + 1; + + mavlink_message_t msg; + msg.checksum = 0; +#ifndef MAVLINK_FTP_UNIT_TEST + uint16_t len = +#endif + mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId, // Sender system id + req->serverComponentId, // Sender component id + req->serverChannel, // Channel to send on + &msg, // Message to pack payload into + 0, // Target network + req->targetSystemId, // Target system id + 0, // Target component id + (const uint8_t*)payload); // Payload to pack into message + + bool success = true; +#ifdef MAVLINK_FTP_UNIT_TEST + // Unit test hook is set, call that instead + _utRcvMsgFunc(&msg, _ftp_test); +#else + Mavlink *mavlink = req->mavlink; + + mavlink->lockMessageBufferMutex(); + success = mavlink->message_buffer_write(&msg, len); + mavlink->unlockMessageBufferMutex(); + +#endif + + if (!success) { + warnx("FTP TX ERR"); + } +#ifdef MAVLINK_FTP_DEBUG + else { + warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d", + req->serverSystemId, + req->serverComponentId, + req->serverChannel, + msg.checksum); + } +#endif +} + +/// @brief Responds to a List command +MavlinkFTP::ErrorCode +MavlinkFTP::_workList(PayloadHeader* payload) +{ + char dirPath[kMaxDataLength]; + strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); + + DIR *dp = opendir(dirPath); + + if (dp == nullptr) { + warnx("FTP: can't open path '%s'", dirPath); + return kErrFailErrno; + } + +#ifdef MAVLINK_FTP_DEBUG + warnx("FTP: list %s offset %d", dirPath, payload->offset); +#endif + + ErrorCode errorCode = kErrNone; + struct dirent entry, *result = nullptr; + unsigned offset = 0; + + // move to the requested offset + seekdir(dp, payload->offset); + + for (;;) { + // read the directory entry + if (readdir_r(dp, &entry, &result)) { + warnx("FTP: list %s readdir_r failure\n", dirPath); + errorCode = kErrFailErrno; + break; + } + + // no more entries? + if (result == nullptr) { + if (payload->offset != 0 && offset == 0) { + // User is requesting subsequent dir entries but there were none. This means the user asked + // to seek past EOF. + errorCode = kErrEOF; + } + // Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that + break; + } + + uint32_t fileSize = 0; + char buf[256]; + char direntType; + + // Determine the directory entry type + switch (entry.d_type) { +#ifdef __PX4_NUTTX + case DTYPE_FILE: +#else + case DT_REG: +#endif + // For files we get the file size as well + direntType = kDirentFile; + snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name); + struct stat st; + if (stat(buf, &st) == 0) { + fileSize = st.st_size; + } + break; +#ifdef __PX4_NUTTX + case DTYPE_DIRECTORY: +#else + case DT_DIR: +#endif + if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { + // Don't bother sending these back + direntType = kDirentSkip; + } else { + direntType = kDirentDir; + } + break; + default: + // We only send back file and diretory entries, skip everything else + direntType = kDirentSkip; + } + + if (direntType == kDirentSkip) { + // Skip send only dirent identifier + buf[0] = '\0'; + } else if (direntType == kDirentFile) { + // Files send filename and file length + snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize); + } else { + // Everything else just sends name + strncpy(buf, entry.d_name, sizeof(buf)); + buf[sizeof(buf)-1] = 0; + } + size_t nameLen = strlen(buf); + + // Do we have room for the name, the one char directory identifier and the null terminator? + if ((offset + nameLen + 2) > kMaxDataLength) { + break; + } + + // Move the data into the buffer + payload->data[offset++] = direntType; + strcpy((char *)&payload->data[offset], buf); +#ifdef MAVLINK_FTP_DEBUG + printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset-1]); +#endif + offset += nameLen + 1; + } + + closedir(dp); + payload->size = offset; + + return errorCode; +} + +/// @brief Responds to an Open command +MavlinkFTP::ErrorCode +MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag) +{ + int session_index = _find_unused_session(); + if (session_index < 0) { + warnx("FTP: Open failed - out of sessions\n"); + return kErrNoSessionsAvailable; + } + + char *filename = _data_as_cstring(payload); + + uint32_t fileSize = 0; + struct stat st; + if (stat(filename, &st) != 0) { + // fail only if requested open for read + if (oflag & O_RDONLY) + return kErrFailErrno; + else + st.st_size = 0; + } + fileSize = st.st_size; + + int fd = ::open(filename, oflag); + if (fd < 0) { + return kErrFailErrno; + } + _session_fds[session_index] = fd; + + payload->session = session_index; + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = fileSize; + + return kErrNone; +} + +/// @brief Responds to a Read command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRead(PayloadHeader* payload) +{ + int session_index = payload->session; + + if (!_valid_session(session_index)) { + return kErrInvalidSession; + } + + // Seek to the specified position +#ifdef MAVLINK_FTP_DEBUG + warnx("seek %d", payload->offset); +#endif + if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) { + // Unable to see to the specified location + warnx("seek fail"); + return kErrEOF; + } + + int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength); + if (bytes_read < 0) { + // Negative return indicates error other than eof + warnx("read fail %d", bytes_read); + return kErrFailErrno; + } + + payload->size = bytes_read; + + return kErrNone; +} + +/// @brief Responds to a Write command +MavlinkFTP::ErrorCode +MavlinkFTP::_workWrite(PayloadHeader* payload) +{ + int session_index = payload->session; + + if (!_valid_session(session_index)) { + return kErrInvalidSession; + } + + // Seek to the specified position +#ifdef MAVLINK_FTP_DEBUG + warnx("seek %d", payload->offset); +#endif + if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) { + // Unable to see to the specified location + warnx("seek fail"); + return kErrFailErrno; + } + + int bytes_written = ::write(_session_fds[session_index], &payload->data[0], payload->size); + if (bytes_written < 0) { + // Negative return indicates error other than eof + warnx("write fail %d", bytes_written); + return kErrFailErrno; + } + + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = bytes_written; + + return kErrNone; +} + +/// @brief Responds to a RemoveFile command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRemoveFile(PayloadHeader* payload) +{ + char file[kMaxDataLength]; + strncpy(file, _data_as_cstring(payload), kMaxDataLength); + + if (unlink(file) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + +/// @brief Responds to a TruncateFile command +MavlinkFTP::ErrorCode +MavlinkFTP::_workTruncateFile(PayloadHeader* payload) +{ + char file[kMaxDataLength]; + const char temp_file[] = "/fs/microsd/.trunc.tmp"; + strncpy(file, _data_as_cstring(payload), kMaxDataLength); + payload->size = 0; + + // emulate truncate(file, payload->offset) by + // copying to temp and overwrite with O_TRUNC flag. + + struct stat st; + if (stat(file, &st) != 0) { + return kErrFailErrno; + } + + if (!S_ISREG(st.st_mode)) { + errno = EISDIR; + return kErrFailErrno; + } + + // check perms allow us to write (not romfs) + if (!(st.st_mode & (S_IWUSR | S_IWGRP | S_IWOTH))) { + errno = EROFS; + return kErrFailErrno; + } + + if (payload->offset == (unsigned)st.st_size) { + // nothing to do + return kErrNone; + } + else if (payload->offset == 0) { + // 1: truncate all data + int fd = ::open(file, O_TRUNC | O_WRONLY); + if (fd < 0) { + return kErrFailErrno; + } + + ::close(fd); + return kErrNone; + } + else if (payload->offset > (unsigned)st.st_size) { + // 2: extend file + int fd = ::open(file, O_WRONLY); + if (fd < 0) { + return kErrFailErrno; + } + + if (lseek(fd, payload->offset - 1, SEEK_SET) < 0) { + ::close(fd); + return kErrFailErrno; + } + + bool ok = 1 == ::write(fd, "", 1); + ::close(fd); + + return (ok)? kErrNone : kErrFailErrno; + } + else { + // 3: truncate + if (_copy_file(file, temp_file, payload->offset) != 0) { + return kErrFailErrno; + } + if (_copy_file(temp_file, file, payload->offset) != 0) { + return kErrFailErrno; + } + if (::unlink(temp_file) != 0) { + return kErrFailErrno; + } + + return kErrNone; + } +} + +/// @brief Responds to a Terminate command +MavlinkFTP::ErrorCode +MavlinkFTP::_workTerminate(PayloadHeader* payload) +{ + if (!_valid_session(payload->session)) { + return kErrInvalidSession; + } + + ::close(_session_fds[payload->session]); + _session_fds[payload->session] = -1; + + payload->size = 0; + + return kErrNone; +} + +/// @brief Responds to a Reset command +MavlinkFTP::ErrorCode +MavlinkFTP::_workReset(PayloadHeader* payload) +{ + for (size_t i=0; isize = 0; + + return kErrNone; +} + +/// @brief Responds to a Rename command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRename(PayloadHeader* payload) +{ + char oldpath[kMaxDataLength]; + char newpath[kMaxDataLength]; + + char *ptr = _data_as_cstring(payload); + size_t oldpath_sz = strlen(ptr); + + if (oldpath_sz == payload->size) { + // no newpath + errno = EINVAL; + return kErrFailErrno; + } + + strncpy(oldpath, ptr, kMaxDataLength); + strncpy(newpath, ptr + oldpath_sz + 1, kMaxDataLength); + + if (rename(oldpath, newpath) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + +/// @brief Responds to a RemoveDirectory command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload) +{ + char dir[kMaxDataLength]; + strncpy(dir, _data_as_cstring(payload), kMaxDataLength); + + if (rmdir(dir) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + +/// @brief Responds to a CreateDirectory command +MavlinkFTP::ErrorCode +MavlinkFTP::_workCreateDirectory(PayloadHeader* payload) +{ + char dir[kMaxDataLength]; + strncpy(dir, _data_as_cstring(payload), kMaxDataLength); + + if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + +/// @brief Responds to a CalcFileCRC32 command +MavlinkFTP::ErrorCode +MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload) +{ + char file_buf[256]; + uint32_t checksum = 0; + ssize_t bytes_read; + strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength); + + int fd = ::open(file_buf, O_RDONLY); + if (fd < 0) { + return kErrFailErrno; + } + + do { + bytes_read = ::read(fd, file_buf, sizeof(file_buf)); + if (bytes_read < 0) { + int r_errno = errno; + ::close(fd); + errno = r_errno; + return kErrFailErrno; + } + + checksum = crc32part((uint8_t*)file_buf, bytes_read, checksum); + } while (bytes_read == sizeof(file_buf)); + + ::close(fd); + + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = checksum; + return kErrNone; +} + +/// @brief Returns true if the specified session is a valid open session +bool +MavlinkFTP::_valid_session(unsigned index) +{ + if ((index >= kMaxSession) || (_session_fds[index] < 0)) { + return false; + } + return true; +} + +/// @brief Returns an unused session index +int +MavlinkFTP::_find_unused_session(void) +{ + for (size_t i=0; isize < kMaxDataLength) { + payload->data[payload->size] = '\0'; + } else { + payload->data[kMaxDataLength - 1] = '\0'; + } + + // and return data + return (char *)&(payload->data[0]); +} + +/// @brief Returns a unused Request entry. NULL if none available. +MavlinkFTP::Request * +MavlinkFTP::_get_request(void) +{ + _lock_request_queue(); + Request* req = reinterpret_cast(dq_remfirst(&_request_queue)); + _unlock_request_queue(); + return req; +} + +/// @brief Locks a semaphore to provide exclusive access to the request queue +void +MavlinkFTP::_lock_request_queue(void) +{ + do {} + while (sem_wait(&_request_queue_sem) != 0); +} + +/// @brief Unlocks the semaphore providing exclusive access to the request queue +void +MavlinkFTP::_unlock_request_queue(void) +{ + sem_post(&_request_queue_sem); +} + +/// @brief Returns a no longer needed request to the queue +void +MavlinkFTP::_return_request(Request *req) +{ + _lock_request_queue(); + dq_addlast(&req->work.dq, &_request_queue); + _unlock_request_queue(); +} + +/// @brief Copy file (with limited space) +int +MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length) +{ + char buff[512]; + int src_fd = -1, dst_fd = -1; + int op_errno = 0; + + src_fd = ::open(src_path, O_RDONLY); + if (src_fd < 0) { + return -1; + } + + dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY, 0x0777); + if (dst_fd < 0) { + op_errno = errno; + ::close(src_fd); + errno = op_errno; + return -1; + } + + while (length > 0) { + ssize_t bytes_read, bytes_written; + size_t blen = (length > sizeof(buff))? sizeof(buff) : length; + + bytes_read = ::read(src_fd, buff, blen); + if (bytes_read == 0) { + // EOF + break; + } + else if (bytes_read < 0) { + warnx("cp: read"); + op_errno = errno; + break; + } + + bytes_written = ::write(dst_fd, buff, bytes_read); + if (bytes_written != bytes_read) { + warnx("cp: short write"); + op_errno = errno; + break; + } + + length -= bytes_written; + } + + ::close(src_fd); + ::close(dst_fd); + + errno = op_errno; + return (length > 0)? -1 : 0; +} diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c285bc4052..216abe954e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2015 Mark Charlebois. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -30,17 +30,9 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - -/** - * @file mavlink_main.h - * MAVLink 1.0 protocol interface definition. - * - * @author Lorenz Meier - * @author Anton Babushkin - */ - #pragma once +<<<<<<< HEAD #include #include #include @@ -413,3 +405,10 @@ private: Mavlink(const Mavlink&); Mavlink operator=(const Mavlink&); }; +======= +#ifdef __PX4_NUTTX +#include "mavlink_main_nuttx.h" +#else +#include "mavlink_main_linux.h" +#endif +>>>>>>> Support for building more modules with Linux diff --git a/src/modules/mavlink/mavlink_main_linux.cpp b/src/modules/mavlink/mavlink_main_linux.cpp new file mode 100644 index 0000000000..f2d8ea90f9 --- /dev/null +++ b/src/modules/mavlink/mavlink_main_linux.cpp @@ -0,0 +1,1795 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 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 + * 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 mavlink_main.cpp + * MAVLink 1.0 protocol implementation. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include /* isinf / isnan checks */ + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "mavlink_bridge_header.h" +#include "mavlink_main.h" +#include "mavlink_messages.h" +#include "mavlink_receiver.h" +#include "mavlink_rate_limiter.h" + +#ifndef MAVLINK_CRC_EXTRA +#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems +#endif + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#define DEFAULT_DEVICE_NAME "/dev/ttyS1" +#define MAX_DATA_RATE 20000 ///< max data rate in bytes/s +#define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate +#define FLOW_CONTROL_DISABLE_THRESHOLD 40 ///< picked so that some messages still would fit it. + +static Mavlink *_mavlink_instances = nullptr; + +static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; + +/** + * mavlink app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); + +extern mavlink_system_t mavlink_system; + +static void usage(void); + +Mavlink::Mavlink() : + CDev("mavlink-log", MAVLINK_LOG_DEVICE), + _device_name(DEFAULT_DEVICE_NAME), + _task_should_exit(false), + next(nullptr), + _instance_id(0), + _mavlink_fd(-1), + _task_running(false), + _hil_enabled(false), + _use_hil_gps(false), + _forward_externalsp(false), + _is_usb_uart(false), + _wait_to_transmit(false), + _received_messages(false), + _main_loop_delay(1000), + _subscriptions(nullptr), + _streams(nullptr), + _mission_manager(nullptr), + _parameters_manager(nullptr), + _mode(MAVLINK_MODE_NORMAL), + _channel(MAVLINK_COMM_0), + _logbuffer {}, + _total_counter(0), + _receive_thread {}, + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _ftp_on(false), + _uart_fd(-1), + _baudrate(57600), + _datarate(1000), + _datarate_events(500), + _rate_mult(1.0f), + _mavlink_param_queue_index(0), + mavlink_link_termination_allowed(false), + _subscribe_to_stream(nullptr), + _subscribe_to_stream_rate(0.0f), + _flow_control_enabled(true), + _last_write_success_time(0), + _last_write_try_time(0), + _bytes_tx(0), + _bytes_txerr(0), + _bytes_rx(0), + _bytes_timestamp(0), + _rate_tx(0.0f), + _rate_txerr(0.0f), + _rate_rx(0.0f), + _rstatus {}, + _message_buffer {}, + _message_buffer_mutex {}, + _send_mutex {}, + _param_initialized(false), + _param_system_id(0), + _param_component_id(0), + _param_system_type(MAV_TYPE_FIXED_WING), + _param_use_hil_gps(0), + _param_forward_externalsp(0), + _system_type(0), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) +{ + _instance_id = Mavlink::instance_count(); + + /* set channel according to instance id */ + switch (_instance_id) { + case 0: + _channel = MAVLINK_COMM_0; + break; + + case 1: + _channel = MAVLINK_COMM_1; + break; + + case 2: + _channel = MAVLINK_COMM_2; + break; + + case 3: + _channel = MAVLINK_COMM_3; + break; +#ifdef MAVLINK_COMM_4 + + case 4: + _channel = MAVLINK_COMM_4; + break; +#endif +#ifdef MAVLINK_COMM_5 + + case 5: + _channel = MAVLINK_COMM_5; + break; +#endif +#ifdef MAVLINK_COMM_6 + + case 6: + _channel = MAVLINK_COMM_6; + break; +#endif + + default: + px4_errx(1, "instance ID is out of range"); + break; + } + + _rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; +} + +Mavlink::~Mavlink() +{ + perf_free(_loop_perf); + perf_free(_txerr_perf); + + if (_task_running) { + /* task wakes up every 10ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + //TODO store main task handle in Mavlink instance to allow killing task + //task_delete(_mavlink_task); + break; + } + } while (_task_running); + } + + LL_DELETE(_mavlink_instances, this); +} + +void +Mavlink::count_txerr() +{ + perf_count(_txerr_perf); +} + +void +Mavlink::set_mode(enum MAVLINK_MODE mode) +{ + _mode = mode; +} + +int +Mavlink::instance_count() +{ + unsigned inst_index = 0; + Mavlink *inst; + + LL_FOREACH(::_mavlink_instances, inst) { + inst_index++; + } + + return inst_index; +} + +Mavlink * +Mavlink::get_instance(unsigned instance) +{ + Mavlink *inst; + unsigned inst_index = 0; + LL_FOREACH(::_mavlink_instances, inst) { + if (instance == inst_index) { + return inst; + } + + inst_index++; + } + + return nullptr; +} + +Mavlink * +Mavlink::get_instance_for_device(const char *device_name) +{ + Mavlink *inst; + + LL_FOREACH(::_mavlink_instances, inst) { + if (strcmp(inst->_device_name, device_name) == 0) { + return inst; + } + } + + return nullptr; +} + +int +Mavlink::destroy_all_instances() +{ + /* start deleting from the end */ + Mavlink *inst_to_del = nullptr; + Mavlink *next_inst = ::_mavlink_instances; + + unsigned iterations = 0; + + warnx("waiting for instances to stop"); + + while (next_inst != nullptr) { + inst_to_del = next_inst; + next_inst = inst_to_del->next; + + /* set flag to stop thread and wait for all threads to finish */ + inst_to_del->_task_should_exit = true; + + while (inst_to_del->_task_running) { + printf("."); + fflush(stdout); + usleep(10000); + iterations++; + + if (iterations > 1000) { + warnx("ERROR: Couldn't stop all mavlink instances."); + return ERROR; + } + } + } + + printf("\n"); + warnx("all instances stopped"); + return OK; +} + +int +Mavlink::get_status_all_instances() +{ + Mavlink *inst = ::_mavlink_instances; + + unsigned iterations = 0; + + while (inst != nullptr) { + + printf("\ninstance #%u:\n", iterations); + inst->display_status(); + + /* move on */ + inst = inst->next; + iterations++; + } + + /* return an error if there are no instances */ + return (iterations == 0); +} + +bool +Mavlink::instance_exists(const char *device_name, Mavlink *self) +{ + Mavlink *inst = ::_mavlink_instances; + + while (inst != nullptr) { + + /* don't compare with itself */ + if (inst != self && !strcmp(device_name, inst->_device_name)) { + return true; + } + + inst = inst->next; + } + + return false; +} + +void +Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) +{ + + Mavlink *inst; + LL_FOREACH(_mavlink_instances, inst) { + if (inst != self) { + + /* if not in normal mode, we are an onboard link + * onboard links should only pass on messages from the same system ID */ + if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) { + inst->pass_message(msg); + } + } + } +} + +int +Mavlink::get_uart_fd(unsigned index) +{ + Mavlink *inst = get_instance(index); + + if (inst) { + return inst->get_uart_fd(); + } + + return -1; +} + +int +Mavlink::get_uart_fd() +{ + return _uart_fd; +} + +int +Mavlink::get_instance_id() +{ + return _instance_id; +} + +mavlink_channel_t +Mavlink::get_channel() +{ + return _channel; +} + +/**************************************************************************** + * MAVLink text message logger + ****************************************************************************/ + +int +Mavlink::ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg) +{ + switch (cmd) { + case (int)MAVLINK_IOC_SEND_TEXT_INFO: + case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: + case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { + + const char *txt = (const char *)arg; + struct mavlink_logmessage msg; + strncpy(msg.text, txt, sizeof(msg.text)); + + switch (cmd) { + case MAVLINK_IOC_SEND_TEXT_INFO: + msg.severity = MAV_SEVERITY_INFO; + break; + + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + msg.severity = MAV_SEVERITY_CRITICAL; + break; + + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + msg.severity = MAV_SEVERITY_EMERGENCY; + break; + + default: + msg.severity = MAV_SEVERITY_INFO; + break; + } + + Mavlink *inst; + LL_FOREACH(_mavlink_instances, inst) { + if (!inst->_task_should_exit) { + mavlink_logbuffer_write(&inst->_logbuffer, &msg); + inst->_total_counter++; + } + } + + return OK; + } + + default: + return ENOTTY; + } +} + +void Mavlink::mavlink_update_system(void) +{ + if (!_param_initialized) { + _param_system_id = param_find("MAV_SYS_ID"); + _param_component_id = param_find("MAV_COMP_ID"); + _param_system_type = param_find("MAV_TYPE"); + _param_use_hil_gps = param_find("MAV_USEHILGPS"); + _param_forward_externalsp = param_find("MAV_FWDEXTSP"); + } + + /* update system and component id */ + int32_t system_id; + param_get(_param_system_id, &system_id); + + int32_t component_id; + param_get(_param_component_id, &component_id); + + + /* only allow system ID and component ID updates + * after reboot - not during operation */ + if (!_param_initialized) { + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } + + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + _param_initialized = true; + } + + /* warn users that they need to reboot to take this + * into effect + */ + if (system_id != mavlink_system.sysid) { + send_statustext_critical("Save params and reboot to change SYSID"); + } + + if (component_id != mavlink_system.compid) { + send_statustext_critical("Save params and reboot to change COMPID"); + } + + int32_t system_type; + param_get(_param_system_type, &system_type); + + if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { + _system_type = system_type; + } + + int32_t use_hil_gps; + param_get(_param_use_hil_gps, &use_hil_gps); + + _use_hil_gps = (bool)use_hil_gps; + + int32_t forward_externalsp; + param_get(_param_forward_externalsp, &forward_externalsp); + + _forward_externalsp = (bool)forward_externalsp; +} + +int Mavlink::get_system_id() +{ + return mavlink_system.sysid; +} + +int Mavlink::get_component_id() +{ + return mavlink_system.compid; +} + +int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) +{ + /* process baud rate */ + int speed; + + switch (baud) { + case 0: speed = B0; break; + + case 50: speed = B50; break; + + case 75: speed = B75; break; + + case 110: speed = B110; break; + + case 134: speed = B134; break; + + case 150: speed = B150; break; + + case 200: speed = B200; break; + + case 300: speed = B300; break; + + case 600: speed = B600; break; + + case 1200: speed = B1200; break; + + case 1800: speed = B1800; break; + + case 2400: speed = B2400; break; + + case 4800: speed = B4800; break; + + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + + case 460800: speed = B460800; break; + + case 921600: speed = B921600; break; + + default: + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", + baud); + return -EINVAL; + } + + /* open uart */ + _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); + + if (_uart_fd < 0) { + return _uart_fd; + } + + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + *is_usb = false; + + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) { + warnx("ERR GET CONF %s: %d\n", uart_name, termios_state); + ::close(_uart_fd); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(_uart_fd, &uart_config); + + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* USB serial is indicated by /dev/ttyACM0*/ + if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) { + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state); + ::close(_uart_fd); + return -1; + } + + } + + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { + warnx("ERR SET CONF %s\n", uart_name); + ::close(_uart_fd); + return -1; + } + + if (!_is_usb_uart) { + /* + * Setup hardware flow control. If the port has no RTS pin this call will fail, + * which is not an issue, but requires a separate call so we can fail silently. + */ + (void)tcgetattr(_uart_fd, &uart_config); +#ifdef CRTS_IFLOW + uart_config.c_cflag |= CRTS_IFLOW; +#else + uart_config.c_cflag |= CRTSCTS; +#endif + (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); + + /* setup output flow control */ + if (enable_flow_control(true)) { + warnx("hardware flow control not supported"); + } + + } else { + _flow_control_enabled = false; + } + + return _uart_fd; +} + +int +Mavlink::enable_flow_control(bool enabled) +{ + // We can't do this on USB - skip + if (_is_usb_uart) { + return OK; + } + + struct termios uart_config; + + int ret = tcgetattr(_uart_fd, &uart_config); + + if (enabled) { + uart_config.c_cflag |= CRTSCTS; + + } else { + uart_config.c_cflag &= ~CRTSCTS; + } + + ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); + + if (!ret) { + _flow_control_enabled = enabled; + } + + return ret; +} + +int +Mavlink::set_hil_enabled(bool hil_enabled) +{ + int ret = OK; + + /* enable HIL */ + if (hil_enabled && !_hil_enabled) { + _hil_enabled = true; + configure_stream("HIL_CONTROLS", 200.0f); + } + + /* disable HIL */ + if (!hil_enabled && _hil_enabled) { + _hil_enabled = false; + configure_stream("HIL_CONTROLS", 0.0f); + + } else { + ret = ERROR; + } + + return ret; +} + +unsigned +Mavlink::get_free_tx_buf() +{ + /* + * Check if the OS buffer is full and disable HW + * flow control if it continues to be full + */ + int buf_free = 0; + +// No FIONWRITE on Linux +#ifndef __PX4_LINUX + (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); +#endif + + if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) { + /* Disable hardware flow control: + * if no successful write since a defined time + * and if the last try was not the last successful write + */ + if (_last_write_try_time != 0 && + hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL && + _last_write_success_time != _last_write_try_time) { + warnx("DISABLING HARDWARE FLOW CONTROL"); + enable_flow_control(false); + } + } + + return buf_free; +} + +void +Mavlink::send_message(const uint8_t msgid, const void *msg) +{ + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } + + pthread_mutex_lock(&_send_mutex); + + unsigned buf_free = get_free_tx_buf(); + + uint8_t payload_len = mavlink_message_lengths[msgid]; + unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + _last_write_try_time = hrt_absolute_time(); + + /* check if there is space in the buffer, let it overflow else */ + if (buf_free < packet_len) { + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + pthread_mutex_unlock(&_send_mutex); + return; + } + + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + /* header */ + buf[0] = MAVLINK_STX; + buf[1] = payload_len; + /* use mavlink's internal counter for the TX seq */ + buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++; + buf[3] = mavlink_system.sysid; + buf[4] = mavlink_system.compid; + buf[5] = msgid; + + /* payload */ + memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len); + + /* checksum */ + uint16_t checksum; + crc_init(&checksum); + crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len); + crc_accumulate(mavlink_message_crcs[msgid], &checksum); + + buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); + + /* send message to UART */ + ssize_t ret = ::write(_uart_fd, buf, packet_len); + + if (ret != (int) packet_len) { + count_txerr(); + count_txerrbytes(packet_len); + + } else { + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); + } + + pthread_mutex_unlock(&_send_mutex); +} + +void +Mavlink::resend_message(mavlink_message_t *msg) +{ + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (!should_transmit()) { + return; + } + + pthread_mutex_lock(&_send_mutex); + + unsigned buf_free = get_free_tx_buf(); + + _last_write_try_time = hrt_absolute_time(); + + unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + /* check if there is space in the buffer, let it overflow else */ + if (buf_free < packet_len) { + /* no enough space in buffer to send */ + count_txerr(); + count_txerrbytes(packet_len); + pthread_mutex_unlock(&_send_mutex); + return; + } + + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + + /* header and payload */ + memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len); + + /* checksum */ + buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); + buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); + + /* send message to UART */ + ssize_t ret = ::write(_uart_fd, buf, packet_len); + + if (ret != (int) packet_len) { + count_txerr(); + count_txerrbytes(packet_len); + + } else { + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); + } + + pthread_mutex_unlock(&_send_mutex); +} + +void +Mavlink::handle_message(const mavlink_message_t *msg) +{ + /* handle packet with mission manager */ + _mission_manager->handle_message(msg); + + /* handle packet with parameter component */ + _parameters_manager->handle_message(msg); + + if (get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(msg, this); + } +} + +void +Mavlink::send_statustext_info(const char *string) +{ + send_statustext(MAV_SEVERITY_INFO, string); +} + +void +Mavlink::send_statustext_critical(const char *string) +{ + send_statustext(MAV_SEVERITY_CRITICAL, string); +} + +void +Mavlink::send_statustext_emergency(const char *string) +{ + send_statustext(MAV_SEVERITY_EMERGENCY, string); +} + +void +Mavlink::send_statustext(unsigned char severity, const char *string) +{ + struct mavlink_logmessage logmsg; + strncpy(logmsg.text, string, sizeof(logmsg.text)); + logmsg.severity = severity; + + mavlink_logbuffer_write(&_logbuffer, &logmsg); +} + +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance) +{ + /* check if already subscribed to this topic */ + MavlinkOrbSubscription *sub; + + LL_FOREACH(_subscriptions, sub) { + if (sub->get_topic() == topic && sub->get_instance() == instance) { + /* already subscribed */ + return sub; + } + } + + /* add new subscription */ + MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance); + + LL_APPEND(_subscriptions, sub_new); + + return sub_new; +} + +unsigned int +Mavlink::interval_from_rate(float rate) +{ + return (rate > 0.0f) ? (1000000.0f / rate) : 0; +} + +int +Mavlink::configure_stream(const char *stream_name, const float rate) +{ + /* calculate interval in us, 0 means disabled stream */ + unsigned int interval = interval_from_rate(rate); + + /* search if stream exists */ + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + if (strcmp(stream_name, stream->get_name()) == 0) { + if (interval > 0) { + /* set new interval */ + stream->set_interval(interval); + + } else { + /* delete stream */ + LL_DELETE(_streams, stream); + delete stream; + } + + return OK; + } + } + + if (interval == 0) { + /* stream was not active and is requested to be disabled, do nothing */ + return OK; + } + + /* search for stream with specified name in supported streams list */ + for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + + if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { + /* create new instance */ + stream = streams_list[i]->new_instance(this); + stream->set_interval(interval); + LL_APPEND(_streams, stream); + + return OK; + } + } + + /* if we reach here, the stream list does not contain the stream */ + warnx("stream %s not found", stream_name); + + return ERROR; +} + +void +Mavlink::adjust_stream_rates(const float multiplier) +{ + /* do not allow to push us to zero */ + if (multiplier < 0.01f) { + return; + } + + /* search if stream exists */ + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + /* set new interval */ + unsigned interval = stream->get_interval(); + interval /= multiplier; + + /* allow max ~600 Hz */ + if (interval < 1600) { + interval = 1600; + } + + /* set new interval */ + stream->set_interval(interval * multiplier); + } +} + +void +Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) +{ + /* orb subscription must be done from the main thread, + * set _subscribe_to_stream and _subscribe_to_stream_rate fields + * which polled in mavlink main loop */ + if (!_task_should_exit) { + /* wait for previous subscription completion */ + while (_subscribe_to_stream != nullptr) { + usleep(MAIN_LOOP_DELAY / 2); + } + + /* copy stream name */ + unsigned n = strlen(stream_name) + 1; + char *s = new char[n]; + strcpy(s, stream_name); + + /* set subscription task */ + _subscribe_to_stream_rate = rate; + _subscribe_to_stream = s; + + /* wait for subscription */ + do { + usleep(MAIN_LOOP_DELAY / 2); + } while (_subscribe_to_stream != nullptr); + + delete s; + } +} + +int +Mavlink::message_buffer_init(int size) +{ + + _message_buffer.size = size; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + _message_buffer.data = (char *)malloc(_message_buffer.size); + + int ret; + + if (_message_buffer.data == 0) { + ret = ERROR; + _message_buffer.size = 0; + + } else { + ret = OK; + } + + return ret; +} + +void +Mavlink::message_buffer_destroy() +{ + _message_buffer.size = 0; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + free(_message_buffer.data); +} + +int +Mavlink::message_buffer_count() +{ + int n = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (n < 0) { + n += _message_buffer.size; + } + + return n; +} + +int +Mavlink::message_buffer_is_empty() +{ + return _message_buffer.read_ptr == _message_buffer.write_ptr; +} + + +bool +Mavlink::message_buffer_write(const void *ptr, int size) +{ + // bytes available to write + int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; + + if (available < 0) { + available += _message_buffer.size; + } + + if (size > available) { + // buffer overflow + return false; + } + + char *c = (char *) ptr; + int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer + + if (n < size) { + // message goes over end of the buffer + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n); + _message_buffer.write_ptr = 0; + + } else { + n = 0; + } + + // now: n = bytes already written + int p = size - n; // number of bytes to write + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p); + _message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size; + return true; +} + +int +Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part) +{ + // bytes available to read + int available = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (available == 0) { + return 0; // buffer is empty + } + + int n = 0; + + if (available > 0) { + // read pointer is before write pointer, all available bytes can be read + n = available; + *is_part = false; + + } else { + // read pointer is after write pointer, read bytes from read_ptr to end of the buffer + n = _message_buffer.size - _message_buffer.read_ptr; + *is_part = _message_buffer.write_ptr > 0; + } + + *ptr = &(_message_buffer.data[_message_buffer.read_ptr]); + return n; +} + +void +Mavlink::message_buffer_mark_read(int n) +{ + _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size; +} + +void +Mavlink::pass_message(const mavlink_message_t *msg) +{ + if (_passing_on) { + /* size is 8 bytes plus variable payload */ + int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; + pthread_mutex_lock(&_message_buffer_mutex); + message_buffer_write(msg, size); + pthread_mutex_unlock(&_message_buffer_mutex); + } +} + +float +Mavlink::get_rate_mult() +{ + return _rate_mult; +} + +void +Mavlink::update_rate_mult() +{ + float const_rate = 0.0f; + float rate = 0.0f; + + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + if (stream->const_rate()) { + const_rate += stream->get_size() * 1000000.0f / stream->get_interval(); + + } else { + rate += stream->get_size() * 1000000.0f / stream->get_interval(); + } + } + + /* don't scale up rates, only scale down if needed */ + _rate_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate); +} + +int +Mavlink::task_main(int argc, char *argv[]) +{ + int ch; + _baudrate = 57600; + _datarate = 0; + _mode = MAVLINK_MODE_NORMAL; + + /* work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + + /* don't exit from getopt loop to leave getopt global variables in consistent state, + * set error flag instead */ + bool err_flag = false; + + while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) { + switch (ch) { + case 'b': + _baudrate = strtoul(optarg, NULL, 10); + + if (_baudrate < 9600 || _baudrate > 921600) { + warnx("invalid baud rate '%s'", optarg); + err_flag = true; + } + + break; + + case 'r': + _datarate = strtoul(optarg, NULL, 10); + + if (_datarate < 10 || _datarate > MAX_DATA_RATE) { + warnx("invalid data rate '%s'", optarg); + err_flag = true; + } + + break; + + case 'd': + _device_name = optarg; + break; + +// case 'e': +// mavlink_link_termination_allowed = true; +// break; + + case 'm': + if (strcmp(optarg, "custom") == 0) { + _mode = MAVLINK_MODE_CUSTOM; + + } else if (strcmp(optarg, "camera") == 0) { + // left in here for compatibility + _mode = MAVLINK_MODE_ONBOARD; + } else if (strcmp(optarg, "onboard") == 0) { + _mode = MAVLINK_MODE_ONBOARD; + } + + break; + + case 'f': + _forwarding_on = true; + break; + + case 'p': + _passing_on = true; + break; + + case 'v': + _verbose = true; + break; + + case 'w': + _wait_to_transmit = true; + break; + + case 'x': + _ftp_on = true; + break; + + default: + err_flag = true; + break; + } + } + + if (err_flag) { + usage(); + return ERROR; + } + + if (_datarate == 0) { + /* convert bits to bytes and use 1/2 of bandwidth by default */ + _datarate = _baudrate / 20; + } + + if (_datarate > MAX_DATA_RATE) { + _datarate = MAX_DATA_RATE; + } + + if (Mavlink::instance_exists(_device_name, this)) { + warnx("%s already running", _device_name); + return ERROR; + } + + warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); + + /* flush stdout in case MAVLink is about to take it over */ + fflush(stdout); + + struct termios uart_config_original; + + /* default values for arguments */ + _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &_is_usb_uart); + + if (_uart_fd < 0) { + warn("could not open %s", _device_name); + return ERROR; + } + + /* initialize send mutex */ + pthread_mutex_init(&_send_mutex, NULL); + + /* initialize mavlink text message buffering */ + mavlink_logbuffer_init(&_logbuffer, 5); + + /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ + if (_passing_on || _ftp_on) { + /* initialize message buffer if multiplexing is on or its needed for FTP. + * make space for two messages plus off-by-one space as we use the empty element + * marker ring buffer approach. + */ + if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { + px4_errx(1, "msg buf:"); + } + + /* initialize message buffer mutex */ + pthread_mutex_init(&_message_buffer_mutex, NULL); + } + + /* create the device node that's used for sending text log messages, etc. */ + register_driver(MAVLINK_LOG_DEVICE, NULL); + + /* initialize logging device */ + _mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); + + /* Initialize system properties */ + mavlink_update_system(); + + /* start the MAVLink receiver */ + _receive_thread = MavlinkReceiver::receive_start(this); + + _task_running = true; + + MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); + uint64_t param_time = 0; + MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); + uint64_t status_time = 0; + + struct vehicle_status_s status; + status_sub->update(&status_time, &status); + + /* add default streams depending on mode */ + + /* HEARTBEAT is constant rate stream, rate never adjusted */ + configure_stream("HEARTBEAT", 1.0f); + + /* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */ + configure_stream("STATUSTEXT", 20.0f); + + /* COMMAND_LONG stream: use high rate to avoid commands skipping */ + configure_stream("COMMAND_LONG", 100.0f); + + /* PARAM_VALUE stream */ + _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); + _parameters_manager->set_interval(interval_from_rate(30.0f)); + LL_APPEND(_streams, _parameters_manager); + + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(10.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + + switch (_mode) { + case MAVLINK_MODE_NORMAL: + configure_stream("SYS_STATUS", 1.0f); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); + configure_stream("HIGHRES_IMU", 1.0f); + configure_stream("ATTITUDE", 10.0f); + configure_stream("VFR_HUD", 8.0f); + configure_stream("GPS_RAW_INT", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 3.0f); + configure_stream("LOCAL_POSITION_NED", 3.0f); + configure_stream("RC_CHANNELS_RAW", 1.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); + configure_stream("ATTITUDE_TARGET", 3.0f); + configure_stream("DISTANCE_SENSOR", 0.5f); + configure_stream("OPTICAL_FLOW_RAD", 5.0f); + break; + + case MAVLINK_MODE_ONBOARD: + configure_stream("SYS_STATUS", 1.0f); + configure_stream("ATTITUDE", 50.0f); + configure_stream("GLOBAL_POSITION_INT", 50.0f); + configure_stream("LOCAL_POSITION_NED", 30.0f); + configure_stream("CAMERA_CAPTURE", 2.0f); + configure_stream("ATTITUDE_TARGET", 10.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f); + configure_stream("DISTANCE_SENSOR", 10.0f); + configure_stream("OPTICAL_FLOW_RAD", 10.0f); + configure_stream("VFR_HUD", 10.0f); + configure_stream("SYSTEM_TIME", 1.0f); + configure_stream("TIMESYNC", 10.0f); + configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); + break; + + default: + break; + } + + /* set main loop delay depending on data rate to minimize CPU overhead */ + _main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate; + + /* now the instance is fully initialized and we can bump the instance count */ + LL_APPEND(_mavlink_instances, this); + + while (!_task_should_exit) { + /* main loop */ + usleep(_main_loop_delay); + + perf_begin(_loop_perf); + + hrt_abstime t = hrt_absolute_time(); + + update_rate_mult(); + + if (param_sub->update(¶m_time, nullptr)) { + /* parameters updated */ + mavlink_update_system(); + } + + if (status_sub->update(&status_time, &status)) { + /* switch HIL mode if required */ + set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON); + } + + /* check for requested subscriptions */ + if (_subscribe_to_stream != nullptr) { + if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { + if (_subscribe_to_stream_rate > 0.0f) { + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, + (double)_subscribe_to_stream_rate); + + } else { + warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); + } + + } else { + warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name); + } + + _subscribe_to_stream = nullptr; + } + + /* update streams */ + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + stream->update(t); + } + + /* pass messages from other UARTs or FTP worker */ + if (_passing_on || _ftp_on) { + + bool is_part; + uint8_t *read_ptr; + uint8_t *write_ptr; + + pthread_mutex_lock(&_message_buffer_mutex); + int available = message_buffer_get_ptr((void **)&read_ptr, &is_part); + pthread_mutex_unlock(&_message_buffer_mutex); + + if (available > 0) { + // Reconstruct message from buffer + + mavlink_message_t msg; + write_ptr = (uint8_t *)&msg; + + // Pull a single message from the buffer + size_t read_count = available; + + if (read_count > sizeof(mavlink_message_t)) { + read_count = sizeof(mavlink_message_t); + } + + memcpy(write_ptr, read_ptr, read_count); + + // We hold the mutex until after we complete the second part of the buffer. If we don't + // we may end up breaking the empty slot overflow detection semantics when we mark the + // possibly partial read below. + pthread_mutex_lock(&_message_buffer_mutex); + + message_buffer_mark_read(read_count); + + /* write second part of buffer if there is some */ + if (is_part && read_count < sizeof(mavlink_message_t)) { + write_ptr += read_count; + available = message_buffer_get_ptr((void **)&read_ptr, &is_part); + read_count = sizeof(mavlink_message_t) - read_count; + memcpy(write_ptr, read_ptr, read_count); + message_buffer_mark_read(available); + } + + pthread_mutex_unlock(&_message_buffer_mutex); + + resend_message(&msg); + } + } + + /* update TX/RX rates*/ + if (t > _bytes_timestamp + 1000000) { + if (_bytes_timestamp != 0) { + float dt = (t - _bytes_timestamp) / 1000.0f; + _rate_tx = _bytes_tx / dt; + _rate_txerr = _bytes_txerr / dt; + _rate_rx = _bytes_rx / dt; + _bytes_tx = 0; + _bytes_txerr = 0; + _bytes_rx = 0; + } + + _bytes_timestamp = t; + } + + perf_end(_loop_perf); + } + + delete _subscribe_to_stream; + _subscribe_to_stream = nullptr; + + /* delete streams */ + MavlinkStream *stream_to_del = nullptr; + MavlinkStream *stream_next = _streams; + + while (stream_next != nullptr) { + stream_to_del = stream_next; + stream_next = stream_to_del->next; + delete stream_to_del; + } + + _streams = nullptr; + + /* delete subscriptions */ + MavlinkOrbSubscription *sub_to_del = nullptr; + MavlinkOrbSubscription *sub_next = _subscriptions; + + while (sub_next != nullptr) { + sub_to_del = sub_next; + sub_next = sub_to_del->next; + delete sub_to_del; + } + + _subscriptions = nullptr; + + /* wait for threads to complete */ + pthread_join(_receive_thread, NULL); + + /* reset the UART flags to original state */ + tcsetattr(_uart_fd, TCSANOW, &uart_config_original); + + /* close UART */ + ::close(_uart_fd); + + /* close mavlink logging device */ + px4_close(_mavlink_fd); + + if (_passing_on || _ftp_on) { + message_buffer_destroy(); + pthread_mutex_destroy(&_message_buffer_mutex); + } + + /* destroy log buffer */ + mavlink_logbuffer_destroy(&_logbuffer); + + warnx("exiting"); + _task_running = false; + + return OK; +} + +int Mavlink::start_helper(int argc, char *argv[]) +{ + /* create the instance in task context */ + Mavlink *instance = new Mavlink(); + + int res; + + if (!instance) { + + /* out of memory */ + res = -ENOMEM; + warnx("OUT OF MEM"); + + } else { + /* this will actually only return once MAVLink exits */ + res = instance->task_main(argc, argv); + + /* delete instance on main thread end */ + delete instance; + } + + return res; +} + +int +Mavlink::start(int argc, char *argv[]) +{ + // Wait for the instance count to go up one + // before returning to the shell + int ic = Mavlink::instance_count(); + + // Instantiate thread + char buf[24]; + sprintf(buf, "mavlink_if%d", ic); + + // This is where the control flow splits + // between the starting task and the spawned + // task - start_helper() only returns + // when the started task exits. + px4_task_spawn_cmd(buf, + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2400, + (px4_main_t)&Mavlink::start_helper, + (char * const *)argv); + + // Ensure that this shell command + // does not return before the instance + // is fully initialized. As this is also + // the only path to create a new instance, + // this is effectively a lock on concurrent + // instance starting. XXX do a real lock. + + // Sleep 500 us between each attempt + const unsigned sleeptime = 500; + + // Wait 100 ms max for the startup. + const unsigned limit = 100 * 1000 / sleeptime; + + unsigned count = 0; + + while (ic == Mavlink::instance_count() && count < limit) { + ::usleep(sleeptime); + count++; + } + + return OK; +} + +void +Mavlink::display_status() +{ + + if (_rstatus.heartbeat_time > 0) { + printf("\tGCS heartbeat:\t%lu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + } + + printf("\tmavlink chan: #%u\n", _channel); + + if (_rstatus.timestamp > 0) { + + printf("\ttype:\t\t"); + + switch (_rstatus.type) { + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + printf("3DR RADIO\n"); + break; + + default: + printf("UNKNOWN RADIO\n"); + break; + } + + printf("\trssi:\t\t%d\n", _rstatus.rssi); + printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi); + printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf); + printf("\tnoise:\t\t%d\n", _rstatus.noise); + printf("\tremote noise:\t%u\n", _rstatus.remote_noise); + printf("\trx errors:\t%u\n", _rstatus.rxerrors); + printf("\tfixed:\t\t%u\n", _rstatus.fixed); + + } else { + printf("\tno telem status.\n"); + } + + printf("\trates:\n"); + printf("\ttx: %.3f kB/s\n", (double)_rate_tx); + printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); + printf("\trx: %.3f kB/s\n", (double)_rate_rx); + printf("\trate mult: %.3f\n", (double)_rate_mult); +} + +int +Mavlink::stream_command(int argc, char *argv[]) +{ + const char *device_name = DEFAULT_DEVICE_NAME; + float rate = -1.0f; + const char *stream_name = nullptr; + + argc -= 2; + argv += 2; + + /* don't exit from getopt loop to leave getopt global variables in consistent state, + * set error flag instead */ + bool err_flag = false; + + int i = 0; + + while (i < argc) { + + if (0 == strcmp(argv[i], "-r") && i < argc - 1) { + rate = strtod(argv[i + 1], nullptr); + + if (rate < 0.0f) { + err_flag = true; + } + + i++; + + } else if (0 == strcmp(argv[i], "-d") && i < argc - 1) { + device_name = argv[i + 1]; + i++; + + } else if (0 == strcmp(argv[i], "-s") && i < argc - 1) { + stream_name = argv[i + 1]; + i++; + + } else { + err_flag = true; + } + + i++; + } + + if (!err_flag && rate >= 0.0f && stream_name != nullptr) { + Mavlink *inst = get_instance_for_device(device_name); + + if (inst != nullptr) { + inst->configure_stream_threadsafe(stream_name, rate); + + } else { + + // If the link is not running we should complain, but not fall over + // because this is so easy to get wrong and not fatal. Warning is sufficient. + px4_errx(0, "mavlink for device %s is not running", device_name); + } + + } else { + px4_errx(1, "usage: mavlink stream [-d device] -s stream -r rate"); + } + + return OK; +} + +static void usage() +{ + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); +} + +int mavlink_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return 1; + } + + if (!strcmp(argv[1], "start")) { + return Mavlink::start(argc, argv); + + } else if (!strcmp(argv[1], "stop")) { + warnx("mavlink stop is deprecated, use stop-all instead"); + usage(); + return 1; + + } else if (!strcmp(argv[1], "stop-all")) { + return Mavlink::destroy_all_instances(); + + } else if (!strcmp(argv[1], "status")) { + return Mavlink::get_status_all_instances(); + + } else if (!strcmp(argv[1], "stream")) { + return Mavlink::stream_command(argc, argv); + + } else { + usage(); + return 1; + } + + return 0; +} diff --git a/src/modules/mavlink/mavlink_main_linux.h b/src/modules/mavlink/mavlink_main_linux.h new file mode 100644 index 0000000000..a785c48f3c --- /dev/null +++ b/src/modules/mavlink/mavlink_main_linux.h @@ -0,0 +1,417 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 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 mavlink_main_linux.h + * MAVLink 1.0 protocol interface definition. + * + * @author Lorenz Meier + * @author Anton Babushkin + * @author Mark Charlebois + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "mavlink_bridge_header.h" +#include "mavlink_orb_subscription.h" +#include "mavlink_stream.h" +#include "mavlink_messages.h" +#include "mavlink_mission.h" +#include "mavlink_parameters.h" + +class Mavlink : public device::CDev +{ + +public: + /** + * Constructor + */ + Mavlink(); + + /** + * Destructor, also kills the mavlinks task. + */ + ~Mavlink(); + + /** + * Start the mavlink task. + * + * @return OK on success. + */ + static int start(int argc, char *argv[]); + + /** + * Display the mavlink status. + */ + void display_status(); + + static int stream_command(int argc, char *argv[]); + + static int instance_count(); + + static Mavlink *new_instance(); + + static Mavlink *get_instance(unsigned instance); + + static Mavlink *get_instance_for_device(const char *device_name); + + static int destroy_all_instances(); + + static int get_status_all_instances(); + + static bool instance_exists(const char *device_name, Mavlink *self); + + static void forward_message(const mavlink_message_t *msg, Mavlink *self); + + static int get_uart_fd(unsigned index); + + int get_uart_fd(); + + /** + * Get the MAVLink system id. + * + * @return The system ID of this vehicle + */ + int get_system_id(); + + /** + * Get the MAVLink component id. + * + * @return The component ID of this vehicle + */ + int get_component_id(); + + const char *_device_name; + + enum MAVLINK_MODE { + MAVLINK_MODE_NORMAL = 0, + MAVLINK_MODE_CUSTOM, + MAVLINK_MODE_ONBOARD + }; + + void set_mode(enum MAVLINK_MODE); + enum MAVLINK_MODE get_mode() { return _mode; } + + bool get_hil_enabled() { return _hil_enabled; } + + bool get_use_hil_gps() { return _use_hil_gps; } + + bool get_forward_externalsp() { return _forward_externalsp; } + + bool get_flow_control_enabled() { return _flow_control_enabled; } + + bool get_forwarding_on() { return _forwarding_on; } + + static int start_helper(int argc, char *argv[]); + + /** + * Handle parameter related messages. + */ + void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + + void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + + /** + * Enable / disable Hardware in the Loop simulation mode. + * + * @param hil_enabled The new HIL enable/disable state. + * @return OK if the HIL state changed, ERROR if the + * requested change could not be made or was + * redundant. + */ + int set_hil_enabled(bool hil_enabled); + + void send_message(const uint8_t msgid, const void *msg); + + /** + * Resend message as is, don't change sequence number and CRC. + */ + void resend_message(mavlink_message_t *msg); + + void handle_message(const mavlink_message_t *msg); + + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0); + + int get_instance_id(); + + /** + * Enable / disable hardware flow control. + * + * @param enabled True if hardware flow control should be enabled + */ + int enable_flow_control(bool enabled); + + mavlink_channel_t get_channel(); + + void configure_stream_threadsafe(const char *stream_name, float rate); + + bool _task_should_exit; /**< if true, mavlink task should exit */ + + int get_mavlink_fd() { return _mavlink_fd; } + + /** + * Send a status text with loglevel INFO + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_info(const char *string); + + /** + * Send a status text with loglevel CRITICAL + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_critical(const char *string); + + /** + * Send a status text with loglevel EMERGENCY + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_emergency(const char *string); + + /** + * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent + * only on this mavlink connection. Useful for reporting communication specific, not system-wide info + * only to client interested in it. Message will be not sent immediately but queued in buffer as + * for mavlink_log_xxx(). + * + * @param string the message to send (will be capped by mavlink max string length) + * @param severity the log level + */ + void send_statustext(unsigned char severity, const char *string); + + MavlinkStream * get_streams() const { return _streams; } + + float get_rate_mult(); + + /* Functions for waiting to start transmission until message received. */ + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + + bool message_buffer_write(const void *ptr, int size); + + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } + + /** + * Count a transmision error + */ + void count_txerr(); + + /** + * Count transmitted bytes + */ + void count_txbytes(unsigned n) { _bytes_tx += n; }; + + /** + * Count bytes not transmitted because of errors + */ + void count_txerrbytes(unsigned n) { _bytes_txerr += n; }; + + /** + * Count received bytes + */ + void count_rxbytes(unsigned n) { _bytes_rx += n; }; + + /** + * Get the receive status of this MAVLink link + */ + struct telemetry_status_s& get_rx_status() { return _rstatus; } + + struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } + + unsigned get_system_type() { return _system_type; } + +protected: + Mavlink *next; + +private: + int _instance_id; + + int _mavlink_fd; + bool _task_running; + + /* states */ + bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ + bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */ + bool _is_usb_uart; /**< Port is USB */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ + + unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ + + MavlinkOrbSubscription *_subscriptions; + MavlinkStream *_streams; + + MavlinkMissionManager *_mission_manager; + MavlinkParametersManager *_parameters_manager; + + MAVLINK_MODE _mode; + + mavlink_channel_t _channel; + + struct mavlink_logbuffer _logbuffer; + unsigned int _total_counter; + + pthread_t _receive_thread; + + bool _verbose; + bool _forwarding_on; + bool _passing_on; + bool _ftp_on; + int _uart_fd; + int _baudrate; + int _datarate; ///< data rate for normal streams (attitude, position, etc.) + int _datarate_events; ///< data rate for params, waypoints, text messages + float _rate_mult; + + /** + * If the queue index is not at 0, the queue sending + * logic will send parameters from the current index + * to len - 1, the end of the param list. + */ + unsigned int _mavlink_param_queue_index; + + bool mavlink_link_termination_allowed; + + char *_subscribe_to_stream; + float _subscribe_to_stream_rate; + + bool _flow_control_enabled; + uint64_t _last_write_success_time; + uint64_t _last_write_try_time; + + unsigned _bytes_tx; + unsigned _bytes_txerr; + unsigned _bytes_rx; + uint64_t _bytes_timestamp; + float _rate_tx; + float _rate_txerr; + float _rate_rx; + + struct telemetry_status_s _rstatus; ///< receive status + + struct mavlink_message_buffer { + int write_ptr; + int read_ptr; + int size; + char *data; + }; + + mavlink_message_buffer _message_buffer; + + pthread_mutex_t _message_buffer_mutex; + pthread_mutex_t _send_mutex; + + bool _param_initialized; + param_t _param_system_id; + param_t _param_component_id; + param_t _param_system_type; + param_t _param_use_hil_gps; + param_t _param_forward_externalsp; + + unsigned _system_type; + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _txerr_perf; /**< TX error counter */ + + void mavlink_update_system(); + + int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + + /** + * Get the free space in the transmit buffer + * + * @return free space in the UART TX buffer + */ + unsigned get_free_tx_buf(); + + static unsigned int interval_from_rate(float rate); + + int configure_stream(const char *stream_name, const float rate); + + /** + * Adjust the stream rates based on the current rate + * + * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease + */ + void adjust_stream_rates(const float multiplier); + + int message_buffer_init(int size); + + void message_buffer_destroy(); + + int message_buffer_count(); + + int message_buffer_is_empty(); + + int message_buffer_get_ptr(void **ptr, bool *is_part); + + void message_buffer_mark_read(int n); + + void pass_message(const mavlink_message_t *msg); + + /** + * Update rate mult so total bitrate will be equal to _datarate. + */ + void update_rate_mult(); + + virtual int ioctl(device::px4_dev_handle_t *handlep, int cmd, unsigned long arg); + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); + + /** + * Main mavlink task. + */ + int task_main(int argc, char *argv[]); + + /* do not allow copying this class */ + Mavlink(const Mavlink&); + Mavlink operator=(const Mavlink&); +}; diff --git a/src/modules/mavlink/mavlink_main_nuttx.h b/src/modules/mavlink/mavlink_main_nuttx.h new file mode 100644 index 0000000000..f7f0ad4b26 --- /dev/null +++ b/src/modules/mavlink/mavlink_main_nuttx.h @@ -0,0 +1,415 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 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 mavlink_main_nuttx.h + * MAVLink 1.0 protocol interface definition. + * + * @author Lorenz Meier + * @author Anton Babushkin + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "mavlink_bridge_header.h" +#include "mavlink_orb_subscription.h" +#include "mavlink_stream.h" +#include "mavlink_messages.h" +#include "mavlink_mission.h" +#include "mavlink_parameters.h" + +class Mavlink +{ + +public: + /** + * Constructor + */ + Mavlink(); + + /** + * Destructor, also kills the mavlinks task. + */ + ~Mavlink(); + + /** + * Start the mavlink task. + * + * @return OK on success. + */ + static int start(int argc, char *argv[]); + + /** + * Display the mavlink status. + */ + void display_status(); + + static int stream_command(int argc, char *argv[]); + + static int instance_count(); + + static Mavlink *new_instance(); + + static Mavlink *get_instance(unsigned instance); + + static Mavlink *get_instance_for_device(const char *device_name); + + static int destroy_all_instances(); + + static int get_status_all_instances(); + + static bool instance_exists(const char *device_name, Mavlink *self); + + static void forward_message(const mavlink_message_t *msg, Mavlink *self); + + static int get_uart_fd(unsigned index); + + int get_uart_fd(); + + /** + * Get the MAVLink system id. + * + * @return The system ID of this vehicle + */ + int get_system_id(); + + /** + * Get the MAVLink component id. + * + * @return The component ID of this vehicle + */ + int get_component_id(); + + const char *_device_name; + + enum MAVLINK_MODE { + MAVLINK_MODE_NORMAL = 0, + MAVLINK_MODE_CUSTOM, + MAVLINK_MODE_ONBOARD + }; + + void set_mode(enum MAVLINK_MODE); + enum MAVLINK_MODE get_mode() { return _mode; } + + bool get_hil_enabled() { return _hil_enabled; } + + bool get_use_hil_gps() { return _use_hil_gps; } + + bool get_forward_externalsp() { return _forward_externalsp; } + + bool get_flow_control_enabled() { return _flow_control_enabled; } + + bool get_forwarding_on() { return _forwarding_on; } + + /** + * Get the free space in the transmit buffer + * + * @return free space in the UART TX buffer + */ + unsigned get_free_tx_buf(); + + static int start_helper(int argc, char *argv[]); + + /** + * Handle parameter related messages. + */ + void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + + void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + + /** + * Enable / disable Hardware in the Loop simulation mode. + * + * @param hil_enabled The new HIL enable/disable state. + * @return OK if the HIL state changed, ERROR if the + * requested change could not be made or was + * redundant. + */ + int set_hil_enabled(bool hil_enabled); + + void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0); + + /** + * Resend message as is, don't change sequence number and CRC. + */ + void resend_message(mavlink_message_t *msg); + + void handle_message(const mavlink_message_t *msg); + + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0); + + int get_instance_id(); + + /** + * Enable / disable hardware flow control. + * + * @param enabled True if hardware flow control should be enabled + */ + int enable_flow_control(bool enabled); + + mavlink_channel_t get_channel(); + + void configure_stream_threadsafe(const char *stream_name, float rate); + + bool _task_should_exit; /**< if true, mavlink task should exit */ + + int get_mavlink_fd() { return _mavlink_fd; } + + /** + * Send a status text with loglevel INFO + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_info(const char *string); + + /** + * Send a status text with loglevel CRITICAL + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_critical(const char *string); + + /** + * Send a status text with loglevel EMERGENCY + * + * @param string the message to send (will be capped by mavlink max string length) + */ + void send_statustext_emergency(const char *string); + + /** + * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent + * only on this mavlink connection. Useful for reporting communication specific, not system-wide info + * only to client interested in it. Message will be not sent immediately but queued in buffer as + * for mavlink_log_xxx(). + * + * @param string the message to send (will be capped by mavlink max string length) + * @param severity the log level + */ + void send_statustext(unsigned char severity, const char *string); + + MavlinkStream * get_streams() const { return _streams; } + + float get_rate_mult(); + + /* Functions for waiting to start transmission until message received. */ + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + + bool message_buffer_write(const void *ptr, int size); + + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } + + /** + * Count a transmision error + */ + void count_txerr(); + + /** + * Count transmitted bytes + */ + void count_txbytes(unsigned n) { _bytes_tx += n; }; + + /** + * Count bytes not transmitted because of errors + */ + void count_txerrbytes(unsigned n) { _bytes_txerr += n; }; + + /** + * Count received bytes + */ + void count_rxbytes(unsigned n) { _bytes_rx += n; }; + + /** + * Get the receive status of this MAVLink link + */ + struct telemetry_status_s& get_rx_status() { return _rstatus; } + + struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } + + unsigned get_system_type() { return _system_type; } + +protected: + Mavlink *next; + +private: + int _instance_id; + + int _mavlink_fd; + bool _task_running; + + /* states */ + bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ + bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */ + bool _is_usb_uart; /**< Port is USB */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ + + unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ + + MavlinkOrbSubscription *_subscriptions; + MavlinkStream *_streams; + + MavlinkMissionManager *_mission_manager; + MavlinkParametersManager *_parameters_manager; + + MAVLINK_MODE _mode; + + mavlink_channel_t _channel; + + struct mavlink_logbuffer _logbuffer; + unsigned int _total_counter; + + pthread_t _receive_thread; + + bool _verbose; + bool _forwarding_on; + bool _passing_on; + bool _ftp_on; + int _uart_fd; + int _baudrate; + int _datarate; ///< data rate for normal streams (attitude, position, etc.) + int _datarate_events; ///< data rate for params, waypoints, text messages + float _rate_mult; + + /** + * If the queue index is not at 0, the queue sending + * logic will send parameters from the current index + * to len - 1, the end of the param list. + */ + unsigned int _mavlink_param_queue_index; + + bool mavlink_link_termination_allowed; + + char *_subscribe_to_stream; + float _subscribe_to_stream_rate; + + bool _flow_control_enabled; + uint64_t _last_write_success_time; + uint64_t _last_write_try_time; + + unsigned _bytes_tx; + unsigned _bytes_txerr; + unsigned _bytes_rx; + uint64_t _bytes_timestamp; + float _rate_tx; + float _rate_txerr; + float _rate_rx; + + struct telemetry_status_s _rstatus; ///< receive status + + struct mavlink_message_buffer { + int write_ptr; + int read_ptr; + int size; + char *data; + }; + + mavlink_message_buffer _message_buffer; + + pthread_mutex_t _message_buffer_mutex; + pthread_mutex_t _send_mutex; + + bool _param_initialized; + param_t _param_system_id; + param_t _param_component_id; + param_t _param_system_type; + param_t _param_use_hil_gps; + param_t _param_forward_externalsp; + + unsigned _system_type; + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _txerr_perf; /**< TX error counter */ + + void mavlink_update_system(); + + int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + + static unsigned int interval_from_rate(float rate); + + int configure_stream(const char *stream_name, const float rate); + + /** + * Adjust the stream rates based on the current rate + * + * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease + */ + void adjust_stream_rates(const float multiplier); + + int message_buffer_init(int size); + + void message_buffer_destroy(); + + int message_buffer_count(); + + int message_buffer_is_empty(); + + int message_buffer_get_ptr(void **ptr, bool *is_part); + + void message_buffer_mark_read(int n); + + void pass_message(const mavlink_message_t *msg); + + /** + * Update rate mult so total bitrate will be equal to _datarate. + */ + void update_rate_mult(); + + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); + + /** + * Main mavlink task. + */ + int task_main(int argc, char *argv[]); + + /* do not allow copying this class */ + Mavlink(const Mavlink&); + Mavlink operator=(const Mavlink&); +}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 81c76ef3f0..bfe7a4552c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -59,7 +59,6 @@ #include #include #include -#include #include #include #include @@ -992,7 +991,7 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) clock_gettime(CLOCK_REALTIME, &tv); // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 - bool onb_unix_valid = tv.tv_sec > PX4_EPOCH_SECS; + bool onb_unix_valid = (unsigned long long)tv.tv_sec > PX4_EPOCH_SECS; bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL; if (!onb_unix_valid && ofb_unix_valid) { @@ -1496,17 +1495,17 @@ MavlinkReceiver::receive_thread(void *arg) sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); prctl(PR_SET_NAME, thread_name, getpid()); - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = uart_fd; fds[0].events = POLLIN; ssize_t nread = 0; while (!_mavlink->_task_should_exit) { - if (poll(fds, 1, timeout) > 0) { + if (px4_poll(fds, 1, timeout) > 0) { /* non-blocking read. read may return negative values */ - if ((nread = read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { + if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { /* to avoid reading very small chunks wait for data before reading */ usleep(1000); } diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index e82b8bd935..10f283e6c7 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -36,16 +36,22 @@ # MODULE_COMMAND = mavlink -SRCS += mavlink_main.cpp \ - mavlink.c \ +ifeq ($(PX$_TARGET_OS),nuttx) +SRCS += mavlink_main_nuttx.cpp \ + mavlink_ftp_nuttx.cpp +else +SRCS += mavlink_main_linux.cpp \ + mavlink_ftp_linux.cpp +endif + +SRCS += mavlink.c \ mavlink_receiver.cpp \ mavlink_mission.cpp \ mavlink_parameters.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ - mavlink_rate_limiter.cpp \ - mavlink_ftp.cpp + mavlink_rate_limiter.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index f37bc93277..d49f6fdfbf 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -38,8 +38,12 @@ MODULE_COMMAND = sensors MODULE_PRIORITY = "SCHED_PRIORITY_MAX-5" -SRCS = sensors.cpp \ - sensor_params.c +ifeq ($(PX$_TARGET_OS),nuttx) +SRCS = sensors_nuttx.cpp +else +SRCS = sensors_linux.cpp +endif +SRCS += sensor_params.c MODULE_STACKSIZE = 1200 diff --git a/src/modules/sensors/sensors_linux.cpp b/src/modules/sensors/sensors_linux.cpp new file mode 100644 index 0000000000..dea1367ef9 --- /dev/null +++ b/src/modules/sensors/sensors_linux.cpp @@ -0,0 +1,2308 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 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 sensors.cpp + * + * PX4 Flight Core transitional mapping layer. + * + * This app / class mapps the PX4 middleware layer / drivers to the application + * layer of the PX4 Flight Core. Individual sensors can be accessed directly as + * well instead of relying on the sensor_combined topic. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Thomas Gubler + * @author Anton Babushkin + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * Analog layout: + * FMU: + * IN2 - battery voltage + * IN3 - battery current + * IN4 - 5V sense + * IN10 - spare (we could actually trim these from the set) + * IN11 - spare (we could actually trim these from the set) + * IN12 - spare (we could actually trim these from the set) + * IN13 - aux1 + * IN14 - aux2 + * IN15 - pressure sensor + * + * IO: + * IN4 - servo supply rail + * IN5 - analog RSSI + */ + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL -1 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#define ADC_BATTERY_VOLTAGE_CHANNEL 2 +#define ADC_BATTERY_CURRENT_CHANNEL 3 +#define ADC_5V_RAIL_SENSE 4 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 +#endif + +#ifdef CONFIG_ARCH_BOARD_AEROCORE +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL -1 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL -1 +#endif + +#define BATT_V_LOWPASS 0.001f +#define BATT_V_IGNORE_THRESHOLD 4.8f + +/** + * HACK - true temperature is much less than indicated temperature in baro, + * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB + */ +#define PCB_TEMP_ESTIMATE_DEG 5.0f +#define STICK_ON_OFF_LIMIT 0.75f +#define MAG_ROT_VAL_INTERNAL -1 + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING SENSOR CAL" + +/** + * Sensor app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int sensors_main(int argc, char *argv[]); + +class Sensors +{ +public: + /** + * Constructor + */ + Sensors(); + + /** + * Destructor, also kills the sensors task. + */ + ~Sensors(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ + + /** + * Get and limit value for specified RC function. Returns NAN if not mapped. + */ + float get_rc_value(uint8_t func, float min_value, float max_value); + + /** + * Get switch position for specified function. + */ + switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv); + switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv); + + /** + * Update paramters from RC channels if the functionality is activated and the + * input has changed since the last update + * + * @param + */ + void set_params_from_rc(); + + /** + * Gather and publish RC input data. + */ + void rc_poll(); + + /* XXX should not be here - should be own driver */ + int _fd_adc; /**< ADC driver handle */ + hrt_abstime _last_adc; /**< last time we took input from the ADC */ + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _sensors_task; /**< task handle for sensor task */ + + bool _hil_enabled; /**< if true, HIL is active */ + bool _publishing; /**< if true, we are publishing sensor data */ + + int _gyro_sub; /**< raw gyro0 data subscription */ + int _accel_sub; /**< raw accel0 data subscription */ + int _mag_sub; /**< raw mag0 data subscription */ + int _gyro1_sub; /**< raw gyro1 data subscription */ + int _accel1_sub; /**< raw accel1 data subscription */ + int _mag1_sub; /**< raw mag1 data subscription */ + int _gyro2_sub; /**< raw gyro2 data subscription */ + int _accel2_sub; /**< raw accel2 data subscription */ + int _mag2_sub; /**< raw mag2 data subscription */ + int _rc_sub; /**< raw rc channels data subscription */ + int _baro_sub; /**< raw baro data subscription */ + int _baro1_sub; /**< raw baro data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _diff_pres_sub; /**< raw differential pressure subscription */ + int _vcontrol_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< notification of parameter updates */ + int _rc_parameter_map_sub; /**< rc parameter map subscription */ + int _manual_control_sub; /**< notification of manual control updates */ + + orb_advert_t _sensor_pub; /**< combined sensor data topic */ + orb_advert_t _manual_control_pub; /**< manual control signal topic */ + orb_advert_t _actuator_group_3_pub; /**< manual control as actuator topic */ + orb_advert_t _rc_pub; /**< raw r/c control topic */ + orb_advert_t _battery_pub; /**< battery status */ + orb_advert_t _airspeed_pub; /**< airspeed */ + orb_advert_t _diff_pres_pub; /**< differential_pressure */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + struct rc_channels_s _rc; /**< r/c channel data */ + struct battery_status_s _battery_status; /**< battery status */ + struct baro_report _barometer; /**< barometer data */ + struct differential_pressure_s _diff_pres; + struct airspeed_s _airspeed; + struct rc_parameter_map_s _rc_parameter_map; + + math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */ + + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ + hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ + + struct { + float min[_rc_max_chan_count]; + float trim[_rc_max_chan_count]; + float max[_rc_max_chan_count]; + float rev[_rc_max_chan_count]; + float dz[_rc_max_chan_count]; + float scaling_factor[_rc_max_chan_count]; + + float diff_pres_offset_pa; + float diff_pres_analog_scale; + + int board_rotation; + int flow_rotation; + + float board_offset[3]; + + int rc_map_roll; + int rc_map_pitch; + int rc_map_yaw; + int rc_map_throttle; + int rc_map_failsafe; + + int rc_map_mode_sw; + int rc_map_return_sw; + int rc_map_posctl_sw; + int rc_map_loiter_sw; + int rc_map_acro_sw; + int rc_map_offboard_sw; + + int rc_map_flaps; + + int rc_map_aux1; + int rc_map_aux2; + int rc_map_aux3; + int rc_map_aux4; + int rc_map_aux5; + + int rc_map_param[RC_PARAM_MAP_NCHAN]; + + int32_t rc_fails_thr; + float rc_assist_th; + float rc_auto_th; + float rc_posctl_th; + float rc_return_th; + float rc_loiter_th; + float rc_acro_th; + float rc_offboard_th; + bool rc_assist_inv; + bool rc_auto_inv; + bool rc_posctl_inv; + bool rc_return_inv; + bool rc_loiter_inv; + bool rc_acro_inv; + bool rc_offboard_inv; + + float battery_voltage_scaling; + float battery_current_scaling; + + float baro_qnh; + + } _parameters; /**< local copies of interesting parameters */ + + struct { + param_t min[_rc_max_chan_count]; + param_t trim[_rc_max_chan_count]; + param_t max[_rc_max_chan_count]; + param_t rev[_rc_max_chan_count]; + param_t dz[_rc_max_chan_count]; + + param_t diff_pres_offset_pa; + param_t diff_pres_analog_scale; + + param_t rc_map_roll; + param_t rc_map_pitch; + param_t rc_map_yaw; + param_t rc_map_throttle; + param_t rc_map_failsafe; + + param_t rc_map_mode_sw; + param_t rc_map_return_sw; + param_t rc_map_posctl_sw; + param_t rc_map_loiter_sw; + param_t rc_map_acro_sw; + param_t rc_map_offboard_sw; + + param_t rc_map_flaps; + + param_t rc_map_aux1; + param_t rc_map_aux2; + param_t rc_map_aux3; + param_t rc_map_aux4; + param_t rc_map_aux5; + + param_t rc_map_param[RC_PARAM_MAP_NCHAN]; + param_t rc_param[RC_PARAM_MAP_NCHAN]; /**< param handles for the paramters which are bound + to a RC channel, equivalent float values in the + _parameters struct are not existing + because these parameters are never read. */ + + param_t rc_fails_thr; + param_t rc_assist_th; + param_t rc_auto_th; + param_t rc_posctl_th; + param_t rc_return_th; + param_t rc_loiter_th; + param_t rc_acro_th; + param_t rc_offboard_th; + + param_t battery_voltage_scaling; + param_t battery_current_scaling; + + param_t board_rotation; + param_t flow_rotation; + + param_t board_offset[3]; + + param_t baro_qnh; + + } _parameter_handles; /**< handles for interesting parameters */ + + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Do accel-related initialisation. + */ + int accel_init(); + + /** + * Do gyro-related initialisation. + */ + int gyro_init(); + + /** + * Do mag-related initialisation. + */ + int mag_init(); + + /** + * Do baro-related initialisation. + */ + int baro_init(); + + /** + * Do adc-related initialisation. + */ + int adc_init(); + + /** + * Poll the accelerometer for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void accel_poll(struct sensor_combined_s &raw); + + /** + * Poll the gyro for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void gyro_poll(struct sensor_combined_s &raw); + + /** + * Poll the magnetometer for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void mag_poll(struct sensor_combined_s &raw); + + /** + * Poll the barometer for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void baro_poll(struct sensor_combined_s &raw); + + /** + * Poll the differential pressure sensor for updated data. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void diff_pres_poll(struct sensor_combined_s &raw); + + /** + * Check for changes in vehicle control mode. + */ + void vehicle_control_mode_poll(); + + /** + * Check for changes in parameters. + */ + void parameter_update_poll(bool forced = false); + + /** + * Check for changes in rc_parameter_map + */ + void rc_parameter_map_poll(bool forced = false); + + /** + * Poll the ADC and update readings to suit. + * + * @param raw Combined sensor data structure into which + * data should be returned. + */ + void adc_poll(struct sensor_combined_s &raw); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main(); +}; + +namespace sensors +{ + +Sensors *g_sensors = nullptr; +} + +Sensors::Sensors() : + _fd_adc(-1), + _last_adc(0), + + _task_should_exit(true), + _sensors_task(-1), + _hil_enabled(false), + _publishing(true), + + /* subscriptions */ + _gyro_sub(-1), + _accel_sub(-1), + _mag_sub(-1), + _gyro1_sub(-1), + _accel1_sub(-1), + _mag1_sub(-1), + _gyro2_sub(-1), + _accel2_sub(-1), + _mag2_sub(-1), + _rc_sub(-1), + _baro_sub(-1), + _baro1_sub(-1), + _vcontrol_mode_sub(-1), + _params_sub(-1), + _rc_parameter_map_sub(-1), + _manual_control_sub(-1), + + /* publications */ + _sensor_pub(-1), + _manual_control_pub(-1), + _actuator_group_3_pub(-1), + _rc_pub(-1), + _battery_pub(-1), + _airspeed_pub(-1), + _diff_pres_pub(-1), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), + + _board_rotation{}, + _mag_rotation{}, + + _battery_discharged(0), + _battery_current_timestamp(0) +{ + memset(&_rc, 0, sizeof(_rc)); + memset(&_diff_pres, 0, sizeof(_diff_pres)); + memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); + + /* basic r/c parameters */ + for (unsigned i = 0; i < _rc_max_chan_count; i++) { + char nbuf[16]; + + /* min values */ + sprintf(nbuf, "RC%d_MIN", i + 1); + _parameter_handles.min[i] = param_find(nbuf); + + /* trim values */ + sprintf(nbuf, "RC%d_TRIM", i + 1); + _parameter_handles.trim[i] = param_find(nbuf); + + /* max values */ + sprintf(nbuf, "RC%d_MAX", i + 1); + _parameter_handles.max[i] = param_find(nbuf); + + /* channel reverse */ + sprintf(nbuf, "RC%d_REV", i + 1); + _parameter_handles.rev[i] = param_find(nbuf); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + _parameter_handles.dz[i] = param_find(nbuf); + + } + + /* mandatory input switched, mapped to channels 1-4 per default */ + _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); + _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); + _parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); + _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); + _parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE"); + + /* mandatory mode switches, mapped to channel 5 and 6 per default */ + _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); + _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); + + _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); + + /* optional mode switches, not mapped per default */ + _parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW"); + _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); + _parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW"); + _parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW"); + + _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); + _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); + _parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3"); + _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); + _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); + + /* RC to parameter mapping for changing parameters with RC */ + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + char name[PARAM_ID_LEN]; + snprintf(name, PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1 + _parameter_handles.rc_map_param[i] = param_find(name); + } + + /* RC thresholds */ + _parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR"); + _parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH"); + _parameter_handles.rc_auto_th = param_find("RC_AUTO_TH"); + _parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH"); + _parameter_handles.rc_return_th = param_find("RC_RETURN_TH"); + _parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH"); + _parameter_handles.rc_acro_th = param_find("RC_ACRO_TH"); + _parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH"); + + /* Differential pressure offset */ + _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); + _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); + + _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + _parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING"); + + /* rotations */ + _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); + _parameter_handles.flow_rotation = param_find("SENS_FLOW_ROT"); + + /* rotation offsets */ + _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); + _parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); + _parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); + + /* Barometer QNH */ + _parameter_handles.baro_qnh = param_find("SENS_BARO_QNH"); + + /* fetch initial parameter values */ + parameters_update(); +} + +Sensors::~Sensors() +{ + if (_sensors_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + px4_task_delete(_sensors_task); + break; + } + } while (_sensors_task != -1); + } + + sensors::g_sensors = nullptr; +} + +int +Sensors::parameters_update() +{ + bool rc_valid = true; + float tmpScaleFactor = 0.0f; + float tmpRevFactor = 0.0f; + + /* rc values */ + for (unsigned int i = 0; i < _rc_max_chan_count; i++) { + + param_get(_parameter_handles.min[i], &(_parameters.min[i])); + param_get(_parameter_handles.trim[i], &(_parameters.trim[i])); + param_get(_parameter_handles.max[i], &(_parameters.max[i])); + param_get(_parameter_handles.rev[i], &(_parameters.rev[i])); + param_get(_parameter_handles.dz[i], &(_parameters.dz[i])); + + tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); + tmpRevFactor = tmpScaleFactor * _parameters.rev[i]; + + /* handle blowup in the scaling factor calculation */ +#ifdef __PX4_NUTTX + if (!isfinite(tmpScaleFactor) || +#else + if (!std::isfinite(tmpScaleFactor) || +#endif + (tmpRevFactor < 0.000001f) || + (tmpRevFactor > 0.2f)) { + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i])); + /* scaling factors do not make sense, lock them down */ + _parameters.scaling_factor[i] = 0.0f; + rc_valid = false; + + } else { + _parameters.scaling_factor[i] = tmpScaleFactor; + } + } + + /* handle wrong values */ + if (!rc_valid) { + warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); + } + + const char *paramerr = "FAIL PARM LOAD"; + + /* channel mapping */ + if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) { + warnx("%s", paramerr); + } + + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { + warnx("%s", paramerr); + } + + param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); + param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)); + param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); + param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); + param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); + + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i])); + } + + param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr)); + param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th)); + _parameters.rc_assist_inv = (_parameters.rc_assist_th < 0); + _parameters.rc_assist_th = fabs(_parameters.rc_assist_th); + param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th)); + _parameters.rc_auto_inv = (_parameters.rc_auto_th < 0); + _parameters.rc_auto_th = fabs(_parameters.rc_auto_th); + param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th)); + _parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0); + _parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th); + param_get(_parameter_handles.rc_return_th, &(_parameters.rc_return_th)); + _parameters.rc_return_inv = (_parameters.rc_return_th < 0); + _parameters.rc_return_th = fabs(_parameters.rc_return_th); + param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th)); + _parameters.rc_loiter_inv = (_parameters.rc_loiter_th < 0); + _parameters.rc_loiter_th = fabs(_parameters.rc_loiter_th); + param_get(_parameter_handles.rc_acro_th, &(_parameters.rc_acro_th)); + _parameters.rc_acro_inv = (_parameters.rc_acro_th < 0); + _parameters.rc_acro_th = fabs(_parameters.rc_acro_th); + param_get(_parameter_handles.rc_offboard_th, &(_parameters.rc_offboard_th)); + _parameters.rc_offboard_inv = (_parameters.rc_offboard_th < 0); + _parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th); + + /* update RC function mappings */ + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; + + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; + } + + /* Airspeed offset */ + param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); + param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale)); + + /* scaling of ADC ticks to battery voltage */ + if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { + warnx("%s", paramerr); + } + + /* scaling of ADC ticks to battery current */ + if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { + warnx("%s", paramerr); + } + + param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); + param_get(_parameter_handles.flow_rotation, &(_parameters.flow_rotation)); + + /* set px4flow rotation */ + int flowfd; + flowfd = px4_open(PX4FLOW0_DEVICE_PATH, 0); + + if (flowfd >= 0) { + int flowret = px4_ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); + + if (flowret) { + warnx("flow rotation could not be set"); + px4_close(flowfd); + return ERROR; + } + + px4_close(flowfd); + } + + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); + + param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0])); + param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); + param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); + + /** fine tune board offset on parameter update **/ + math::Matrix<3, 3> board_rotation_offset; + board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _parameters.board_offset[0], + M_DEG_TO_RAD_F * _parameters.board_offset[1], + M_DEG_TO_RAD_F * _parameters.board_offset[2]); + + _board_rotation = _board_rotation * board_rotation_offset; + + /* update barometer qnh setting */ + param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); + int barofd; + barofd = px4_open(BARO0_DEVICE_PATH, 0); + + if (barofd < 0) { + warnx("ERROR: no barometer found on %s", BARO0_DEVICE_PATH); + return ERROR; + + } else { + int baroret = px4_ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + + if (baroret) { + warnx("qnh could not be set"); + px4_close(barofd); + return ERROR; + } + + px4_close(barofd); + } + + return OK; +} + +int +Sensors::accel_init() +{ + int fd; + + fd = px4_open(ACCEL0_DEVICE_PATH, 0); + + if (fd < 0) { + warnx("FATAL: no accelerometer found: %s", ACCEL0_DEVICE_PATH); + return ERROR; + + } else { + + /* set the accel internal sampling rate to default rate */ + px4_ioctl(fd, ACCELIOCSSAMPLERATE, ACCEL_SAMPLERATE_DEFAULT); + + /* set the driver to poll at default rate */ + px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + + px4_close(fd); + } + + return OK; +} + +int +Sensors::gyro_init() +{ + int fd; + + fd = px4_open(GYRO0_DEVICE_PATH, 0); + + if (fd < 0) { + warnx("FATAL: no gyro found: %s", GYRO0_DEVICE_PATH); + return ERROR; + + } else { + + /* set the gyro internal sampling rate to default rate */ + px4_ioctl(fd, GYROIOCSSAMPLERATE, GYRO_SAMPLERATE_DEFAULT); + + /* set the driver to poll at default rate */ + px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); + + } + + return OK; +} + +int +Sensors::mag_init() +{ + int fd; + int ret; + + fd = px4_open(MAG0_DEVICE_PATH, 0); + + if (fd < 0) { + warnx("FATAL: no magnetometer found: %s", MAG0_DEVICE_PATH); + return ERROR; + } + + /* try different mag sampling rates */ + + + ret = px4_ioctl(fd, MAGIOCSSAMPLERATE, 150); + + if (ret == OK) { + /* set the pollrate accordingly */ + px4_ioctl(fd, SENSORIOCSPOLLRATE, 150); + + } else { + ret = px4_ioctl(fd, MAGIOCSSAMPLERATE, 100); + + /* if the slower sampling rate still fails, something is wrong */ + if (ret == OK) { + /* set the driver to poll also at the slower rate */ + px4_ioctl(fd, SENSORIOCSPOLLRATE, 100); + + } else { + warnx("FATAL: mag sampling rate could not be set"); + return ERROR; + } + } + + px4_close(fd); + + return OK; +} + +int +Sensors::baro_init() +{ + int fd; + + fd = px4_open(BARO0_DEVICE_PATH, 0); + + if (fd < 0) { + warnx("FATAL: No barometer found: %s", BARO0_DEVICE_PATH); + return ERROR; + } + + /* set the driver to poll at 150Hz */ + px4_ioctl(fd, SENSORIOCSPOLLRATE, 150); + + px4_close(fd); + + return OK; +} + +int +Sensors::adc_init() +{ + + _fd_adc = px4_open(ADC0_DEVICE_PATH, O_RDONLY | O_NONBLOCK); + + if (_fd_adc < 0) { + warnx("FATAL: no ADC found: %s", ADC0_DEVICE_PATH); + return ERROR; + } + + return OK; +} + +void +Sensors::accel_poll(struct sensor_combined_s &raw) +{ + bool accel_updated; + orb_check(_accel_sub, &accel_updated); + + if (accel_updated) { + struct accel_report accel_report; + + orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); + + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); + vect = _board_rotation * vect; + + raw.accelerometer_m_s2[0] = vect(0); + raw.accelerometer_m_s2[1] = vect(1); + raw.accelerometer_m_s2[2] = vect(2); + + raw.accelerometer_raw[0] = accel_report.x_raw; + raw.accelerometer_raw[1] = accel_report.y_raw; + raw.accelerometer_raw[2] = accel_report.z_raw; + + raw.accelerometer_timestamp = accel_report.timestamp; + raw.accelerometer_errcount = accel_report.error_count; + } + + orb_check(_accel1_sub, &accel_updated); + + if (accel_updated) { + struct accel_report accel_report; + + orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report); + + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); + vect = _board_rotation * vect; + + raw.accelerometer1_m_s2[0] = vect(0); + raw.accelerometer1_m_s2[1] = vect(1); + raw.accelerometer1_m_s2[2] = vect(2); + + raw.accelerometer1_raw[0] = accel_report.x_raw; + raw.accelerometer1_raw[1] = accel_report.y_raw; + raw.accelerometer1_raw[2] = accel_report.z_raw; + + raw.accelerometer1_timestamp = accel_report.timestamp; + raw.accelerometer1_errcount = accel_report.error_count; + } + + orb_check(_accel2_sub, &accel_updated); + + if (accel_updated) { + struct accel_report accel_report; + + orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report); + + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); + vect = _board_rotation * vect; + + raw.accelerometer2_m_s2[0] = vect(0); + raw.accelerometer2_m_s2[1] = vect(1); + raw.accelerometer2_m_s2[2] = vect(2); + + raw.accelerometer2_raw[0] = accel_report.x_raw; + raw.accelerometer2_raw[1] = accel_report.y_raw; + raw.accelerometer2_raw[2] = accel_report.z_raw; + + raw.accelerometer2_timestamp = accel_report.timestamp; + raw.accelerometer2_errcount = accel_report.error_count; + } +} + +void +Sensors::gyro_poll(struct sensor_combined_s &raw) +{ + bool gyro_updated; + orb_check(_gyro_sub, &gyro_updated); + + if (gyro_updated) { + struct gyro_report gyro_report; + + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); + + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); + vect = _board_rotation * vect; + + raw.gyro_rad_s[0] = vect(0); + raw.gyro_rad_s[1] = vect(1); + raw.gyro_rad_s[2] = vect(2); + + raw.gyro_raw[0] = gyro_report.x_raw; + raw.gyro_raw[1] = gyro_report.y_raw; + raw.gyro_raw[2] = gyro_report.z_raw; + + raw.timestamp = gyro_report.timestamp; + raw.gyro_errcount = gyro_report.error_count; + } + + orb_check(_gyro1_sub, &gyro_updated); + + if (gyro_updated) { + struct gyro_report gyro_report; + + orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report); + + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); + vect = _board_rotation * vect; + + raw.gyro1_rad_s[0] = vect(0); + raw.gyro1_rad_s[1] = vect(1); + raw.gyro1_rad_s[2] = vect(2); + + raw.gyro1_raw[0] = gyro_report.x_raw; + raw.gyro1_raw[1] = gyro_report.y_raw; + raw.gyro1_raw[2] = gyro_report.z_raw; + + raw.gyro1_timestamp = gyro_report.timestamp; + raw.gyro1_errcount = gyro_report.error_count; + } + + orb_check(_gyro2_sub, &gyro_updated); + + if (gyro_updated) { + struct gyro_report gyro_report; + + orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report); + + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); + vect = _board_rotation * vect; + + raw.gyro2_rad_s[0] = vect(0); + raw.gyro2_rad_s[1] = vect(1); + raw.gyro2_rad_s[2] = vect(2); + + raw.gyro2_raw[0] = gyro_report.x_raw; + raw.gyro2_raw[1] = gyro_report.y_raw; + raw.gyro2_raw[2] = gyro_report.z_raw; + + raw.gyro2_timestamp = gyro_report.timestamp; + raw.gyro2_errcount = gyro_report.error_count; + } +} + +void +Sensors::mag_poll(struct sensor_combined_s &raw) +{ + bool mag_updated; + orb_check(_mag_sub, &mag_updated); + + if (mag_updated) { + struct mag_report mag_report; + + orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); + + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); + + vect = _mag_rotation[0] * vect; + + raw.magnetometer_ga[0] = vect(0); + raw.magnetometer_ga[1] = vect(1); + raw.magnetometer_ga[2] = vect(2); + + raw.magnetometer_raw[0] = mag_report.x_raw; + raw.magnetometer_raw[1] = mag_report.y_raw; + raw.magnetometer_raw[2] = mag_report.z_raw; + + raw.magnetometer_timestamp = mag_report.timestamp; + raw.magnetometer_errcount = mag_report.error_count; + } + + orb_check(_mag1_sub, &mag_updated); + + if (mag_updated) { + struct mag_report mag_report; + + orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report); + + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); + + vect = _mag_rotation[1] * vect; + + raw.magnetometer1_ga[0] = vect(0); + raw.magnetometer1_ga[1] = vect(1); + raw.magnetometer1_ga[2] = vect(2); + + raw.magnetometer1_raw[0] = mag_report.x_raw; + raw.magnetometer1_raw[1] = mag_report.y_raw; + raw.magnetometer1_raw[2] = mag_report.z_raw; + + raw.magnetometer1_timestamp = mag_report.timestamp; + raw.magnetometer1_errcount = mag_report.error_count; + } + + orb_check(_mag2_sub, &mag_updated); + + if (mag_updated) { + struct mag_report mag_report; + + orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report); + + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); + + vect = _mag_rotation[2] * vect; + + raw.magnetometer2_ga[0] = vect(0); + raw.magnetometer2_ga[1] = vect(1); + raw.magnetometer2_ga[2] = vect(2); + + raw.magnetometer2_raw[0] = mag_report.x_raw; + raw.magnetometer2_raw[1] = mag_report.y_raw; + raw.magnetometer2_raw[2] = mag_report.z_raw; + + raw.magnetometer2_timestamp = mag_report.timestamp; + raw.magnetometer2_errcount = mag_report.error_count; + } +} + +void +Sensors::baro_poll(struct sensor_combined_s &raw) +{ + bool baro_updated; + orb_check(_baro_sub, &baro_updated); + + if (baro_updated) { + + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer); + + raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar + raw.baro_alt_meter = _barometer.altitude; // Altitude in meters + raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius + + raw.baro_timestamp = _barometer.timestamp; + } + + orb_check(_baro1_sub, &baro_updated); + + if (baro_updated) { + + struct baro_report baro_report; + + orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report); + + raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar + raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters + raw.baro1_temp_celcius = baro_report.temperature; // Temperature in degrees celcius + + raw.baro1_timestamp = baro_report.timestamp; + } +} + +void +Sensors::diff_pres_poll(struct sensor_combined_s &raw) +{ + bool updated; + orb_check(_diff_pres_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); + + raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa; + raw.differential_pressure_timestamp = _diff_pres.timestamp; + raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; + + float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : + (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + + _airspeed.timestamp = _diff_pres.timestamp; + + /* don't risk to feed negative airspeed into the system */ + _airspeed.indicated_airspeed_m_s = math::max(0.0f, + calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); + _airspeed.true_airspeed_m_s = math::max(0.0f, + calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, + raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); + _airspeed.air_temperature_celsius = air_temperature_celsius; + + /* announce the airspeed if needed, just publish else */ + if (_airspeed_pub > 0) { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed); + + } else { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed); + } + } +} + +void +Sensors::vehicle_control_mode_poll() +{ + struct vehicle_control_mode_s vcontrol_mode; + bool vcontrol_mode_updated; + + /* Check HIL state if vehicle control mode has changed */ + orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); + + if (vcontrol_mode_updated) { + + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); + + /* switching from non-HIL to HIL mode */ + //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); + if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) { + _hil_enabled = true; + _publishing = false; + + /* switching from HIL to non-HIL mode */ + + } else if (!_publishing && !_hil_enabled) { + _hil_enabled = false; + _publishing = true; + } + } +} + +void +Sensors::parameter_update_poll(bool forced) +{ + bool param_updated; + + /* Check if any parameter has changed */ + orb_check(_params_sub, ¶m_updated); + + if (param_updated || forced) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters */ + parameters_update(); + + /* set offset parameters to new values */ + bool failed; + int res; + char str[30]; + unsigned mag_count = 0; + unsigned gyro_count = 0; + unsigned accel_count = 0; + + /* run through all gyro sensors */ + for (unsigned s = 0; s < 3; s++) { + + res = ERROR; + (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); + + int fd = open(str, 0); + + if (fd < 0) { + continue; + } + + bool config_ok = false; + + /* run through all stored calibrations */ + for (unsigned i = 0; i < 3; i++) { + /* initially status is ok per config */ + failed = false; + + (void)sprintf(str, "CAL_GYRO%u_ID", i); + int device_id; + failed = failed || (OK != param_get(param_find(str), &device_id)); + + if (failed) { + px4_close(fd); + continue; + } + + /* if the calibration is for this device, apply it */ + if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) { + struct gyro_scale gscale = {}; + (void)sprintf(str, "CAL_GYRO%u_XOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); + (void)sprintf(str, "CAL_GYRO%u_YOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.y_offset)); + (void)sprintf(str, "CAL_GYRO%u_ZOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.z_offset)); + (void)sprintf(str, "CAL_GYRO%u_XSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.x_scale)); + (void)sprintf(str, "CAL_GYRO%u_YSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.y_scale)); + (void)sprintf(str, "CAL_GYRO%u_ZSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); + + if (failed) { + warnx("%s: gyro #%u", CAL_FAILED_APPLY_CAL_MSG, gyro_count); + } else { + /* apply new scaling and offsets */ + res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); + if (res) { + warnx(CAL_FAILED_APPLY_CAL_MSG); + } else { + gyro_count++; + config_ok = true; + } + } + break; + } + } + + px4_close(fd); + + if (!config_ok) { + warnx("NO CONFIG FOR GYRO #%u", s); + } + } + + /* run through all accel sensors */ + for (unsigned s = 0; s < 3; s++) { + + res = ERROR; + (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); + + int fd = open(str, 0); + + if (fd < 0) { + continue; + } + + bool config_ok = false; + + /* run through all stored calibrations */ + for (unsigned i = 0; i < 3; i++) { + /* initially status is ok per config */ + failed = false; + + (void)sprintf(str, "CAL_ACC%u_ID", i); + int device_id; + failed = failed || (OK != param_get(param_find(str), &device_id)); + + if (failed) { + px4_close(fd); + continue; + } + + /* if the calibration is for this device, apply it */ + if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) { + struct accel_scale gscale = {}; + (void)sprintf(str, "CAL_ACC%u_XOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); + (void)sprintf(str, "CAL_ACC%u_YOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.y_offset)); + (void)sprintf(str, "CAL_ACC%u_ZOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.z_offset)); + (void)sprintf(str, "CAL_ACC%u_XSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.x_scale)); + (void)sprintf(str, "CAL_ACC%u_YSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.y_scale)); + (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); + + if (failed) { + warnx("%s: acc #%u", CAL_FAILED_APPLY_CAL_MSG, accel_count); + } else { + /* apply new scaling and offsets */ + res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); + if (res) { + warnx(CAL_FAILED_APPLY_CAL_MSG); + } else { + accel_count++; + config_ok = true; + } + } + break; + } + } + + px4_close(fd); + + if (!config_ok) { + warnx("NO CONFIG FOR ACCEL #%u", s); + } + } + + /* run through all mag sensors */ + for (unsigned s = 0; s < 3; s++) { + + res = ERROR; + (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); + + int fd = px4_open(str, 0); + + if (fd < 0) { + continue; + } + + bool config_ok = false; + + /* run through all stored calibrations */ + for (unsigned i = 0; i < 3; i++) { + /* initially status is ok per config */ + failed = false; + + (void)sprintf(str, "CAL_MAG%u_ID", i); + int device_id; + failed = failed || (OK != param_get(param_find(str), &device_id)); + + if (failed) { + px4_close(fd); + continue; + } + + /* if the calibration is for this device, apply it */ + if (device_id == px4_ioctl(fd, DEVIOCGDEVICEID, 0)) { + struct mag_scale gscale = {}; + (void)sprintf(str, "CAL_MAG%u_XOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); + (void)sprintf(str, "CAL_MAG%u_YOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.y_offset)); + (void)sprintf(str, "CAL_MAG%u_ZOFF", i); + failed = failed || (OK != param_get(param_find(str), &gscale.z_offset)); + (void)sprintf(str, "CAL_MAG%u_XSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.x_scale)); + (void)sprintf(str, "CAL_MAG%u_YSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.y_scale)); + (void)sprintf(str, "CAL_MAG%u_ZSCALE", i); + failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); + + (void)sprintf(str, "CAL_MAG%u_ROT", i); + + if (px4_ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0) { + /* mag is internal */ + _mag_rotation[s] = _board_rotation; + /* reset param to -1 to indicate internal mag */ + int32_t minus_one = MAG_ROT_VAL_INTERNAL; + param_set_no_notification(param_find(str), &minus_one); + } else { + + int32_t mag_rot; + param_get(param_find(str), &mag_rot); + + /* check if this mag is still set as internal */ + if (mag_rot < 0) { + /* it was marked as internal, change to external with no rotation */ + mag_rot = 0; + param_set_no_notification(param_find(str), &mag_rot); + } + + /* handling of old setups, will be removed later (noted Feb 2015) */ + int32_t deprecated_mag_rot = 0; + param_get(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot); + + /* + * If the deprecated parameter is non-default (is != 0), + * and the new parameter is default (is == 0), then this board + * was configured already and we need to copy the old value + * to the new parameter. + * The < 0 case is special: It means that this param slot was + * used previously by an internal sensor, but the the call above + * proved that it is currently occupied by an external sensor. + * In that case we consider the orientation to be default as well. + */ + if ((deprecated_mag_rot != 0) && (mag_rot <= 0)) { + mag_rot = deprecated_mag_rot; + param_set_no_notification(param_find(str), &mag_rot); + /* clear the old param, not supported in GUI anyway */ + deprecated_mag_rot = 0; + param_set_no_notification(param_find("SENS_EXT_MAG_ROT"), &deprecated_mag_rot); + } + + /* handling of transition from internal to external */ + if (mag_rot < 0) { + mag_rot = 0; + } + + get_rot_matrix((enum Rotation)mag_rot, &_mag_rotation[s]); + } + + if (failed) { + warnx("%s: mag #%u", CAL_FAILED_APPLY_CAL_MSG, mag_count); + } else { + /* apply new scaling and offsets */ + res = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); + if (res) { + warnx(CAL_FAILED_APPLY_CAL_MSG); + } else { + mag_count++; + config_ok = true; + } + } + break; + } + } + + px4_close(fd); + + if (!config_ok) { + warnx("NO CONFIG FOR MAG #%u", s); + } + } + + int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0); + + /* this sensor is optional, abort without error */ + + if (fd >= 0) { + struct airspeed_scale airscale = { + _parameters.diff_pres_offset_pa, + 1.0f, + }; + + if (OK != px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + warn("WARNING: failed to set scale / offsets for airspeed sensor"); + } + + px4_close(fd); + } + + warnx("config: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count); + } +} + +void +Sensors::rc_parameter_map_poll(bool forced) +{ + bool map_updated; + orb_check(_rc_parameter_map_sub, &map_updated); + + if (map_updated) { + orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); + + /* update paramter handles to which the RC channels are mapped */ + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) + * or no request to map this channel to a param has been sent via mavlink + */ + continue; + } + + /* Set the handle by index if the index is set, otherwise use the id */ + if (_rc_parameter_map.param_index[i] >= 0) { + _parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]); + + } else { + _parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]); + } + + } + + warnx("rc to parameter map updated"); + + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f", + i, + _rc_parameter_map.param_id[i], + (double)_rc_parameter_map.scale[i], + (double)_rc_parameter_map.value0[i], + (double)_rc_parameter_map.value_min[i], + (double)_rc_parameter_map.value_max[i] + ); + } + } +} + +void +Sensors::adc_poll(struct sensor_combined_s &raw) +{ + /* only read if publishing */ + if (!_publishing) { + return; + } + + hrt_abstime t = hrt_absolute_time(); + + /* rate limit to 100 Hz */ + if (t - _last_adc >= 10000) { + /* make space for a maximum of twelve channels (to ensure reading all channels at once) */ + struct adc_msg_s buf_adc[12]; + /* read all channels available */ + int ret = px4_read(_fd_adc, &buf_adc, sizeof(buf_adc)); + + if (ret >= (int)sizeof(buf_adc[0])) { + + /* Read add channels we got */ + for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) { + /* Save raw voltage values */ + if (i < (sizeof(raw.adc_voltage_v) / sizeof(raw.adc_voltage_v[0]))) { + raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); + raw.adc_mapping[i] = buf_adc[i].am_channel; + } + + /* look for specific channels and process the raw voltage to measurement data */ + if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { + /* Voltage in volts */ + float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); + + if (voltage > BATT_V_IGNORE_THRESHOLD) { + _battery_status.voltage_v = voltage; + + /* one-time initialization of low-pass value to avoid long init delays */ + if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) { + _battery_status.voltage_filtered_v = voltage; + } + + _battery_status.timestamp = t; + _battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS; + + } else { + /* mark status as invalid */ + _battery_status.voltage_v = -1.0f; + _battery_status.voltage_filtered_v = -1.0f; + } + + } else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) { + /* handle current only if voltage is valid */ + if (_battery_status.voltage_v > 0.0f) { + float current = (buf_adc[i].am_data * _parameters.battery_current_scaling); + + /* check measured current value */ + if (current >= 0.0f) { + _battery_status.timestamp = t; + _battery_status.current_a = current; + + if (_battery_current_timestamp != 0) { + /* initialize discharged value */ + if (_battery_status.discharged_mah < 0.0f) { + _battery_status.discharged_mah = 0.0f; + } + + _battery_discharged += current * (t - _battery_current_timestamp); + _battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f; + } + } + } + + _battery_current_timestamp = t; + + } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { + + /* calculate airspeed, raw is the difference from */ + float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor) + + /** + * The voltage divider pulls the signal down, only act on + * a valid voltage from a connected sensor. Also assume a non- + * zero offset from the sensor if its connected. + */ + if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) { + + float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; + + _diff_pres.timestamp = t; + _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; + _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + + (diff_pres_pa_raw * 0.1f); + _diff_pres.temperature = -1000.0f; + + /* announce the airspeed if needed, just publish else */ + if (_diff_pres_pub > 0) { + orb_publish(ORB_ID(differential_pressure), _diff_pres_pub, &_diff_pres); + + } else { + _diff_pres_pub = orb_advertise(ORB_ID(differential_pressure), &_diff_pres); + } + } + } + } + + _last_adc = t; + + if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) { + /* announce the battery status if needed, just publish else */ + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); + + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } + } + } + } +} + +float +Sensors::get_rc_value(uint8_t func, float min_value, float max_value) +{ + if (_rc.function[func] >= 0) { + float value = _rc.channels[_rc.function[func]]; + + if (value < min_value) { + return min_value; + + } else if (value > max_value) { + return max_value; + + } else { + return value; + } + + } else { + return 0.0f; + } +} + +switch_pos_t +Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv) +{ + if (_rc.function[func] >= 0) { + float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; + + if (on_inv ? value < on_th : value > on_th) { + return manual_control_setpoint_s::SWITCH_POS_ON; + + } else if (mid_inv ? value < mid_th : value > mid_th) { + return manual_control_setpoint_s::SWITCH_POS_MIDDLE; + + } else { + return manual_control_setpoint_s::SWITCH_POS_OFF; + } + + } else { + return manual_control_setpoint_s::SWITCH_POS_NONE; + } +} + +switch_pos_t +Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) +{ + if (_rc.function[func] >= 0) { + float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; + + if (on_inv ? value < on_th : value > on_th) { + return manual_control_setpoint_s::SWITCH_POS_ON; + + } else { + return manual_control_setpoint_s::SWITCH_POS_OFF; + } + + } else { + return manual_control_setpoint_s::SWITCH_POS_NONE; + } +} + +void +Sensors::set_params_from_rc() +{ + static float param_rc_values[RC_PARAM_MAP_NCHAN] = {}; + + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) + * or no request to map this channel to a param has been sent via mavlink + */ + continue; + } + + float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); + /* Check if the value has changed, + * maybe we need to introduce a more aggressive limit here */ + if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { + param_rc_values[i] = rc_val; + float param_val = math::constrain( + _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, + _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); + param_set(_parameter_handles.rc_param[i], ¶m_val); + } + } +} + +void +Sensors::rc_poll() +{ + bool rc_updated; + orb_check(_rc_sub, &rc_updated); + + if (rc_updated) { + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ + struct rc_input_values rc_input; + + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + + /* detect RC signal loss */ + bool signal_lost; + + /* check flags and require at least four channels to consider the signal valid */ + if (rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4) { + /* signal is lost or no enough channels */ + signal_lost = true; + + } else { + /* signal looks good */ + signal_lost = false; + + /* check failsafe */ + int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; // get channel mapped to throttle + + if (_parameters.rc_map_failsafe > 0) { // if not 0, use channel number instead of rc.function mapping + fs_ch = _parameters.rc_map_failsafe - 1; + } + + if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) { + /* failsafe configured */ + if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) || + (_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) { + /* failsafe triggered, signal is lost by receiver */ + signal_lost = true; + } + } + } + + unsigned channel_limit = rc_input.channel_count; + + if (channel_limit > _rc_max_chan_count) { + channel_limit = _rc_max_chan_count; + } + + /* read out and scale values from raw message even if signal is invalid */ + for (unsigned int i = 0; i < channel_limit; i++) { + + /* + * 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.channels[i] = (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])) { + _rc.channels[i] = (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.channels[i] = 0.0f; + } + + _rc.channels[i] *= _parameters.rev[i]; + + /* handle any parameter-induced blowups */ +#ifdef __PX4_NUTTX + if (!isfinite(_rc.channels[i])) { +#else + if (!std::isfinite(_rc.channels[i])) { +#endif + _rc.channels[i] = 0.0f; + } + } + + _rc.channel_count = rc_input.channel_count; + _rc.rssi = rc_input.rssi; + _rc.signal_lost = signal_lost; + _rc.timestamp = rc_input.timestamp_last_signal; + + /* publish rc_channels topic even if signal is invalid, for debug */ + if (_rc_pub > 0) { + orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); + + } else { + _rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc); + } + + if (!signal_lost) { + struct manual_control_setpoint_s manual; + memset(&manual, 0 , sizeof(manual)); + + /* fill values in manual_control_setpoint topic only if signal is valid */ + manual.timestamp = rc_input.timestamp_last_signal; + + /* limit controls */ + manual.y = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); + manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); + manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); + manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); + manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); + manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); + manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); + manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); + manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); + manual.aux5 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); + + /* mode switches */ + manual.mode_switch = get_rc_sw3pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.posctl_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); + manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); + manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); + + /* publish manual_control_setpoint topic */ + if (_manual_control_pub > 0) { + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual); + + } else { + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); + } + + /* copy from mapped manual control to control group 3 */ + struct actuator_controls_s actuator_group_3; + memset(&actuator_group_3, 0 , sizeof(actuator_group_3)); + + actuator_group_3.timestamp = rc_input.timestamp_last_signal; + + actuator_group_3.control[0] = manual.y; + actuator_group_3.control[1] = manual.x; + actuator_group_3.control[2] = manual.r; + actuator_group_3.control[3] = manual.z; + actuator_group_3.control[4] = manual.flaps; + actuator_group_3.control[5] = manual.aux1; + actuator_group_3.control[6] = manual.aux2; + actuator_group_3.control[7] = manual.aux3; + + /* publish actuator_controls_3 topic */ + if (_actuator_group_3_pub > 0) { + orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + + } else { + _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); + } + + /* Update parameters from RC Channels (tuning with RC) if activated */ + static hrt_abstime last_rc_to_param_map_time = 0; + + if (hrt_elapsed_time(&last_rc_to_param_map_time) > 1e6) { + set_params_from_rc(); + last_rc_to_param_map_time = hrt_absolute_time(); + } + } + } +} + +void +Sensors::task_main_trampoline(int argc, char *argv[]) +{ + sensors::g_sensors->task_main(); +} + +void +Sensors::task_main() +{ + + /* start individual sensors */ + int ret; + ret = accel_init(); + + if (ret) { + goto exit_immediate; + } + + ret = gyro_init(); + + if (ret) { + goto exit_immediate; + } + + ret = mag_init(); + + if (ret) { + goto exit_immediate; + } + + ret = baro_init(); + + if (ret) { + goto exit_immediate; + } + + ret = adc_init(); + + if (ret) { + goto exit_immediate; + } + + /* + * do subscriptions + */ + _gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); + _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); + _mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0); + _gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1); + _accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1); + _mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1); + _gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2); + _accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2); + _mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2); + _rc_sub = orb_subscribe(ORB_ID(input_rc)); + _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); + _baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1); + _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map)); + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vcontrol_mode_sub, 200); + + /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ + orb_set_interval(_gyro_sub, 4); + + /* + * do advertisements + */ + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + raw.timestamp = hrt_absolute_time(); + raw.adc_voltage_v[0] = 0.0f; + raw.adc_voltage_v[1] = 0.0f; + raw.adc_voltage_v[2] = 0.0f; + raw.adc_voltage_v[3] = 0.0f; + + /* set high initial error counts to deselect gyros */ + raw.gyro_errcount = 100000; + raw.gyro1_errcount = 100000; + raw.gyro2_errcount = 100000; + + /* set high initial error counts to deselect accels */ + raw.accelerometer_errcount = 100000; + raw.accelerometer1_errcount = 100000; + raw.accelerometer2_errcount = 100000; + + /* set high initial error counts to deselect mags */ + raw.magnetometer_errcount = 100000; + raw.magnetometer1_errcount = 100000; + raw.magnetometer2_errcount = 100000; + + memset(&_battery_status, 0, sizeof(_battery_status)); + _battery_status.voltage_v = -1.0f; + _battery_status.voltage_filtered_v = -1.0f; + _battery_status.current_a = -1.0f; + _battery_status.discharged_mah = -1.0f; + + /* get a set of initial values */ + accel_poll(raw); + gyro_poll(raw); + mag_poll(raw); + baro_poll(raw); + diff_pres_poll(raw); + + parameter_update_poll(true /* forced */); + rc_parameter_map_poll(true /* forced */); + + /* advertise the sensor_combined topic and make the initial publication */ + _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); + + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ + fds[0].fd = _gyro_sub; + fds[0].events = POLLIN; + + _task_should_exit = false; + + while (!_task_should_exit) { + + /* wait for up to 50ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); + + /* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */ + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* check vehicle status for changes to publication state */ + vehicle_control_mode_poll(); + + /* the timestamp of the raw struct is updated by the gyro_poll() method */ + /* copy most recent sensor data */ + gyro_poll(raw); + accel_poll(raw); + mag_poll(raw); + baro_poll(raw); + + /* work out if main gyro timed out and fail over to alternate gyro */ + if (hrt_elapsed_time(&raw.timestamp) > 20 * 1000) { + + /* if the secondary failed as well, go to the tertiary */ + if (hrt_elapsed_time(&raw.gyro1_timestamp) > 20 * 1000) { + fds[0].fd = _gyro2_sub; + } else { + fds[0].fd = _gyro1_sub; + } + } + + /* check battery voltage */ + adc_poll(raw); + + diff_pres_poll(raw); + + /* Inform other processes that new data is available to copy */ + if (_publishing) { + orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); + } + + /* check parameters for updates */ + parameter_update_poll(); + + /* check rc parameter map for updates */ + rc_parameter_map_poll(); + + /* Look for new r/c input data */ + rc_poll(); + + perf_end(_loop_perf); + } + + warnx("exiting."); + +exit_immediate: + _sensors_task = -1; + px4_task_exit(ret); +} + +int +Sensors::start() +{ + ASSERT(_sensors_task == -1); + + /* start the task */ + _sensors_task = task_spawn_cmd("sensors_task", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + (px4_main_t)&Sensors::task_main_trampoline, + nullptr); + + /* wait until the task is up and running or has failed */ + while (_sensors_task > 0 && _task_should_exit) { + usleep(100); + } + + if (_sensors_task < 0) { + return -ERROR; + } + + return OK; +} + +int sensors_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: sensors {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (sensors::g_sensors != nullptr) { + warnx("already running"); + return 0; + } + + sensors::g_sensors = new Sensors; + + if (sensors::g_sensors == nullptr) { + warnx("alloc failed"); + return 1; + } + + if (OK != sensors::g_sensors->start()) { + delete sensors::g_sensors; + sensors::g_sensors = nullptr; + warnx("start failed"); + return 1; + } + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + if (sensors::g_sensors == nullptr) { + warnx("not running"); + return 1; + } + + delete sensors::g_sensors; + sensors::g_sensors = nullptr; + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (sensors::g_sensors) { + warnx("is running"); + return 0; + + } else { + warnx("not running"); + return 1; + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors_nuttx.cpp similarity index 100% rename from src/modules/sensors/sensors.cpp rename to src/modules/sensors/sensors_nuttx.cpp diff --git a/src/platforms/linux/include/queue.h b/src/platforms/linux/include/queue.h index c65f760a2d..4d95541e02 100644 --- a/src/platforms/linux/include/queue.h +++ b/src/platforms/linux/include/queue.h @@ -96,14 +96,6 @@ struct dq_queue_s }; typedef struct dq_queue_s dq_queue_t; -// FIXME - hack for mavlink until kernel work queue can be removed -#define LPWORK 1 -typedef struct { - dq_entry_t dq; -} work_s; -inline void work_queue(int worktype, work_s *dummy, void (*_worker_trampoline)(void *arg), void *req, int dummy2); -inline void work_queue(int worktype, work_s *dummy, void (*_worker_trampoline)(void *arg), void *req, int dummy2) {} - /************************************************************************ * Global Function Prototypes ************************************************************************/ diff --git a/src/platforms/linux/main.cpp b/src/platforms/linux/main.cpp index a0b2104150..790f8d6cba 100644 --- a/src/platforms/linux/main.cpp +++ b/src/platforms/linux/main.cpp @@ -41,6 +41,7 @@ #include #include #include +#include using namespace std; @@ -48,16 +49,22 @@ typedef int (*px4_main_t)(int argc, char *argv[]); #include "apps.h" -// FIXME - the code below only passes 1 arg for now -void run_cmd(const string &command, const string &apparg); -void run_cmd(const string &command, const string &apparg) { - const char *arg[3]; - +void run_cmd(const vector &appargs); +void run_cmd(const vector &appargs) { + // command is appargs[0] + string command = appargs[0]; + cout << "appargs.size() = " << appargs.size() << endl; if (apps.find(command) != apps.end()) { - arg[0] = (char *)command.c_str(); - arg[1] = (char *)apparg.c_str(); - arg[2] = (char *)0; - apps[command](2,(char **)arg); + const char *arg[appargs.size()+2]; + + unsigned int i = 0; + while (i < appargs.size() && appargs[i] != "") { + arg[i] = (char *)appargs[i].c_str(); + ++i; + } + arg[i] = (char *)0; + cout << command << " " << i << endl; + apps[command](i,(char **)arg); } else { @@ -65,12 +72,22 @@ void run_cmd(const string &command, const string &apparg) { } } +void process_line(string &line) +{ + vector appargs(5); + + stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4]; + cout << "Command " << appargs[0] << endl; + unsigned int i = 1; + while ( i < appargs.size() && appargs[i] != "") { + cout << " appargs[" << i << "] = " << appargs[i] << endl; + ++i; + } + run_cmd(appargs); +} + int main(int argc, char **argv) { - string mystr; - string command; - string apparg; - // Execute a command list of provided if (argc == 2) { ifstream infile(argv[1]); @@ -78,21 +95,21 @@ int main(int argc, char **argv) //vector tokens; for (string line; getline(infile, line, '\n'); ) { - stringstream(line) >> command >> apparg; - cout << "Command " << command << ", apparg " << apparg << endl; - run_cmd(command, apparg); + process_line(line); } } + string mystr; + vector appargs(2); + + appargs[0] = "list_builtins"; + while(1) { - run_cmd("list_builtins", ""); + run_cmd(appargs); cout << "Enter a command and its args:" << endl; getline (cin,mystr); - stringstream(mystr) >> command >> apparg; - run_cmd(command, apparg); + process_line(mystr); mystr = ""; - command = ""; - apparg = ""; } } diff --git a/src/platforms/linux/px4_layer/dq_addlast.c b/src/platforms/linux/px4_layer/dq_addlast.c new file mode 100644 index 0000000000..3ef08abd05 --- /dev/null +++ b/src/platforms/linux/px4_layer/dq_addlast.c @@ -0,0 +1,74 @@ +/************************************************************ + * libc/queue/dq_addlast.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: dq_addlast + * + * Description + * dq_addlast adds 'node' to the end of 'queue' + * + ************************************************************/ + +void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue) +{ + node->flink = NULL; + node->blink = queue->tail; + + if (!queue->head) + { + queue->head = node; + queue->tail = node; + } + else + { + queue->tail->flink = node; + queue->tail = node; + } +} + diff --git a/src/platforms/linux/px4_layer/dq_remfirst.c b/src/platforms/linux/px4_layer/dq_remfirst.c new file mode 100644 index 0000000000..e87acc3382 --- /dev/null +++ b/src/platforms/linux/px4_layer/dq_remfirst.c @@ -0,0 +1,82 @@ +/************************************************************ + * libc/queue/dq_remfirst.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: dq_remfirst + * + * Description: + * dq_remfirst removes 'node' from the head of 'queue' + * + ************************************************************/ + +FAR dq_entry_t *dq_remfirst(dq_queue_t *queue) +{ + FAR dq_entry_t *ret = queue->head; + + if (ret) + { + FAR dq_entry_t *next = ret->flink; + if (!next) + { + queue->head = NULL; + queue->tail = NULL; + } + else + { + queue->head = next; + next->blink = NULL; + } + + ret->flink = NULL; + ret->blink = NULL; + } + + return ret; +} + diff --git a/src/platforms/linux/px4_layer/drv_hrt.c b/src/platforms/linux/px4_layer/drv_hrt.c index d501236d57..ad6aee4748 100644 --- a/src/platforms/linux/px4_layer/drv_hrt.c +++ b/src/platforms/linux/px4_layer/drv_hrt.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,14 +32,16 @@ ****************************************************************************/ /** - * @file drv_hrt.h + * @file drv_hrt.c * * High-resolution timer with callouts and timekeeping. */ #include +#include +#include -static hrt_abstime dummy_timer = 0; +static struct sq_queue_s callout_queue; /* latency histogram */ #define LATENCY_BUCKET_COUNT 8 @@ -47,12 +49,20 @@ __EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; __EXPORT const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; __EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; +static void hrt_call_reschedule(void); + +#define HRT_INTERVAL_MIN 50 +#define HRT_INTERVAL_MAX 50000 + /* * Get absolute time. */ hrt_abstime hrt_absolute_time(void) { - return dummy_timer; + struct timespec ts; + + clock_gettime(CLOCK_MONOTONIC, &ts); + return ts_to_abstime(&ts); } /* @@ -60,7 +70,12 @@ hrt_abstime hrt_absolute_time(void) */ hrt_abstime ts_to_abstime(struct timespec *ts) { - return dummy_timer; + hrt_abstime result; + + result = (hrt_abstime)(ts->tv_sec) * 1000000; + result += ts->tv_nsec / 1000; + + return result; } /* @@ -77,7 +92,8 @@ void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); */ hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) { - return dummy_timer; + hrt_abstime delta = hrt_absolute_time() - *then; + return delta; } /* @@ -87,35 +103,10 @@ hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) */ hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now) { - return dummy_timer; + hrt_abstime ts = hrt_absolute_time(); + return ts; } -/* - * Call callout(arg) after delay has elapsed. - * - * If callout is NULL, this can be used to implement a timeout by testing the call - * with hrt_called(). - */ -void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) -{ -} - -/* - * Call callout(arg) at absolute time calltime. - */ -void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) -{ -} - -/* - * Call callout(arg) after delay, and then after every interval. - * - * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may - * jitter but should not drift. - */ -void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) -{ -} /* * If this returns true, the entry has been invoked and removed from the callout list, @@ -125,7 +116,7 @@ void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime inter */ bool hrt_called(struct hrt_call *entry) { - return true; + return (entry->deadline == 0); } /* @@ -133,6 +124,15 @@ bool hrt_called(struct hrt_call *entry) */ void hrt_cancel(struct hrt_call *entry) { + // FIXME - need a lock + sq_rem(&entry->link, &callout_queue); + entry->deadline = 0; + + /* if this is a periodic call being removed by the callout, prevent it from + * being re-entered when the callout returns. + */ + entry->period = 0; + // endif } /* @@ -140,6 +140,7 @@ void hrt_cancel(struct hrt_call *entry) */ void hrt_call_init(struct hrt_call *entry) { + memset(entry, 0, sizeof(*entry)); } /* @@ -151,6 +152,7 @@ void hrt_call_init(struct hrt_call *entry) */ void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) { + entry->deadline = hrt_absolute_time() + delay; } /* @@ -158,5 +160,132 @@ void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) */ void hrt_init(void) { + sq_init(&callout_queue); +} + +static void +hrt_call_enter(struct hrt_call *entry) +{ + struct hrt_call *call, *next; + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if ((call == NULL) || (entry->deadline < call->deadline)) { + sq_addfirst(&entry->link, &callout_queue); + //lldbg("call enter at head, reschedule\n"); + /* we changed the next deadline, reschedule the timer event */ + hrt_call_reschedule(); + + } else { + do { + next = (struct hrt_call *)sq_next(&call->link); + + if ((next == NULL) || (entry->deadline < next->deadline)) { + //lldbg("call enter after head\n"); + sq_addafter(&call->link, &entry->link, &callout_queue); + break; + } + } while ((call = next) != NULL); + } + + //lldbg("scheduled\n"); +} + +/** + * Reschedule the next timer interrupt. + * + * This routine must be called with interrupts disabled. + */ +static void +hrt_call_reschedule() +{ + hrt_abstime now = hrt_absolute_time(); + struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); + hrt_abstime deadline = now + HRT_INTERVAL_MAX; + + /* + * Determine what the next deadline will be. + * + * Note that we ensure that this will be within the counter + * period, so that when we truncate all but the low 16 bits + * the next time the compare matches it will be the deadline + * we want. + * + * It is important for accurate timekeeping that the compare + * interrupt fires sufficiently often that the base_time update in + * hrt_absolute_time runs at least once per timer period. + */ + if (next != NULL) { + //lldbg("entry in queue\n"); + if (next->deadline <= (now + HRT_INTERVAL_MIN)) { + //lldbg("pre-expired\n"); + /* set a minimal deadline so that we call ASAP */ + deadline = now + HRT_INTERVAL_MIN; + + } else if (next->deadline < deadline) { + //lldbg("due soon\n"); + deadline = next->deadline; + } + } +} + +static void +hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) +{ + /* if the entry is currently queued, remove it */ + /* note that we are using a potentially uninitialised + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialised entry->link but we don't do + anything actually unsafe. + */ + if (entry->deadline != 0) + sq_rem(&entry->link, &callout_queue); + + entry->deadline = deadline; + entry->period = interval; + entry->callout = callout; + entry->arg = arg; + + hrt_call_enter(entry); +} + +/* + * Call callout(arg) after delay has elapsed. + * + * If callout is NULL, this can be used to implement a timeout by testing the call + * with hrt_called(). + */ +void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + 0, + callout, + arg); +} + +/* + * Call callout(arg) after delay, and then after every interval. + * + * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may + * jitter but should not drift. + */ +void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + interval, + callout, + arg); +} + +/* + * Call callout(arg) at absolute time calltime. + */ +void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, calltime, 0, callout, arg); } diff --git a/src/platforms/linux/px4_layer/module.mk b/src/platforms/linux/px4_layer/module.mk index 2dd89b0216..1d987418ac 100644 --- a/src/platforms/linux/px4_layer/module.mk +++ b/src/platforms/linux/px4_layer/module.mk @@ -41,7 +41,10 @@ SRCS = \ lib_crc32.c \ drv_hrt.c \ queue.c \ + dq_addlast.c \ + dq_remfirst.c \ sq_addlast.c \ - sq_remfirst.c + sq_remfirst.c \ + sq_addafter.c MAXOPTIMIZATION = -Os diff --git a/src/platforms/linux/px4_layer/px4_linux_impl.cpp b/src/platforms/linux/px4_layer/px4_linux_impl.cpp index e94d51f288..059e1c9176 100644 --- a/src/platforms/linux/px4_layer/px4_linux_impl.cpp +++ b/src/platforms/linux/px4_layer/px4_linux_impl.cpp @@ -37,9 +37,12 @@ * PX4 Middleware Wrapper Linux Implementation */ +#include #include +#include #include #include +#include #include "systemlib/param/param.h" __BEGIN_DECLS @@ -49,6 +52,8 @@ struct param_info_s param_array[256]; struct param_info_s *param_info_base; struct param_info_s *param_info_limit; +long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK); + __END_DECLS namespace px4 @@ -92,3 +97,19 @@ void init(int argc, char *argv[], const char *app_name) } } + + + + +int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay) +{ + printf("work_queue: UNIMPLEMENTED\n"); + return PX4_OK; +} + +int work_cancel(int qid, struct work_s *work) +{ + printf("work_cancel: UNIMPLEMENTED\n"); + return PX4_OK; +} + diff --git a/src/platforms/linux/px4_layer/px4_linux_tasks.c b/src/platforms/linux/px4_layer/px4_linux_tasks.c index 0e069dd990..f62f4227ff 100644 --- a/src/platforms/linux/px4_layer/px4_linux_tasks.c +++ b/src/platforms/linux/px4_layer/px4_linux_tasks.c @@ -151,21 +151,14 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int int px4_task_delete(px4_task_t id) { int rv = 0; - int i; pthread_t pid; //printf("Called px4_task_delete\n"); - // Get pthread ID from the opaque ID - for (i=0; i=PX4_MAX_TASKS) { + if (id < PX4_MAX_TASKS && taskmap[id] != 0) + pid = taskmap[id]; + else return -EINVAL; - } + // If current thread then exit, otherwise cancel if (pthread_self() == pid) { pthread_exit(0); @@ -178,6 +171,25 @@ int px4_task_delete(px4_task_t id) return rv; } +void px4_task_exit(int ret) +{ + int i; + pthread_t pid = pthread_self(); + + // Get pthread ID from the opaque ID + for (i=0; i=PX4_MAX_TASKS) + printf("px4_task_exit: self task not found!\n"); + + pthread_exit((void *)(unsigned long)ret); +} + void px4_killall(void) { //printf("Called px4_killall\n"); diff --git a/src/platforms/linux/px4_layer/sq_addafter.c b/src/platforms/linux/px4_layer/sq_addafter.c new file mode 100644 index 0000000000..5d47feba0f --- /dev/null +++ b/src/platforms/linux/px4_layer/sq_addafter.c @@ -0,0 +1,71 @@ +/************************************************************ + * libc/queue/sq_addafter.c + * + * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************/ + +/************************************************************ + * Compilation Switches + ************************************************************/ + +/************************************************************ + * Included Files + ************************************************************/ + +#include + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: sq_addafter.c + * + * Description: + * The sq_addafter function adds 'node' after 'prev' in the + * 'queue.' + * + ************************************************************/ + +void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, + sq_queue_t *queue) +{ + if (!queue->head || prev == queue->tail) + { + sq_addlast(node, queue); + } + else + { + node->flink = prev->flink; + prev->flink = node; + } +} diff --git a/src/drivers/device/vdevice.cpp b/src/platforms/px4_adc.h similarity index 59% rename from src/drivers/device/vdevice.cpp rename to src/platforms/px4_adc.h index 487249aed5..0b123ed0c0 100644 --- a/src/drivers/device/vdevice.cpp +++ b/src/platforms/px4_adc.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014 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 @@ -32,76 +32,35 @@ ****************************************************************************/ /** - * @file device.cpp + * @file px4_adc.h * - * Fundamental driver base class for the virtual device framework. + * ADC header depending on the build target */ -#include "device.h" +#pragma once -#include -#include -#include +#if defined(__PX4_ROS) +#error "ADC not supported in ROS" +#elif defined(__PX4_NUTTX) +/* + * Building for NuttX + */ +#include +#elif defined(__PX4_LINUX) -namespace device +// FIXME - this needs to be a px4_adc_msg_s type +// Curently copied from NuttX +struct adc_msg_s { + uint8_t am_channel; /* The 8-bit ADC Channel */ + int32_t am_data; /* ADC convert result (4 bytes) */ +} packed_struct; -Device::Device(const char *name) : - // public - // protected - _name(name), - _debug_enabled(false) -{ - sem_init(&_lock, 0, 1); - - /* setup a default device ID. When bus_type is UNKNOWN the - other fields are invalid */ - _device_id.devid = 0; - _device_id.devid_s.bus_type = DeviceBusType_UNKNOWN; - _device_id.devid_s.bus = 0; - _device_id.devid_s.address = 0; - _device_id.devid_s.devtype = 0; -} +// Example settings +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL -1 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 -Device::~Device() -{ - sem_destroy(&_lock); -} - -int -Device::init() -{ - int ret = OK; - - return ret; -} - -void -Device::log(const char *fmt, ...) -{ - va_list ap; - - printf("[%s] ", _name); - va_start(ap, fmt); - vprintf(fmt, ap); - va_end(ap); - printf("\n"); - fflush(stdout); -} - -void -Device::debug(const char *fmt, ...) -{ - va_list ap; - - if (_debug_enabled) { - printf("<%s> ", _name); - va_start(ap, fmt); - vprintf(fmt, ap); - va_end(ap); - printf("\n"); - fflush(stdout); - } -} - -} // namespace device +#else +#error "No target platform defined" +#endif diff --git a/src/platforms/px4_config.h b/src/platforms/px4_config.h index 83bcb8882d..ac5a04c277 100644 --- a/src/platforms/px4_config.h +++ b/src/platforms/px4_config.h @@ -44,4 +44,10 @@ #include #elif defined (__PX4_LINUX) #define CONFIG_NFILE_STREAMS 1 + +extern long PX4_TICKS_PER_SEC; +#define USEC2TICKS(x) (PX4_TICKS_PER_SEC*(long)x/1000000L) + +#define px4_errx(x, ...) errx(x, __VA_ARGS__) + #endif diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index ab41625d5e..6a4b1f2cff 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -45,6 +45,8 @@ #define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) #define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) +#define PX4_ERROR (-1) +#define PX4_OK 0 #if defined(__PX4_ROS) /* @@ -111,6 +113,7 @@ typedef param_t px4_param_t; /* FIXME - Used to satisfy build */ #define UNIQUE_ID 0x1FFF7A10 //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register) +#define USEC2TICK(x) 0 #define getreg32(a) (*(volatile uint32_t *)(a)) diff --git a/src/platforms/px4_i2c.h b/src/platforms/px4_i2c.h new file mode 100644 index 0000000000..9270cb203b --- /dev/null +++ b/src/platforms/px4_i2c.h @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. 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 px4_i2c.h + * + * Includes device headers depending on the build target + */ + +#pragma once + +#define PX4_I2C_M_READ 0x0001 /* read data, from slave to master */ + +#if defined(__PX4_ROS) +#error "Devices not supported in ROS" +#elif defined (__PX4_NUTTX) +/* + * Building for NuttX + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "up_internal.h" +#include "up_arch.h" + +#define px4_i2c_msg_t i2c_msg_s + +typedef struct i2c_dev_s px4_i2c_dev_t; + +#define px4_i2cuninitialize(x) up_i2cuninitialize(x) +#define px4_i2cinitialize(x) up_i2cinitialize(x) +#define px4_i2creset(x) up_i2creset(x) + +#define px4_interrupt_context() up_interrupt_context() + +#elif defined(__PX4_LINUX) +#include + +#define I2C_M_READ 0x0001 /* read data, from slave to master */ +#define I2C_M_TEN 0x0002 /* ten bit address */ +#define I2C_M_NORESTART 0x0080 /* message should not begin with (re-)start of transfer */ + +// NOTE - This is a copy of the NuttX i2c_msg_s structure +typedef struct { + uint16_t addr; /* Slave address */ + uint16_t flags; /* See I2C_M_* definitions */ + uint8_t *buffer; + int length; +} px4_i2c_msg_t; + +struct px4_i2c_ops_t; +// NOTE - This is a copy of the NuttX i2c_ops_s structure +typedef struct { + const struct px4_i2c_ops_t *ops; /* I2C vtable */ +} px4_i2c_dev_t; + +// FIXME - Stub implementations +inline void px4_i2cuninitialize(px4_i2c_dev_t *dev); +inline void px4_i2cuninitialize(px4_i2c_dev_t *dev) {} + +inline px4_i2c_dev_t *px4_i2cinitialize(int bus); +inline px4_i2c_dev_t *px4_i2cinitialize(int bus) { return (px4_i2c_dev_t *)0; } + +inline void px4_i2creset(px4_i2c_dev_t *dev); +inline void px4_i2creset(px4_i2c_dev_t *dev) { } + +inline bool px4_interrupt_context(void); +inline bool px4_interrupt_context(void) { return false; } + +// FIXME - Empty defines for I2C ops +// Original version commented out +//#define I2C_SETFREQUENCY(d,f) ((d)->ops->setfrequency(d,f)) +#define I2C_SETFREQUENCY(d,f) +//#define SPI_SELECT(d,id,s) ((d)->ops->select(d,id,s)) +#define SPI_SELECT(d,id,s) + +// FIXME - Stub implementation +// Original version commented out +//#define I2C_TRANSFER(d,m,c) ((d)->ops->transfer(d,m,c)) +inline int I2C_TRANSFER(px4_i2c_dev_t *dev, px4_i2c_msg_t *msg, int count); +inline int I2C_TRANSFER(px4_i2c_dev_t *dev, px4_i2c_msg_t *msg, int count) { return 0; } + +struct i2c_msg_s +{ + uint16_t addr; /* Slave address */ + uint16_t flags; /* See I2C_M_* definitions */ + uint8_t *buffer; + int length; +}; +#else +#error "No target platform defined" +#endif diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index 0e732df4b8..ea56c3425c 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -39,6 +39,7 @@ #pragma once +#include #include #include #include @@ -48,12 +49,11 @@ #define PX4_F_RDONLY 1 #define PX4_F_WRONLY 2 -#define PX4_DIOC_GETPRIV 1 -#define PX4_DEVIOCSPUBBLOCK 2 -#define PX4_DEVIOCGPUBBLOCK 3 +#define PX4_DEVIOCGDEVICEID 1 -#define PX4_ERROR (-1) -#define PX4_OK 0 +#define PX4_DIOC_GETPRIV 2 +#define PX4_DEVIOCSPUBBLOCK 3 +#define PX4_DEVIOCGPUBBLOCK 4 //#define PX4_DEBUG(...) #define PX4_DEBUG(...) printf(__VA_ARGS__) diff --git a/src/platforms/px4_spi.h b/src/platforms/px4_spi.h new file mode 100644 index 0000000000..41289aba03 --- /dev/null +++ b/src/platforms/px4_spi.h @@ -0,0 +1,35 @@ +#pragma once + +#ifdef __PX4_NUTTX +#include +#elif defined(__PX4_LINUX) +enum spi_dev_e +{ + SPIDEV_NONE = 0, /* Not a valid value */ + SPIDEV_MMCSD, /* Select SPI MMC/SD device */ + SPIDEV_FLASH, /* Select SPI FLASH device */ + SPIDEV_ETHERNET, /* Select SPI ethernet device */ + SPIDEV_DISPLAY, /* Select SPI LCD/OLED display device */ + SPIDEV_WIRELESS, /* Select SPI Wireless device */ + SPIDEV_TOUCHSCREEN, /* Select SPI touchscreen device */ + SPIDEV_EXPANDER, /* Select SPI I/O expander device */ + SPIDEV_MUX, /* Select SPI multiplexer device */ + SPIDEV_AUDIO_DATA, /* Select SPI audio codec device data port */ + SPIDEV_AUDIO_CTRL, /* Select SPI audio codec device control port */ +}; + +/* Certain SPI devices may required differnt clocking modes */ + +enum spi_mode_e +{ + SPIDEV_MODE0 = 0, /* CPOL=0 CHPHA=0 */ + SPIDEV_MODE1, /* CPOL=0 CHPHA=1 */ + SPIDEV_MODE2, /* CPOL=1 CHPHA=0 */ + SPIDEV_MODE3 /* CPOL=1 CHPHA=1 */ +}; +struct spi_dev_s +{ + int unused; +}; +#else +#endif diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 18a6ced79e..3a9adfd37e 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -46,6 +46,9 @@ #error "PX4 tasks not supported in ROS" #elif defined(__PX4_NUTTX) typedef int px4_task_t; + +#define px4_task_exit(x) _exit(x) + #elif defined(__PX4_LINUX) #include #include @@ -83,6 +86,7 @@ __EXPORT px4_task_t px4_task_spawn_cmd(const char *name, char * const argv[]); __EXPORT int px4_task_delete(px4_task_t pid); +__EXPORT void px4_task_exit(int ret); __END_DECLS diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h new file mode 100644 index 0000000000..b482022a60 --- /dev/null +++ b/src/platforms/px4_workqueue.h @@ -0,0 +1,116 @@ +/**************************************************************************** + * include/nuttx/wqueue.h + * + * Copyright (C) 2009, 2011-2013 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +#pragma once + +#ifdef __PX4_ROS +#error "Work queue not supported on ROS +#elif defined(__PX4_NUTTX) +#include +#include +#include +#elif defined(__PX4_LINUX) + +#include + +#define LPWORK 1 +#define HPWORK 2 + +/* Defines the work callback */ + +typedef void (*worker_t)(void *arg); + +struct work_s +{ + struct dq_entry_s dq; /* Implements a doubly linked list */ + worker_t worker; /* Work callback */ + void *arg; /* Callback argument */ + uint32_t qtime; /* Time work queued */ + uint32_t delay; /* Delay until work performed */ +}; + +/**************************************************************************** + * Name: work_queue + * + * Description: + * Queue work to be performed at a later time. All queued work will be + * performed on the worker thread of of execution (not the caller's). + * + * The work structure is allocated by caller, but completely managed by + * the work queue logic. The caller should never modify the contents of + * the work queue structure; the caller should not call work_queue() + * again until either (1) the previous work has been performed and removed + * from the queue, or (2) work_cancel() has been called to cancel the work + * and remove it from the work queue. + * + * Input parameters: + * qid - The work queue ID + * work - The work structure to queue + * worker - The worker callback to be invoked. The callback will invoked + * on the worker thread of execution. + * arg - The argument that will be passed to the workder callback when + * int is invoked. + * delay - Delay (in clock ticks) from the time queue until the worker + * is invoked. Zero means to perform the work immediately. + * + * Returned Value: + * Zero on success, a negated errno on failure + * + ****************************************************************************/ + +int work_queue(int qid, FAR struct work_s *work, worker_t worker, + FAR void *arg, uint32_t delay); + +/**************************************************************************** + * Name: work_cancel + * + * Description: + * Cancel previously queued work. This removes work from the work queue. + * After work has been canceled, it may be re-queue by calling work_queue() + * again. + * + * Input parameters: + * qid - The work queue ID + * work - The previously queue work structure to cancel + * + * Returned Value: + * Zero on success, a negated errno on failure + * + ****************************************************************************/ + +int work_cancel(int qid, FAR struct work_s *work); +#else +#error "Unknown target OS" +#endif