diff options
25 files changed, 452 insertions, 151 deletions
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index d5a50cfae..4f997e96b 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -18,6 +18,12 @@ MODULES += drivers/stm32/tone_alarm MODULES += drivers/led MODULES += drivers/boards/px4fmu-v2 MODULES += drivers/px4io +MODULES += drivers/rgbled +MODULES += drivers/mpu6000 +MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 +MODULES += drivers/hmc5883 +MODULES += drivers/ms5611 MODULES += systemcmds/perf MODULES += systemcmds/reboot MODULES += systemcmds/tests @@ -31,6 +37,9 @@ MODULES += systemcmds/hw_ver MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/uORB +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter # # Transitional support - add commands from the NuttX export archive. diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index f73a3ef01..771f2128f 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 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 @@ -86,6 +86,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : _collect_phase(false), _diff_pres_offset(0.0f), _airspeed_pub(-1), + _class_instance(-1), _conversion_interval(conversion_interval), _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) @@ -102,6 +103,9 @@ Airspeed::~Airspeed() /* make sure we are truly inactive */ stop(); + if (_class_instance != -1) + unregister_class_devname(AIRSPEED_DEVICE_PATH, _class_instance); + /* free any existing reports */ if (_reports != nullptr) delete _reports; @@ -126,13 +130,23 @@ Airspeed::init() if (_reports == nullptr) goto out; - /* get a publish handle on the airspeed topic */ - differential_pressure_s zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report); + /* register alternate interfaces if we have to */ + _class_instance = register_class_devname(AIRSPEED_DEVICE_PATH); + + /* publication init */ + if (_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct differential_pressure_s arp; + measure(); + _reports->get(&arp); - if (_airspeed_pub < 0) - warnx("failed to create airspeed sensor object. Did you start uOrb?"); + /* measurement will have generated a report, publish */ + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); + + if (_airspeed_pub < 0) + warnx("failed to create airspeed sensor object. uORB started?"); + } ret = OK; /* sensor is ok, but we don't really know if it is within range */ diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index c341aa2c6..c27b1bcd8 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 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 @@ -127,6 +127,8 @@ protected: orb_advert_t _airspeed_pub; + int _class_instance; + unsigned _conversion_interval; perf_counter_t _sample_perf; diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index 1590cc182..e43a34805 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -153,6 +153,7 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; + int _class_instance; unsigned _current_lowpass; unsigned _current_range; @@ -238,6 +239,7 @@ BMA180::BMA180(int bus, spi_dev_e device) : _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _class_instance(-1), _current_lowpass(0), _current_range(0), _sample_perf(perf_alloc(PC_ELAPSED, "bma180_read")) @@ -282,11 +284,6 @@ BMA180::init() if (_reports == nullptr) goto out; - /* advertise sensor topic */ - struct accel_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - /* perform soft reset (p48) */ write_reg(ADDR_RESET, SOFT_RESET); @@ -322,6 +319,19 @@ BMA180::init() ret = ERROR; } + _class_instance = register_class_devname(ACCEL_DEVICE_PATH); + + /* advertise sensor topic, measure manually to initialize valid report */ + measure(); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + struct accel_report arp; + _reports->get(&arp); + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp); + } + out: return ret; } @@ -723,7 +733,8 @@ BMA180::measure() poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &report); + if (_accel_topic > 0 && !(_pub_blocked)) + orb_publish(ORB_ID(sensor_accel), _accel_topic, &report); /* stop the perf counter */ perf_end(_sample_perf); diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 7954ce5ab..b157b3f18 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -38,6 +38,7 @@ */ #include "device.h" +#include "drivers/drv_device.h" #include <sys/ioctl.h> #include <arch/irq.h> @@ -93,6 +94,7 @@ CDev::CDev(const char *name, Device(name, irq), // public // protected + _pub_blocked(false), // private _devname(devname), _registered(false), @@ -256,6 +258,14 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg) case DIOC_GETPRIV: *(void **)(uintptr_t)arg = (void *)this; return OK; + break; + case DEVIOCSPUBBLOCK: + _pub_blocked = (arg != 0); + return OK; + break; + case DEVIOCGPUBBLOCK: + return _pub_blocked; + break; } return -ENOTTY; diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index c3ee77b1c..683455149 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 0235f6284..d99f22922 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -415,6 +415,8 @@ protected: */ virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); + bool _pub_blocked; /**< true if publishing should be blocked */ + private: static const unsigned _max_pollwaiters = 8; diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h new file mode 100644 index 000000000..b310beb74 --- /dev/null +++ b/src/drivers/drv_device.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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 drv_device.h + * + * Generic device / sensor interface. + */ + +#ifndef _DRV_DEVICE_H +#define _DRV_DEVICE_H + +#include <stdint.h> +#include <sys/ioctl.h> + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +/* + * ioctl() definitions + */ + +#define _DEVICEIOCBASE (0x100) +#define _DEVICEIOC(_n) (_IOC(_DEVICEIOCBASE, _n)) + +/** ask device to stop publishing */ +#define DEVIOCSPUBBLOCK _DEVICEIOC(0) + +/** check publication block status */ +#define DEVIOCGPUBBLOCK _DEVICEIOC(1) + +#endif /* _DRV_DEVICE_H */ diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index de371bf32..8bbef5cfa 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 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 @@ -184,8 +184,10 @@ ETSAirspeed::collect() report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; - /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + if (_airspeed_pub > 0 && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + } new_report(report); diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 6b72d560f..a736cbdf6 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -289,11 +289,13 @@ GPS::task_main() //no time and satellite information simulated - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + if (!(_pub_blocked)) { + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } } usleep(2e5); @@ -330,11 +332,14 @@ GPS::task_main() while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + if (!(_pub_blocked)) { + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } } last_rate_count++; diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 2e2cbc8dd..2360ff39b 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> + * 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 @@ -41,6 +39,9 @@ /** * @file gps_helper.cpp + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> */ float diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index 73d4b889c..cfb9e0d43 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> + * 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 @@ -35,6 +33,8 @@ /** * @file gps_helper.h + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> */ #ifndef GPS_HELPER_H diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk index 097db2abf..82c67d40a 100644 --- a/src/drivers/gps/module.mk +++ b/src/drivers/gps/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# 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 diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 56b702ea6..c90ecbe28 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> + * 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 @@ -33,7 +31,12 @@ * ****************************************************************************/ -/* @file mkt.cpp */ +/** + * @file mtk.cpp + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + */ #include <unistd.h> #include <stdio.h> diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h index b5cfbf0a6..a2d5e27bb 100644 --- a/src/drivers/gps/mtk.h +++ b/src/drivers/gps/mtk.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> + * 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 @@ -33,7 +31,12 @@ * ****************************************************************************/ -/* @file mtk.h */ +/** + * @file mtk.cpp + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + */ #ifndef MTK_H_ #define MTK_H_ diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 86291901c..8a2afecb7 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Anton Babushkin <anton.babushkin@me.com> + * 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 @@ -40,8 +37,13 @@ * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description * including Prototol Specification. * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf */ + #include <assert.h> #include <math.h> #include <poll.h> diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 76ef873a3..79a904f4a 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Anton Babushkin <anton.babushkin@me.com> + * 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 @@ -34,7 +31,17 @@ * ****************************************************************************/ -/* @file U-Blox protocol definitions */ +/** + * @file ubx.h + * + * U-Blox protocol definition. Following u-blox 6/7 Receiver Description + * including Prototol Specification. + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * + */ #ifndef UBX_H_ #define UBX_H_ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 9b9c11af2..4c85c0cda 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * 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 @@ -381,16 +381,6 @@ HMC5883::init() reset(); _class_instance = register_class_devname(MAG_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the mag topic if we are - * the primary mag */ - struct mag_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); - - if (_mag_topic < 0) - debug("failed to create sensor_mag object"); - } ret = OK; /* sensor is ok, but not calibrated */ @@ -867,9 +857,17 @@ HMC5883::collect() /* z remains z */ new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; - if (_mag_topic != -1) { - /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) { + + if (_mag_topic != -1) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + } else { + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report); + + if (_mag_topic < 0) + debug("failed to create sensor_mag publication"); + } } /* post a report to the ring */ @@ -1140,10 +1138,12 @@ int HMC5883::check_calibration() SUBSYSTEM_TYPE_MAG}; static orb_advert_t pub = -1; - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); + if (!(_pub_blocked)) { + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } } } diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 670e51b97..90c3db9ae 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * 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 @@ -379,15 +379,24 @@ L3GD20::init() goto out; _class_instance = register_class_devname(GYRO_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* advertise sensor topic */ - struct gyro_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); - } reset(); + measure(); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _reports->get(&grp); + + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp); + + if (_gyro_topic < 0) + debug("failed to create sensor_gyro publication"); + + } + ret = OK; out: return ret; @@ -894,8 +903,10 @@ L3GD20::measure() poll_notify(POLLIN); /* publish for subscribers */ - if (_gyro_topic > 0) + if (_gyro_topic > 0 && !(_pub_blocked)) { + /* publish it */ orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); + } _read++; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 969b5e25f..4dee7649b 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 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 @@ -277,15 +277,15 @@ private: unsigned _mag_samplerate; orb_advert_t _accel_topic; - int _class_instance; + int _accel_class_instance; unsigned _accel_read; unsigned _mag_read; perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; - perf_counter_t _reg7_resets; perf_counter_t _reg1_resets; + perf_counter_t _reg7_resets; perf_counter_t _extreme_values; perf_counter_t _accel_reschedules; @@ -295,8 +295,8 @@ private: // expceted values of reg1 and reg7 to catch in-flight // brownouts of the sensor - uint8_t _reg7_expected; uint8_t _reg1_expected; + uint8_t _reg7_expected; // accel logging int _accel_log_fd; @@ -500,7 +500,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), - _class_instance(-1), + _accel_class_instance(-1), _accel_read(0), _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), @@ -551,8 +551,8 @@ LSM303D::~LSM303D() if (_mag_reports != nullptr) delete _mag_reports; - if (_class_instance != -1) - unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance); + if (_accel_class_instance != -1) + unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance); delete _mag; @@ -562,13 +562,13 @@ LSM303D::~LSM303D() perf_free(_reg1_resets); perf_free(_reg7_resets); perf_free(_extreme_values); + perf_free(_accel_reschedules); } int LSM303D::init() { int ret = ERROR; - int mag_ret; /* do SPI init (and probe) first */ if (SPI::init() != OK) { @@ -597,13 +597,37 @@ LSM303D::init() goto out; } - _class_instance = register_class_devname(ACCEL_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - // we are the primary accel device, so advertise to - // the ORB - struct accel_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); + /* fill report structures */ + measure(); + + if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct mag_report mrp; + _mag_reports->get(&mrp); + + /* measurement will have generated a report, publish */ + _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp); + + if (_mag->_mag_topic < 0) + debug("failed to create sensor_mag publication"); + + } + + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); + + if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct accel_report arp; + _accel_reports->get(&arp); + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp); + + if (_accel_topic < 0) + debug("failed to create sensor_accel publication"); + } out: @@ -727,7 +751,7 @@ LSM303D::check_extremes(const accel_report *arb) _last_log_us = now; ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n", (unsigned long long)arb->timestamp, - arb->x, arb->y, arb->z, + (double)arb->x, (double)arb->y, (double)arb->z, (int)arb->x_raw, (int)arb->y_raw, (int)arb->z_raw, @@ -1517,8 +1541,8 @@ LSM303D::measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - if (_accel_topic != -1) { - /* publish for subscribers */ + if (_accel_topic > 0 && !(_pub_blocked)) { + /* publish it */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); } @@ -1591,8 +1615,8 @@ LSM303D::mag_measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - if (_mag->_mag_topic != -1) { - /* publish for subscribers */ + if (_mag->_mag_topic > 0 && !(_pub_blocked)) { + /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); } @@ -1707,13 +1731,6 @@ LSM303D_mag::init() goto out; _mag_class_instance = register_class_devname(MAG_DEVICE_PATH); - if (_mag_class_instance == CLASS_DEVICE_PRIMARY) { - // we are the primary mag device, so advertise to - // the ORB - struct mag_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); - } out: return ret; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 9251cff7b..05ae21c1f 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 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 @@ -208,8 +208,10 @@ MEASAirspeed::collect() report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; - /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + if (_airspeed_pub > 0 && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + } new_report(report); diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index bbc595af4..bf80c9cff 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * 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 @@ -443,7 +443,6 @@ int MPU6000::init() { int ret; - int gyro_ret; /* do SPI init (and probe) first */ ret = SPI::init(); @@ -488,16 +487,36 @@ MPU6000::init() return ret; } - /* fetch an initial set of measurements for advertisement */ + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); + measure(); - _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { - /* advertise accel topic */ - accel_report ar; - _accel_reports->get(&ar); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); - } + + /* advertise sensor topic, measure manually to initialize valid report */ + struct accel_report arp; + _accel_reports->get(&arp); + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp); + + if (_accel_topic < 0) + debug("failed to create sensor_accel publication"); + + } + + if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) { + + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _gyro_reports->get(&grp); + + _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp); + + if (_gyro->_gyro_topic < 0) + debug("failed to create sensor_gyro publication"); + + } out: return ret; @@ -1307,10 +1326,13 @@ MPU6000::measure() poll_notify(POLLIN); _gyro->parent_poll_notify(); - if (_accel_topic != -1) { + if (_accel_topic > 0 && !(_pub_blocked)) { + /* publish it */ orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } - if (_gyro->_gyro_topic != -1) { + + if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) { + /* publish it */ orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } @@ -1356,11 +1378,6 @@ MPU6000_gyro::init() } _gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH); - if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) { - gyro_report gr; - memset(&gr, 0, sizeof(gr)); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); - } out: return ret; diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 6326cf7fc..0ef056273 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * 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 @@ -90,6 +90,7 @@ static const int ERROR = -1; /* 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 "/dev/ms5611" class MS5611 : public device::CDev { @@ -132,6 +133,8 @@ protected: orb_advert_t _baro_topic; + int _class_instance; + perf_counter_t _sample_perf; perf_counter_t _measure_perf; perf_counter_t _comms_errors; @@ -192,7 +195,7 @@ protected: extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : - CDev("MS5611", BARO_DEVICE_PATH), + CDev("MS5611", MS5611_BARO_DEVICE_PATH), _interface(interface), _prom(prom_buf.s), _measure_ticks(0), @@ -204,6 +207,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _SENS(0), _msl_pressure(101325), _baro_topic(-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")), @@ -218,6 +222,9 @@ MS5611::~MS5611() /* make sure we are truly inactive */ stop_cycle(); + if (_class_instance != -1) + unregister_class_devname(MS5611_BARO_DEVICE_PATH, _class_instance); + /* free any existing reports */ if (_reports != nullptr) delete _reports; @@ -251,18 +258,57 @@ MS5611::init() goto out; } - /* get a publish handle on the baro topic */ - struct baro_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report); + /* register alternate interfaces if we have to */ + _class_instance = register_class_devname(BARO_DEVICE_PATH); - if (_baro_topic < 0) { - debug("failed to create sensor_baro object"); - ret = -ENOSPC; - goto out; - } + 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; + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + + _baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp); + + if (_baro_topic < 0) + debug("failed to create sensor_baro publication"); + } + + } while (0); - ret = OK; out: return ret; } @@ -670,7 +716,10 @@ MS5611::collect() report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + if (_baro_topic > 0 && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + } if (_reports->force(&report)) { perf_count(_buffer_overflows); @@ -812,7 +861,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(BARO_DEVICE_PATH, O_RDONLY); + fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); if (fd < 0) { warnx("can't open baro device"); goto fail; @@ -846,10 +895,10 @@ test() ssize_t sz; int ret; - int fd = open(BARO_DEVICE_PATH, O_RDONLY); + int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH); + err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -905,7 +954,7 @@ test() void reset() { - int fd = open(BARO_DEVICE_PATH, O_RDONLY); + int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -944,10 +993,10 @@ calibrate(unsigned altitude) float pressure; float p1; - int fd = open(BARO_DEVICE_PATH, O_RDONLY); + int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH); + err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH); /* start the sensor polling at max */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 490fc8fc6..7ae61d9ef 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -42,6 +42,8 @@ #include <unistd.h> #include <stdint.h> #include <stdbool.h> +#include <dirent.h> +#include <fcntl.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> @@ -51,6 +53,7 @@ #include <systemlib/param/param.h> #include <systemlib/err.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_device.h> #include <mavlink/mavlink_log.h> #include "state_machine_helper.h" @@ -491,6 +494,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); valid_transition = true; + + // Disable publication of all attached sensors + + /* list directory */ + DIR *d; + struct dirent *direntry; + d = opendir("/dev"); + if (d) { + + while ((direntry = readdir(d)) != NULL) { + + int sensfd = ::open(direntry->d_name, 0); + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); + close(sensfd); + + printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL"); + } + + closedir(d); + + warnx("directory listing ok (FS mounted and readable)"); + + } else { + /* failed opening dir */ + warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); + return 1; + } } break; diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 80689f20c..476015f3e 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -56,6 +56,7 @@ #include <drivers/drv_gyro.h> #include <drivers/drv_accel.h> #include <drivers/drv_mag.h> +#include <drivers/drv_device.h> #include "systemlib/systemlib.h" #include "systemlib/err.h" @@ -65,6 +66,7 @@ __EXPORT int config_main(int argc, char *argv[]); static void do_gyro(int argc, char *argv[]); static void do_accel(int argc, char *argv[]); static void do_mag(int argc, char *argv[]); +static void do_device(int argc, char *argv[]); int config_main(int argc, char *argv[]) @@ -72,14 +74,12 @@ config_main(int argc, char *argv[]) if (argc >= 2) { if (!strcmp(argv[1], "gyro")) { do_gyro(argc - 2, argv + 2); - } - - if (!strcmp(argv[1], "accel")) { + } else if (!strcmp(argv[1], "accel")) { do_accel(argc - 2, argv + 2); - } - - if (!strcmp(argv[1], "mag")) { + } else if (!strcmp(argv[1], "mag")) { do_mag(argc - 2, argv + 2); + } else { + do_device(argc - 1, argv + 1); } } @@ -87,6 +87,48 @@ config_main(int argc, char *argv[]) } static void +do_device(int argc, char *argv[]) +{ + if (argc < 2) { + errx(1, "no device path provided and command provided."); + } + + int fd; + int ret; + + fd = open(argv[0], 0); + + if (fd < 0) { + warn("%s", argv[0]); + errx(1, "FATAL: no device found"); + + } else { + + if (argc == 2 && !strcmp(argv[1], "block")) { + + /* disable the device publications */ + ret = ioctl(fd, DEVIOCSPUBBLOCK, 1); + + if (ret) + errx(ret,"uORB publications could not be blocked"); + + } else if (argc == 2 && !strcmp(argv[1], "unblock")) { + + /* enable the device publications */ + ret = ioctl(fd, DEVIOCSPUBBLOCK, 0); + + if (ret) + errx(ret,"uORB publications could not be unblocked"); + + } else { + errx("no valid command: %s", argv[1]); + } + } + + exit(0); +} + +static void do_gyro(int argc, char *argv[]) { int fd; @@ -124,7 +166,7 @@ do_gyro(int argc, char *argv[]) if (ret) errx(ret,"range could not be set"); - } else if(argc == 1 && !strcmp(argv[0], "check")) { + } else if (argc == 1 && !strcmp(argv[0], "check")) { ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret) { |