diff options
Diffstat (limited to 'src')
49 files changed, 1675 insertions, 725 deletions
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index f73a3ef01..524151c90 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 @@ -76,8 +76,8 @@ #include <drivers/airspeed/airspeed.h> -Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : - I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), +Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) : + I2C("Airspeed", path, bus, address, 100000), _reports(nullptr), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), _max_differential_pressure_pa(0), @@ -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..186602eda 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 @@ -90,7 +90,7 @@ static const int ERROR = -1; class __EXPORT Airspeed : public device::I2C { public: - Airspeed(int bus, int address, unsigned conversion_interval); + Airspeed(int bus, int address, unsigned conversion_interval, const char* path); virtual ~Airspeed(); virtual int init(); @@ -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..b6e80ce1d 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 @@ -77,6 +77,7 @@ /* I2C bus address */ #define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ +#define ETS_PATH "/dev/ets_airspeed" /* Register address */ #define READ_CMD 0x07 /* Read the data */ @@ -93,7 +94,7 @@ class ETSAirspeed : public Airspeed { public: - ETSAirspeed(int bus, int address = I2C_ADDRESS); + ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH); protected: @@ -112,8 +113,8 @@ protected: */ extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); -ETSAirspeed::ETSAirspeed(int bus, int address) : Airspeed(bus, address, - CONVERSION_INTERVAL) +ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, + CONVERSION_INTERVAL, path) { } @@ -184,8 +185,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..06d89abf7 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 @@ -50,6 +50,7 @@ * - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf) */ + #include <nuttx/config.h> #include <drivers/device/i2c.h> @@ -89,8 +90,10 @@ /* I2C bus address is 1010001x */ #define I2C_ADDRESS_MS4525DO 0x28 //0x51 /* 7-bit address. */ +#define PATH_MS4525 "/dev/ms4525" /* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ #define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */ +#define PATH_MS5525 "/dev/ms5525" /* Register address */ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */ @@ -101,7 +104,7 @@ class MEASAirspeed : public Airspeed { public: - MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO); + MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525); protected: @@ -120,8 +123,8 @@ protected: */ extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); -MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address, - CONVERSION_INTERVAL) +MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, + CONVERSION_INTERVAL, path) { } @@ -208,8 +211,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); @@ -302,7 +307,7 @@ start(int i2c_bus) errx(1, "already started"); /* create the driver, try the MS4525DO first */ - g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO); + g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525); /* check if the MS4525DO was instantiated */ if (g_dev == nullptr) @@ -311,7 +316,7 @@ start(int i2c_bus) /* try the MS5525DSO next if init fails */ if (OK != g_dev->Airspeed::init()) { delete g_dev; - g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO); + g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525); /* check if the MS5525DSO was instantiated */ if (g_dev == nullptr) @@ -384,7 +389,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %d pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) @@ -409,7 +414,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); } diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 30d6069b3..705e98eea 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.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. * Author: Marco Bauer <marco@wtns.de> * * Redistribution and use in source and binary forms, with or without @@ -65,7 +65,6 @@ #include <drivers/device/device.h> #include <drivers/drv_pwm_output.h> -#include <drivers/drv_gpio.h> #include <drivers/drv_hrt.h> #include <drivers/drv_rc_input.h> @@ -93,16 +92,12 @@ #define MOTOR_SPINUP_COUNTER 30 #define ESC_UORB_PUBLISH_DELAY 500000 + + + class MK : public device::I2C { public: - enum Mode { - MODE_NONE, - MODE_2PWM, - MODE_4PWM, - MODE_6PWM, - }; - enum MappingMode { MAPPING_MK = 0, MAPPING_PX4, @@ -120,8 +115,7 @@ public: virtual int init(unsigned motors); virtual ssize_t write(file *filp, const char *buffer, size_t len); - int set_mode(Mode mode); - int set_pwm_rate(unsigned rate); + int set_update_rate(unsigned rate); int set_motor_count(unsigned count); int set_motor_test(bool motortest); int set_overrideSecurityChecks(bool overrideSecurityChecks); @@ -133,7 +127,6 @@ private: static const unsigned _max_actuators = MAX_MOTORS; static const bool showDebug = false; - Mode _mode; int _update_rate; int _current_update_rate; int _task; @@ -180,33 +173,15 @@ private: static const GPIOConfig _gpio_tab[]; static const unsigned _ngpio; - void gpio_reset(void); - void gpio_set_function(uint32_t gpios, int function); - void gpio_write(uint32_t gpios, int function); - uint32_t gpio_read(void); - int gpio_ioctl(file *filp, int cmd, unsigned long arg); int mk_servo_arm(bool status); - int mk_servo_set(unsigned int chan, short val); int mk_servo_set_value(unsigned int chan, short val); int mk_servo_test(unsigned int chan); short scaling(float val, float inMin, float inMax, float outMin, float outMax); - - }; -const MK::GPIOConfig MK::_gpio_tab[] = { - {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, - {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, - {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, - {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1}, - {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1}, - {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, - {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, - {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, -}; -const unsigned MK::_ngpio = sizeof(MK::_gpio_tab) / sizeof(MK::_gpio_tab[0]); + const int blctrlAddr_quad_plus[] = { 2, 2, -2, -2, 0, 0, 0, 0 }; // Addresstranslator for Quad + configuration const int blctrlAddr_hexa_plus[] = { 0, 2, 2, -2, 1, -3, 0, 0 }; // Addresstranslator for Hexa + configuration @@ -247,8 +222,7 @@ MK *g_mk; MK::MK(int bus, const char *_device_path) : I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED), - _mode(MODE_NONE), - _update_rate(50), + _update_rate(400), _task(-1), _t_actuators(-1), _t_actuator_armed(-1), @@ -317,26 +291,23 @@ MK::init(unsigned motors) usleep(500000); - if (sizeof(_device) > 0) { - ret = register_driver(_device, &fops, 0666, (void *)this); + if (sizeof(_device) > 0) { + ret = register_driver(_device, &fops, 0666, (void *)this); - if (ret == OK) { + if (ret == OK) { log("creating alternate output device"); _primary_pwm_device = true; } - } - - /* reset GPIOs */ - gpio_reset(); + } /* start the IO interface task */ _task = task_spawn_cmd("mkblctrl", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - (main_t)&MK::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 2048, + (main_t)&MK::task_main_trampoline, + nullptr); if (_task < 0) { @@ -354,43 +325,7 @@ MK::task_main_trampoline(int argc, char *argv[]) } int -MK::set_mode(Mode mode) -{ - /* - * Configure for PWM output. - * - * Note that regardless of the configured mode, the task is always - * listening and mixing; the mode just selects which of the channels - * are presented on the output pins. - */ - switch (mode) { - case MODE_2PWM: - up_pwm_servo_deinit(); - _update_rate = UPDATE_RATE; /* default output rate */ - break; - - case MODE_4PWM: - up_pwm_servo_deinit(); - _update_rate = UPDATE_RATE; /* default output rate */ - break; - - case MODE_NONE: - debug("MODE_NONE"); - /* disable servo outputs and set a very low update rate */ - up_pwm_servo_deinit(); - _update_rate = UPDATE_RATE; - break; - - default: - return -EINVAL; - } - - _mode = mode; - return OK; -} - -int -MK::set_pwm_rate(unsigned rate) +MK::set_update_rate(unsigned rate) { if ((rate > 500) || (rate < 10)) return -EINVAL; @@ -621,11 +556,13 @@ MK::task_main() } } - if(!_overrideSecurityChecks) { + if (!_overrideSecurityChecks) { /* don't go under BLCTRL_MIN_VALUE */ + if (outputs.output[i] < BLCTRL_MIN_VALUE) { outputs.output[i] = BLCTRL_MIN_VALUE; } + } /* output to BLCtrl's */ @@ -675,21 +612,24 @@ MK::task_main() esc.esc[i].esc_current = (uint16_t) Motor[i].Current; esc.esc[i].esc_rpm = (uint16_t) 0; esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4; + if (Motor[i].Version == 1) { // BLCtrl 2.0 (11Bit) - esc.esc[i].esc_setpoint_raw = (uint16_t) (Motor[i].SetPoint<<3) | Motor[i].SetPointLowerBits; + esc.esc[i].esc_setpoint_raw = (uint16_t)(Motor[i].SetPoint << 3) | Motor[i].SetPointLowerBits; + } else { // BLCtrl < 2.0 (8Bit) esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint; } + esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature; esc.esc[i].esc_state = (uint16_t) Motor[i].State; esc.esc[i].esc_errorcount = (uint16_t) 0; - // if motortest is requested - do it... - if (_motortest == true) { - mk_servo_test(i); - } + // if motortest is requested - do it... + if (_motortest == true) { + mk_servo_test(i); + } } @@ -728,7 +668,7 @@ MK::mk_servo_arm(bool status) unsigned int MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) { - if(initI2C) { + if (initI2C) { I2C::init(); } @@ -765,7 +705,7 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit; foundMotorCount++; - if (Motor[i].MaxPWM == 250) { + if ((Motor[i].MaxPWM & 252) == 248) { Motor[i].Version = BLCTRL_NEW; } else { @@ -781,8 +721,8 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); } - - if(!_overrideSecurityChecks) { + + if (!_overrideSecurityChecks) { if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) { _task_should_exit = true; } @@ -811,8 +751,8 @@ MK::mk_servo_set(unsigned int chan, short val) tmpVal = 0; } - Motor[chan].SetPoint = (uint8_t)(tmpVal>>3)&0xff; - Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal%8)&0x07; + Motor[chan].SetPoint = (uint8_t)(tmpVal >> 3) & 0xff; + Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal % 8) & 0x07; if (_armed == false) { Motor[chan].SetPoint = 0; @@ -1019,28 +959,6 @@ MK::ioctl(file *filp, int cmd, unsigned long arg) { int ret; - // XXX disabled, confusing users - - /* try it as a GPIO ioctl first */ - ret = gpio_ioctl(filp, cmd, arg); - - if (ret != -ENOTTY) - return ret; - - /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - /* - switch (_mode) { - case MODE_2PWM: - case MODE_4PWM: - case MODE_6PWM: - ret = pwm_ioctl(filp, cmd, arg); - break; - - default: - debug("not in a PWM mode"); - break; - } - */ ret = pwm_ioctl(filp, cmd, arg); /* if nobody wants it, let CDev have it */ @@ -1075,6 +993,10 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = OK; break; + case PWM_SERVO_GET_UPDATE_RATE: + *(uint32_t *)arg = _update_rate; + break; + case PWM_SERVO_SET_SELECT_UPDATE_RATE: ret = OK; break; @@ -1084,6 +1006,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) if (arg < 2150) { Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg; mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047)); + } else { ret = -EINVAL; } @@ -1198,139 +1121,10 @@ MK::write(file *filp, const char *buffer, size_t len) return count * 2; } -void -MK::gpio_reset(void) -{ - /* - * Setup default GPIO config - all pins as GPIOs, GPIO driver chip - * to input mode. - */ - for (unsigned i = 0; i < _ngpio; i++) - stm32_configgpio(_gpio_tab[i].input); - - stm32_gpiowrite(GPIO_GPIO_DIR, 0); - stm32_configgpio(GPIO_GPIO_DIR); -} - -void -MK::gpio_set_function(uint32_t gpios, int function) -{ - /* - * GPIOs 0 and 1 must have the same direction as they are buffered - * by a shared 2-port driver. Any attempt to set either sets both. - */ - if (gpios & 3) { - gpios |= 3; - - /* flip the buffer to output mode if required */ - if (GPIO_SET_OUTPUT == function) - stm32_gpiowrite(GPIO_GPIO_DIR, 1); - } - - /* configure selected GPIOs as required */ - for (unsigned i = 0; i < _ngpio; i++) { - if (gpios & (1 << i)) { - switch (function) { - case GPIO_SET_INPUT: - stm32_configgpio(_gpio_tab[i].input); - break; - - case GPIO_SET_OUTPUT: - stm32_configgpio(_gpio_tab[i].output); - break; - - case GPIO_SET_ALT_1: - if (_gpio_tab[i].alt != 0) - stm32_configgpio(_gpio_tab[i].alt); - - break; - } - } - } - - /* flip buffer to input mode if required */ - if ((GPIO_SET_INPUT == function) && (gpios & 3)) - stm32_gpiowrite(GPIO_GPIO_DIR, 0); -} - -void -MK::gpio_write(uint32_t gpios, int function) -{ - int value = (function == GPIO_SET) ? 1 : 0; - - for (unsigned i = 0; i < _ngpio; i++) - if (gpios & (1 << i)) - stm32_gpiowrite(_gpio_tab[i].output, value); -} - -uint32_t -MK::gpio_read(void) -{ - uint32_t bits = 0; - - for (unsigned i = 0; i < _ngpio; i++) - if (stm32_gpioread(_gpio_tab[i].input)) - bits |= (1 << i); - - return bits; -} - -int -MK::gpio_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - int ret = OK; - - lock(); - - switch (cmd) { - - case GPIO_RESET: - gpio_reset(); - break; - - case GPIO_SET_OUTPUT: - case GPIO_SET_INPUT: - case GPIO_SET_ALT_1: - gpio_set_function(arg, cmd); - break; - - case GPIO_SET_ALT_2: - case GPIO_SET_ALT_3: - case GPIO_SET_ALT_4: - ret = -EINVAL; - break; - - case GPIO_SET: - case GPIO_CLEAR: - gpio_write(arg, cmd); - break; - - case GPIO_GET: - *(uint32_t *)arg = gpio_read(); - break; - - default: - ret = -ENOTTY; - } - - unlock(); - - return ret; -} namespace { -enum PortMode { - PORT_MODE_UNSET = 0, - PORT_FULL_GPIO, - PORT_FULL_SERIAL, - PORT_FULL_PWM, - PORT_GPIO_AND_SERIAL, - PORT_PWM_AND_SERIAL, - PORT_PWM_AND_GPIO, -}; - enum MappingMode { MAPPING_MK = 0, MAPPING_PX4, @@ -1341,20 +1135,11 @@ enum FrameType { FRAME_X, }; -PortMode g_port_mode; int -mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks) +mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks) { - uint32_t gpio_bits; int shouldStop = 0; - MK::Mode servo_mode; - - /* reset to all-inputs */ - g_mk->ioctl(0, GPIO_RESET, 0); - - gpio_bits = 0; - servo_mode = MK::MODE_NONE; /* native PX4 addressing) */ g_mk->set_px4mode(px4mode); @@ -1368,7 +1153,6 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, /* ovveride security checks if enabled */ g_mk->set_overrideSecurityChecks(overrideSecurityChecks); - /* count used motors */ do { if (g_mk->mk_check_for_blctrl(8, false, false) != 0) { @@ -1383,86 +1167,54 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false)); - /* (re)set the PWM output mode */ - g_mk->set_mode(servo_mode); - - - if ((servo_mode != MK::MODE_NONE) && (update_rate != 0)) - g_mk->set_pwm_rate(update_rate); + g_mk->set_update_rate(update_rate); return OK; } int -mk_start(unsigned bus, unsigned motors, char *device_path) -{ - int ret = OK; - - if (g_mk == nullptr) { - - g_mk = new MK(bus, device_path); - - if (g_mk == nullptr) { - ret = -ENOMEM; - - } else { - ret = g_mk->init(motors); - - if (ret != OK) { - delete g_mk; - g_mk = nullptr; - } - } - } - - return ret; -} - - -int -mk_check_for_i2c_esc_bus(char *device_path, int motors) +mk_start(unsigned motors, char *device_path) { int ret; - if (g_mk == nullptr) { + // try i2c3 first + g_mk = new MK(3, device_path); - g_mk = new MK(3, device_path); + if (!g_mk) + return -ENOMEM; - if (g_mk == nullptr) { - return -1; - - } else { - ret = g_mk->mk_check_for_blctrl(8, false, true); - delete g_mk; - g_mk = nullptr; - - if (ret > 0) { - return 3; - } + if (OK == g_mk->init(motors)) { + warnx("[mkblctrl] scanning i2c3...\n"); + ret = g_mk->mk_check_for_blctrl(8, false, true); + if (ret > 0) { + return OK; } + } + delete g_mk; + g_mk = nullptr; - g_mk = new MK(1, device_path); - - if (g_mk == nullptr) { - return -1; + // fallback to bus 1 + g_mk = new MK(1, device_path); - } else { - ret = g_mk->mk_check_for_blctrl(8, false, true); - delete g_mk; - g_mk = nullptr; + if (!g_mk) + return -ENOMEM; - if (ret > 0) { - return 1; - } + if (OK == g_mk->init(motors)) { + warnx("[mkblctrl] scanning i2c1...\n"); + ret = g_mk->mk_check_for_blctrl(8, false, true); + if (ret > 0) { + return OK; } } - return -1; -} + delete g_mk; + g_mk = nullptr; + return -ENXIO; +} } // namespace @@ -1472,10 +1224,8 @@ extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]); int mkblctrl_main(int argc, char *argv[]) { - PortMode port_mode = PORT_FULL_PWM; int pwm_update_rate_in_hz = UPDATE_RATE; int motorcount = 8; - int bus = -1; int px4mode = MAPPING_PX4; int frametype = FRAME_PLUS; // + plus is default bool motortest = false; @@ -1489,18 +1239,6 @@ mkblctrl_main(int argc, char *argv[]) */ for (int i = 1; i < argc; i++) { - /* look for the optional i2c bus parameter */ - if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { - if (argc > i + 1) { - bus = atoi(argv[i + 1]); - newMode = true; - - } else { - errx(1, "missing argument for i2c bus (-b)"); - return 1; - } - } - /* look for the optional frame parameter */ if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) { if (argc > i + 1) { @@ -1560,51 +1298,43 @@ mkblctrl_main(int argc, char *argv[]) fprintf(stderr, "mkblctrl: help:\n"); fprintf(stderr, " [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n"); fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); - fprintf(stderr, "\t -b {i2c_bus_number} \t\t Set the i2c bus where the ESCs are connected to (default autoscan).\n"); fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n"); fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); fprintf(stderr, "\n"); fprintf(stderr, "Motortest:\n"); fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n"); fprintf(stderr, "mkblctrl -t\n"); - fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n"); + fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n"); exit(1); } if (!motortest) { - if (g_mk == nullptr) { - if (bus == -1) { - bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); - } - - if (bus != -1) { - if (mk_start(bus, motorcount, devicepath) != OK) { - errx(1, "failed to start the MK-BLCtrl driver"); - } - } else { - errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)"); - } - - /* parameter set ? */ - if (newMode) { - /* switch parameter */ - return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); - } - - exit(0); - } else { - errx(1, "MK-BLCtrl driver already running"); - } - - } else { - if (g_mk == nullptr) { - errx(1, "MK-BLCtrl driver not running. You have to start it first."); - - } else { - g_mk->set_motor_test(motortest); - exit(0); - - } - } + if (g_mk == nullptr) { + if (mk_start(motorcount, devicepath) != OK) { + errx(1, "failed to start the MK-BLCtrl driver"); + } + + /* parameter set ? */ + if (newMode) { + /* switch parameter */ + return mk_new_mode(pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); + } + + exit(0); + + } else { + errx(1, "MK-BLCtrl driver already running"); + } + + } else { + if (g_mk == nullptr) { + errx(1, "MK-BLCtrl driver not running. You have to start it first."); + + } else { + g_mk->set_motor_test(motortest); + exit(0); + + } + } } diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index bbc595af4..ac75682c4 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); } @@ -1331,6 +1353,7 @@ MPU6000::print_info() MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), _parent(parent), + _gyro_topic(-1), _gyro_class_instance(-1) { } @@ -1356,11 +1379,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/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 63a8981aa..45d7957f1 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -45,28 +45,46 @@ #include <systemlib/param/param.h> /* - * Launch detection parameters, accessible via MAVLink + * Catapult launch detection parameters, accessible via MAVLink * */ -/* Catapult Launch detection */ - -// @DisplayName Switch to enable launchdetection -// @Description if set to 1 launchdetection is enabled -// @Range 0 or 1 +/** + * Enable launch detection. + * + * @min 0 + * @max 1 + * @group Launch detection + */ PARAM_DEFINE_INT32(LAUN_ALL_ON, 0); -// @DisplayName Catapult Accelerometer Threshold -// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection -// @Range > 0 +/** + * Catapult accelerometer theshold. + * + * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. + * + * @min 0 + * @group Launch detection + */ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); -// @DisplayName Catapult Time Threshold -// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection -// @Range > 0, in seconds +/** + * Catapult time theshold. + * + * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. + * + * @min 0 + * @group Launch detection + */ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); -// @DisplayName Throttle setting while detecting the launch -// @Description The throttle is set to this value while the system is waiting for the takeoff -// @Range 0 to 1 +/** + * Throttle setting while detecting launch. + * + * The throttle is set to this value while the system is waiting for the take-off. + * + * @min 0 + * @max 1 + * @group Launch detection + */ PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c039b8573..6d14472f3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -152,6 +152,7 @@ static uint64_t last_print_mode_reject_time = 0; static bool on_usb_power = false; static float takeoff_alt = 5.0f; +static int parachute_enabled = 0; static struct vehicle_status_s status; static struct actuator_armed_s armed; @@ -429,7 +430,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) { + if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; @@ -515,7 +516,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe transition_result_t arming_res = TRANSITION_NOT_CHANGED; if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { - if (safety->safety_switch_available && !safety->safety_off) { + if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; @@ -563,7 +564,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command - if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO + //XXX: to enable the parachute, a param needs to be set + //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO + if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -615,6 +618,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); + param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); /* welcome user */ warnx("starting"); @@ -860,10 +864,10 @@ int commander_thread_main(int argc, char *argv[]) /* re-check RC calibration */ rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); - - /* navigation parameters */ - param_get(_param_takeoff_alt, &takeoff_alt); } + /* navigation parameters */ + param_get(_param_takeoff_alt, &takeoff_alt); + param_get(_param_enable_parachute, ¶chute_enabled); } orb_check(sp_man_sub, &updated); @@ -1152,7 +1156,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.arming_state == ARMING_STATE_STANDBY && sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - if (safety.safety_switch_available && !safety.safety_off) { + if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); } else if (status.main_state != MAIN_STATE_MANUAL) { @@ -1251,7 +1255,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO remove this hack /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(); } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index d3155f7bf..80ca68f21 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -48,7 +48,39 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); + +/** + * Empty cell voltage. + * + * Defines the voltage where a single cell of the battery is considered empty. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); + +/** + * Full cell voltage. + * + * Defines the voltage where a single cell of the battery is considered full. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f); + +/** + * Number of cells. + * + * Defines the number of cells the attached battery consists of. + * + * @group Battery Calibration + */ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); + +/** + * Battery capacity. + * + * Defines the capacity of the attached battery. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e5d77b246..31955d3e5 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> @@ -50,6 +52,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" @@ -332,6 +335,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s 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/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3ef1871a8..45fdaa355 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -236,7 +236,7 @@ private: float land_slope_length; float land_H1_virt; float land_flare_alt_relative; - float land_thrust_lim_horizontal_distance; + float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; } _parameters; /**< local copies of interesting parameters */ @@ -281,7 +281,7 @@ private: param_t land_slope_length; param_t land_H1_virt; param_t land_flare_alt_relative; - param_t land_thrust_lim_horizontal_distance; + param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; } _parameter_handles; /**< handles for interesting parameters */ @@ -434,7 +434,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_slope_length = param_find("FW_LND_SLLR"); _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); - _parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST"); + _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); @@ -523,7 +523,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); - param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance)); + param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); _l1_control.set_l1_damping(_parameters.l1_damping); @@ -558,7 +558,7 @@ FixedwingPositionControl::parameters_update() } /* Update the landing slope */ - landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt); + landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt); /* Update and publish the navigation capabilities */ _nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad(); @@ -836,6 +836,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); @@ -846,7 +848,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!land_noreturn_horizontal) {//set target_bearing in first occurrence if (pos_sp_triplet.previous.valid) { - target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + target_bearing = bearing_lastwp_currwp; } else { target_bearing = _att.yaw; } @@ -888,10 +890,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_approach = 1.3f * _parameters.airspeed_min; float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length; - float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); - - + float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); + float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); + float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out @@ -903,7 +904,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (wp_distance < landingslope.motor_lim_horizontal_distance() || land_motor_lim) { + if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; @@ -912,7 +913,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt); + float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 7954d75c2..62a340e90 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -40,12 +40,10 @@ */ #include <nuttx/config.h> - #include <systemlib/param/param.h> /* * Controller parameters, accessible via MAVLink - * */ /** @@ -119,58 +117,268 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f); */ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); - +/** + * Controller roll limit + * + * The maximum roll the controller will output. + * + * @unit degrees + * @min 0.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); +/** + * Throttle limit max + * + * This is the maximum throttle % that can be used by the controller. + * For overpowered aircraft, this should be reduced to a value that + * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); +/** + * Throttle limit min + * + * This is the minimum throttle % that can be used by the controller. + * For electric aircraft this will normally be set to zero, but can be set + * to a small non-zero value if a folding prop is fitted to prevent the + * prop from folding and unfolding repeatedly in-flight or to provide + * some aerodynamic drag from a turning prop to improve the descent rate. + * + * For aircraft with internal combustion engine this parameter should be set + * for desired idle rpm. + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); - -PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); - +/** + * Throttle limit value before flare + * + * This throttle value will be set as throttle limit at FW_LND_TLALT, + * before arcraft will flare. + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); +/** + * Maximum climb rate + * + * This is the best climb rate that the aircraft can achieve with + * the throttle set to THR_MAX and the airspeed set to the + * default value. For electric aircraft make sure this number can be + * achieved towards the end of flight when the battery voltage has reduced. + * The setting of this parameter can be checked by commanding a positive + * altitude change of 100m in loiter, RTL or guided mode. If the throttle + * required to climb is close to THR_MAX and the aircraft is maintaining + * airspeed, then this parameter is set correctly. If the airspeed starts + * to reduce, then the parameter is set to high, and if the throttle + * demand required to climb and maintain speed is noticeably less than + * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or + * FW_THR_MAX reduced. + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); - +/** + * Minimum descent rate + * + * This is the sink rate of the aircraft with the throttle + * set to THR_MIN and flown at the same airspeed as used + * to measure FW_T_CLMB_MAX. + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); +/** + * Maximum descent rate + * + * This sets the maximum descent rate that the controller will use. + * If this value is too large, the aircraft can over-speed on descent. + * This should be set to a value that can be achieved without + * exceeding the lower pitch angle limit and without over-speeding + * the aircraft. + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); +/** + * TECS time constant + * + * This is the time constant of the TECS control algorithm (in seconds). + * Smaller values make it faster to respond, larger values make it slower + * to respond. + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); - +/** + * Throttle damping factor + * + * This is the damping gain for the throttle demand loop. + * Increase to add damping to correct for oscillations in speed and height. + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); - +/** + * Integrator gain + * + * This is the integrator gain on the control loop. + * Increasing this gain increases the speed at which speed + * and height offsets are trimmed out, but reduces damping and + * increases overshoot. + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); - +/** + * Maximum vertical acceleration + * + * This is the maximum vertical acceleration (in metres/second^2) + * either up or down that the controller will use to correct speed + * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) + * allows for reasonably aggressive pitch changes if required to recover + * from under-speed conditions. + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); - +/** + * Complementary filter "omega" parameter for height + * + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse vertical acceleration and barometric height to obtain + * an estimate of height rate and height. Increasing this frequency weights + * the solution more towards use of the barometer, whilst reducing it weights + * the solution more towards use of the accelerometer data. + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); - +/** + * Complementary filter "omega" parameter for speed + * + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse longitudinal acceleration and airspeed to obtain an + * improved airspeed estimate. Increasing this frequency weights the solution + * more towards use of the arispeed sensor, whilst reducing it weights the + * solution more towards use of the accelerometer data. + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); - +/** + * Roll -> Throttle feedforward + * + * Increasing this gain turn increases the amount of throttle that will + * be used to compensate for the additional drag created by turning. + * Ideally this should be set to approximately 10 x the extra sink rate + * in m/s created by a 45 degree bank turn. Increase this gain if + * the aircraft initially loses energy in turns and reduce if the + * aircraft initially gains energy in turns. Efficient high aspect-ratio + * aircraft (eg powered sailplanes) can use a lower value, whereas + * inefficient low aspect-ratio models (eg delta wings) can use a higher value. + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); - +/** + * Speed <--> Altitude priority + * + * This parameter adjusts the amount of weighting that the pitch control + * applies to speed vs height errors. Setting it to 0.0 will cause the + * pitch control to control height and ignore speed errors. This will + * normally improve height accuracy but give larger airspeed errors. + * Setting it to 2.0 will cause the pitch control loop to control speed + * and ignore height errors. This will normally reduce airspeed errors, + * but give larger height errors. The default value of 1.0 allows the pitch + * control to simultaneously control height and speed. + * Note to Glider Pilots - set this parameter to 2.0 (The glider will + * adjust its pitch angle to maintain airspeed, ignoring changes in height). + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); - +/** + * Pitch damping factor + * + * This is the damping gain for the pitch demand loop. Increase to add + * damping to correct for oscillations in height. The default value of 0.0 + * will work well provided the pitch to servo controller has been tuned + * properly. + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); - -PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); - +/** + * Height rate P factor + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); + +/** + * Speed rate P factor + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); -PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); +/** + * Landing slope angle + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); + +/** + * Landing slope length + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); + +/** + * + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); + +/** + * Landing flare altitude (relative) + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); -PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f); + +/** + * Landing throttle limit altitude (relative) + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); + +/** + * Landing heading hold horizontal distance + * + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index b139a6397..e5f7023ae 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -48,13 +48,13 @@ void Landingslope::update(float landing_slope_angle_rad, float flare_relative_alt, - float motor_lim_horizontal_distance, + float motor_lim_relative_alt, float H1_virt) { _landing_slope_angle_rad = landing_slope_angle_rad; _flare_relative_alt = flare_relative_alt; - _motor_lim_horizontal_distance = motor_lim_horizontal_distance; + _motor_lim_relative_alt = motor_lim_relative_alt; _H1_virt = H1_virt; calculateSlopeValues(); @@ -74,9 +74,21 @@ float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_ return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); } -float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude) +float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude) { - return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; + /* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */ + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) + return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude); + else + return wp_altitude; +} +float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude) +{ + /* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */ + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) + return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; + else + return wp_landing_altitude; } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index 1a149fc7c..76d65a55f 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -49,7 +49,7 @@ private: /* see Documentation/fw_landing.png for a plot of the landing slope */ float _landing_slope_angle_rad; /**< phi in the plot */ float _flare_relative_alt; /**< h_flare,rel in the plot */ - float _motor_lim_horizontal_distance; + float _motor_lim_relative_alt; float _H1_virt; /**< H1 in the plot */ float _H0; /**< h_flare,rel + H1 in the plot */ float _d1; /**< d1 in the plot */ @@ -63,7 +63,18 @@ public: Landingslope() {} ~Landingslope() {} - float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude); + /** + * + * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude); + + /** + * + * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + * Performs check if aircraft is in front of waypoint to avoid climbout + */ + float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude); /** * @@ -85,17 +96,17 @@ public: } - float getFlareCurveAltitude(float wp_distance, float wp_altitude); + float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); void update(float landing_slope_angle_rad, float flare_relative_alt, - float motor_lim_horizontal_distance, + float motor_lim_relative_alt, float H1_virt); inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;} inline float flare_relative_alt() {return _flare_relative_alt;} - inline float motor_lim_horizontal_distance() {return _motor_lim_horizontal_distance;} + inline float motor_lim_relative_alt() {return _motor_lim_relative_alt;} inline float H1_virt() {return _H1_virt;} inline float H0() {return _H0;} inline float d1() {return _d1;} diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4d975066f..ade4469c5 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -76,8 +76,20 @@ #include <uORB/topics/mission_result.h> /* define MAVLink specific parameters */ +/** + * MAVLink system ID + * @group MAVLink + */ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); +/** + * MAVLink component ID + * @group MAVLink + */ PARAM_DEFINE_INT32(MAV_COMP_ID, 50); +/** + * MAVLink type + * @group MAVLink + */ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); __EXPORT int mavlink_main(int argc, char *argv[]); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a371a499e..1dbe56495 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -351,7 +351,7 @@ handle_message(mavlink_message_t *msg) tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; - if (telemetry_status_pub == 0) { + if (telemetry_status_pub <= 0) { telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); } else { diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index db5e2e9bb..24226880e 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -53,11 +53,9 @@ #include <stdlib.h> #include <string.h> #include <unistd.h> -#include <fcntl.h> #include <errno.h> #include <math.h> #include <poll.h> -#include <time.h> #include <drivers/drv_hrt.h> #include <arch/board/board.h> #include <uORB/uORB.h> @@ -71,7 +69,6 @@ #include <uORB/topics/parameter_update.h> #include <systemlib/param/param.h> #include <systemlib/err.h> -#include <systemlib/pid/pid.h> #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> @@ -84,9 +81,9 @@ */ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); -#define MIN_TAKEOFF_THROTTLE 0.3f #define YAW_DEADZONE 0.05f -#define RATES_I_LIMIT 0.5f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f class MulticopterAttitudeControl { @@ -135,15 +132,13 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - math::Matrix<3, 3> _R_sp; /**< attitude setpoint rotation matrix */ - math::Matrix<3, 3> _R; /**< rotation matrix for current state */ math::Vector<3> _rates_prev; /**< angular rates on previous step */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ math::Vector<3> _rates_int; /**< angular rates integral error */ float _thrust_sp; /**< thrust setpoint */ math::Vector<3> _att_control; /**< attitude control vector */ - math::Matrix<3, 3> I; /**< identity matrix */ + math::Matrix<3, 3> _I; /**< identity matrix */ bool _reset_yaw_sp; /**< reset yaw setpoint flag */ @@ -262,7 +257,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _actuators_0_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")) + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { memset(&_v_att, 0, sizeof(_v_att)); @@ -276,15 +271,13 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_i.zero(); _params.rate_d.zero(); - _R_sp.identity(); - _R.identity(); _rates_prev.zero(); _rates_sp.zero(); _rates_int.zero(); _thrust_sp = 0.0f; _att_control.zero(); - I.identity(); + _I.identity(); _params_handles.roll_p = param_find("MC_ROLL_P"); _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); @@ -535,16 +528,18 @@ MulticopterAttitudeControl::control_attitude(float dt) _thrust_sp = _v_att_sp.thrust; /* construct attitude setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; + if (_v_att_sp.R_valid) { /* rotation matrix in _att_sp is valid, use it */ - _R_sp.set(&_v_att_sp.R_body[0][0]); + R_sp.set(&_v_att_sp.R_body[0][0]); } else { /* rotation matrix in _att_sp is not valid, use euler angles instead */ - _R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp.R_body[0][0], &_R_sp.data[0][0], sizeof(_v_att_sp.R_body)); + memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body)); _v_att_sp.R_valid = true; } @@ -561,23 +556,24 @@ MulticopterAttitudeControl::control_attitude(float dt) } /* rotation matrix for current state */ - _R.set(_v_att.R); + math::Matrix<3, 3> R; + R.set(_v_att.R); /* all input data is ready, run controller itself */ /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector<3> R_z(_R(0, 2), _R(1, 2), _R(2, 2)); - math::Vector<3> R_sp_z(_R_sp(0, 2), _R_sp(1, 2), _R_sp(2, 2)); + math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); /* axis and sin(angle) of desired rotation */ - math::Vector<3> e_R = _R.transposed() * (R_z % R_sp_z); + math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); /* calculate angle error */ float e_R_z_sin = e_R.length(); float e_R_z_cos = R_z * R_sp_z; /* calculate weight for yaw control */ - float yaw_w = _R_sp(2, 2) * _R_sp(2, 2); + float yaw_w = R_sp(2, 2) * R_sp(2, 2); /* calculate rotation matrix after roll/pitch only rotation */ math::Matrix<3, 3> R_rp; @@ -600,15 +596,15 @@ MulticopterAttitudeControl::control_attitude(float dt) e_R_cp(2, 1) = e_R_z_axis(0); /* rotation matrix for roll/pitch only rotation */ - R_rp = _R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); } else { /* zero roll/pitch rotation */ - R_rp = _R; + R_rp = R; } - /* R_rp and _R_sp has the same Z axis, calculate yaw error */ - math::Vector<3> R_sp_x(_R_sp(0, 0), _R_sp(1, 0), _R_sp(2, 0)); + /* R_rp and R_sp has the same Z axis, calculate yaw error */ + math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; @@ -616,7 +612,7 @@ MulticopterAttitudeControl::control_attitude(float dt) /* for large thrust vector rotations use another rotation method: * calculate angle and axis for R -> R_sp rotation directly */ math::Quaternion q; - q.from_dcm(_R.transposed() * _R_sp); + q.from_dcm(R.transposed() * R_sp); math::Vector<3> e_R_d = q.imag(); e_R_d.normalize(); e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); @@ -658,7 +654,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) _rates_prev = rates; /* update integral only if not saturated on low limit */ - if (_thrust_sp > 0.1f) { + if (_thrust_sp > MIN_TAKEOFF_THRUST) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; @@ -695,9 +691,6 @@ MulticopterAttitudeControl::task_main() _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - /* rate limit attitude updates to 200Hz, failsafe against spam, normally runs at the same rate as attitude estimator */ - orb_set_interval(_v_att_sub, 5); - /* initialize parameters cache */ parameters_update(); @@ -767,10 +760,12 @@ MulticopterAttitudeControl::task_main() } } else { - /* attitude controller disabled */ - // TODO poll 'attitude_rates_setpoint' topic - _rates_sp.zero(); - _thrust_sp = 0.0f; + /* attitude controller disabled, poll rates setpoint topic */ + vehicle_rates_setpoint_poll(); + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; + _thrust_sp = _v_rates_sp.thrust; } if (_v_control_mode.flag_control_rates_enabled) { diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 27a45b6bb..488107d58 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -41,16 +41,135 @@ #include <systemlib/param/param.h> +/** + * Roll P gain + * + * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); + +/** + * Roll rate P gain + * + * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); + +/** + * Roll rate I gain + * + * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); + +/** + * Roll rate D gain + * + * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); + +/** + * Pitch P gain + * + * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @unit 1/s + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); + +/** + * Pitch rate P gain + * + * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); + +/** + * Pitch rate I gain + * + * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); + +/** + * Pitch rate D gain + * + * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); + +/** + * Yaw P gain + * + * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @unit 1/s + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); + +/** + * Yaw rate P gain + * + * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); + +/** + * Yaw rate I gain + * + * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); + +/** + * Yaw rate D gain + * + * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); + +/** + * Yaw feed forward + * + * Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 25d34c872..b06655385 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -51,7 +51,6 @@ #include <errno.h> #include <math.h> #include <poll.h> -#include <time.h> #include <drivers/drv_hrt.h> #include <arch/board/board.h> #include <uORB/uORB.h> @@ -68,7 +67,6 @@ #include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <systemlib/param/param.h> #include <systemlib/err.h> -#include <systemlib/pid/pid.h> #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> #include <lib/geo/geo.h> diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 9eb56545d..0082a5e6a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -39,20 +39,164 @@ #include <systemlib/param/param.h> -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f); +/** + * Minimum thrust + * + * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f); + +/** + * Maximum thrust + * + * Limit max allowed thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); + +/** + * Proportional gain for vertical position error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); + +/** + * Proportional gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); + +/** + * Integral gain for vertical velocity error + * + * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.02f); + +/** + * Differential gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); + +/** + * Maximum vertical velocity + * + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY). + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); + +/** + * Vertical velocity feed forward + * + * Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f); + +/** + * Proportional gain for horizontal position error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f); + +/** + * Proportional gain for horizontal velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f); + +/** + * Integral gain for horizontal velocity error + * + * Non-zero value allows to resist wind. + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f); + +/** + * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); + +/** + * Maximum horizontal velocity + * + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY). + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); + +/** + * Horizontal velocity feed forward + * + * Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); + +/** + * Maximum tilt + * + * Limits maximum tilt in AUTO and EASY modes. + * + * @min 0.0 + * @max 1.57 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f); + +/** + * Landing descend rate + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); + +/** + * Maximum landing tilt + * + * Limits maximum tilt on landing. + * + * @min 0.0 + * @max 1.57 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f); diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 20dd1fe2f..5831a0ca9 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -45,11 +45,17 @@ #include <systemlib/param/param.h> /* - * geofence parameters, accessible via MAVLink - * + * Geofence parameters, accessible via MAVLink */ -// @DisplayName Switch to enable geofence -// @Description if set to 1 geofence is enabled, defaults to 1 because geofence is only enabled when the geofence.txt file is present -// @Range 0 or 1 +/** + * Enable geofence. + * + * Set to 1 to enable geofence. + * Defaults to 1 because geofence is only enabled when the geofence.txt file is present. + * + * @min 0 + * @max 1 + * @group Geofence + */ PARAM_DEFINE_INT32(GF_ON, 1); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 6f0fec917..42d1f5c24 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -306,6 +306,12 @@ private: void start_land_home(); /** + * Fork for state transitions + */ + void request_loiter_or_ready(); + void request_mission_if_available(); + + /** * Guards offboard mission */ bool offboard_mission_available(unsigned relative_index); @@ -699,24 +705,17 @@ Navigator::task_main() } else { /* MISSION switch */ if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); + request_loiter_or_ready(); stick_mode = true; } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - + request_mission_if_available(); stick_mode = true; } if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); + request_mission_if_available(); stick_mode = true; } } @@ -733,17 +732,11 @@ Navigator::task_main() break; case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); + request_loiter_or_ready(); break; case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - + request_mission_if_available(); break; case NAV_STATE_RTL: @@ -768,12 +761,7 @@ Navigator::task_main() } else { /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } + request_mission_if_available(); } } } @@ -1069,7 +1057,7 @@ Navigator::start_loiter() float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; /* use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < min_alt_amsl) { + if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) { _pos_sp_triplet.current.alt = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); @@ -1078,9 +1066,8 @@ Navigator::start_loiter() mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } - _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; } - + _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; _pos_sp_triplet.current.loiter_direction = 1; _pos_sp_triplet.previous.valid = false; @@ -1397,6 +1384,28 @@ Navigator::set_rtl_item() } void +Navigator::request_loiter_or_ready() +{ + if (_vstatus.condition_landed) { + dispatch(EVENT_READY_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } +} + +void +Navigator::request_mission_if_available() +{ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + + } else { + request_loiter_or_ready(); + } +} + +void Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) { sp->valid = true; @@ -1554,13 +1563,7 @@ Navigator::on_mission_item_reached() /* loiter at last waypoint */ _reset_loiter_pos = false; mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); - - if (_vstatus.condition_landed) { - dispatch(EVENT_READY_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } + request_loiter_or_ready(); } } else if (myState == NAV_STATE_RTL) { diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index d5e00e35d..9ef359c6d 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -50,14 +50,91 @@ /* * Navigator parameters, accessible via MAVLink - * */ +/** + * Minimum altitude (fixed wing only) + * + * Minimum altitude above home for LOITER. + * + * @unit meters + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); + +/** + * Waypoint acceptance radius + * + * Default value of acceptance radius (if not specified in mission item). + * + * @unit meters + * @min 0.0 + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); -PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); + +/** + * Loiter radius (fixed wing only) + * + * Default value of loiter radius (if not specified in mission item). + * + * @unit meters + * @min 0.0 + * @group Navigation + */ +PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); + +/** + * Enable onboard mission + * + * @group Navigation + */ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); -PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude -PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing -PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode -PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT + +/** + * Take-off altitude + * + * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint. + * + * @unit meters + * @group Navigation + */ +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); + +/** + * Landing altitude + * + * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed. + * + * @unit meters + * @group Navigation + */ +PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); + +/** + * Return-To-Launch altitude + * + * Minimum altitude above home position for going home in RTL mode. + * + * @unit meters + * @group Navigation + */ +PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); + +/** + * Return-To-Launch delay + * + * Delay after descend before landing in RTL mode. + * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * + * @unit seconds + * @group Navigation + */ +PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); + +/** + * Enable parachute deployment + * + * @group Navigation + */ +PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index e045ce4cc..ad363efe0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -42,14 +42,11 @@ #include <stdio.h> #include <stdbool.h> #include <fcntl.h> -#include <float.h> #include <string.h> #include <nuttx/config.h> #include <nuttx/sched.h> #include <sys/prctl.h> #include <termios.h> -#include <errno.h> -#include <limits.h> #include <math.h> #include <uORB/uORB.h> #include <uORB/topics/parameter_update.h> @@ -527,13 +524,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps.fix_type >= 3) { /* hysteresis for GPS quality */ if (gps_valid) { - if (gps.eph_m > 10.0f || gps.epv_m > 10.0f) { + if (gps.eph_m > 10.0f || gps.epv_m > 20.0f) { gps_valid = false; mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { - if (gps.eph_m < 5.0f && gps.epv_m < 5.0f) { + if (gps.eph_m < 5.0f && gps.epv_m < 10.0f) { gps_valid = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); } @@ -589,8 +586,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } - w_gps_xy = 1.0f / fmaxf(1.0f, gps.eph_m); - w_gps_z = 1.0f / fmaxf(1.0f, gps.epv_m); + w_gps_xy = 2.0f / fmaxf(2.0f, gps.eph_m); + w_gps_z = 4.0f / fmaxf(4.0f, gps.epv_m); } } else { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index e1bbd75a6..b71f9472f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,7 +40,7 @@ #include "position_estimator_inav_params.h" -PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index f39fcf7ec..6a4429461 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -179,7 +179,7 @@ mixer_tick(void) ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) ) ); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3c218e21f..68e6a7469 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -82,6 +82,7 @@ #include <uORB/topics/airspeed.h> #include <uORB/topics/rc_channels.h> #include <uORB/topics/esc_status.h> +#include <uORB/topics/telemetry_status.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -758,6 +759,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct esc_status_s esc; struct vehicle_global_velocity_setpoint_s global_vel_sp; struct battery_status_s battery; + struct telemetry_status_s telemetry; } buf; memset(&buf, 0, sizeof(buf)); @@ -783,6 +785,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int esc_sub; int global_vel_sp_sub; int battery_sub; + int telemetry_sub; } subs; /* log message buffer: header + body */ @@ -811,6 +814,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GVSP_s log_GVSP; struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; + struct log_TELE_s log_TELE; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -946,6 +950,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- TELEMETRY STATUS --- */ + subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + fds[fdsc_count].fd = subs.telemetry_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -1347,6 +1357,20 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(BATT); } + /* --- TELEMETRY --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry); + log_msg.msg_type = LOG_TELE_MSG; + log_msg.body.log_TELE.rssi = buf.telemetry.rssi; + log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi; + log_msg.body.log_TELE.noise = buf.telemetry.noise; + log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise; + log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors; + log_msg.body.log_TELE.fixed = buf.telemetry.fixed; + log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf; + LOGBUFFER_WRITE_AND_COUNT(TELE); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { /* only request write if several packets can be written at once */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index db87e3a6a..16bfc355d 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -264,6 +264,18 @@ struct log_DIST_s { uint8_t flags; }; +/* --- TELE - TELEMETRY STATUS --- */ +#define LOG_TELE_MSG 22 +struct log_TELE_s { + uint8_t rssi; + uint8_t remote_rssi; + uint8_t noise; + uint8_t remote_noise; + uint16_t rxerrors; + uint16_t fixed; + uint8_t txbuf; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -311,6 +323,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), + LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + /* system-level messages, ID >= 0x80 */ // FMT: don't write format of format message, it's useless LOG_FORMAT(TIME, "Q", "StartTime"), diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 30659fd3a..288c6e00a 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -42,13 +42,10 @@ */ #include <nuttx/config.h> - #include <systemlib/param/param.h> /** - * Gyro X offset - * - * This is an X-axis offset for the gyro. Adjust it according to the calibration data. + * Gyro X-axis offset * * @min -10.0 * @max 10.0 @@ -57,7 +54,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); /** - * Gyro Y offset + * Gyro Y-axis offset * * @min -10.0 * @max 10.0 @@ -66,7 +63,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); /** - * Gyro Z offset + * Gyro Z-axis offset * * @min -5.0 * @max 5.0 @@ -75,9 +72,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); /** - * Gyro X scaling - * - * X-axis scaling. + * Gyro X-axis scaling factor * * @min -1.5 * @max 1.5 @@ -86,9 +81,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); /** - * Gyro Y scaling - * - * Y-axis scaling. + * Gyro Y-axis scaling factor * * @min -1.5 * @max 1.5 @@ -97,9 +90,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); /** - * Gyro Z scaling - * - * Z-axis scaling. + * Gyro Z-axis scaling factor * * @min -1.5 * @max 1.5 @@ -107,10 +98,9 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); + /** - * Magnetometer X offset - * - * This is an X-axis offset for the magnetometer. + * Magnetometer X-axis offset * * @min -500.0 * @max 500.0 @@ -119,9 +109,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); /** - * Magnetometer Y offset - * - * This is an Y-axis offset for the magnetometer. + * Magnetometer Y-axis offset * * @min -500.0 * @max 500.0 @@ -130,9 +118,7 @@ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); /** - * Magnetometer Z offset - * - * This is an Z-axis offset for the magnetometer. + * Magnetometer Z-axis offset * * @min -500.0 * @max 500.0 @@ -140,24 +126,134 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); */ PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f); +/** + * Magnetometer X-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f); + +/** + * Magnetometer Y-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f); + +/** + * Magnetometer Z-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f); + +/** + * Accelerometer X-axis offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f); + +/** + * Accelerometer Y-axis offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f); + +/** + * Accelerometer Z-axis offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f); +/** + * Accelerometer X-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); + +/** + * Accelerometer Y-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); + +/** + * Accelerometer Z-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); + +/** + * Differential pressure sensor offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); + +/** + * Differential pressure sensor analog enabled + * + * @group Sensor Calibration + */ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); + +/** + * Board rotation + * + * This parameter defines the rotation of the FMU board relative to the platform. + * Possible values are: + * 0 = No rotation + * 1 = Yaw 45° + * 2 = Yaw 90° + * 3 = Yaw 135° + * 4 = Yaw 180° + * 5 = Yaw 225° + * 6 = Yaw 270° + * 7 = Yaw 315° + * 8 = Roll 180° + * 9 = Roll 180°, Yaw 45° + * 10 = Roll 180°, Yaw 90° + * 11 = Roll 180°, Yaw 135° + * 12 = Pitch 180° + * 13 = Roll 180°, Yaw 225° + * 14 = Roll 180°, Yaw 270° + * 15 = Roll 180°, Yaw 315° + * 16 = Roll 90° + * 17 = Roll 90°, Yaw 45° + * 18 = Roll 90°, Yaw 90° + * 19 = Roll 90°, Yaw 135° + * 20 = Roll 270° + * 21 = Roll 270°, Yaw 45° + * 22 = Roll 270°, Yaw 90° + * 23 = Roll 270°, Yaw 135° + * 24 = Pitch 90° + * 25 = Pitch 270° + * + * @group Sensor Calibration + */ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); + +/** + * External magnetometer rotation + * + * This parameter defines the rotation of the external magnetometer relative + * to the platform (not relative to the FMU). + * See SENS_BOARD_ROT for possible values. + * + * @group Sensor Calibration + */ PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); + /** * RC Channel 1 Minimum * @@ -367,20 +463,52 @@ PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ #endif -PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ +/** + * DSM binding trigger. + * + * -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind + * + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_DSM_BIND, -1); + + +/** + * Scaling factor for battery voltage sensor on PX4IO. + * + * @group Battery Calibration + */ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +/** + * Scaling factor for battery voltage sensor on FMU v2. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); #else -/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ -/* PX4IOAR: 0.00838095238 */ -/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ -/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */ +/** + * Scaling factor for battery voltage sensor on FMU v1. + * + * FMUv1 standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 + * FMUv1 with PX4IO: 0.00459340659 + * FMUv1 with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) = 0.00838095238 + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); #endif + +/** + * Scaling factor for battery current sensor. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */ + /** * Roll control channel mapping. * @@ -446,22 +574,127 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); * @group Radio Calibration */ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); + +/** + * Return switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); + +/** + * Assist switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); + +/** + * Mission switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +/** + * Flaps channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); -PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera pitch */ +/** + * Auxiliary switch 1 channel mapping. + * + * Default function: Camera pitch + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); + +/** + * Auxiliary switch 2 channel mapping. + * + * Default function: Camera roll + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ -PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw */ +/** + * Auxiliary switch 3 channel mapping. + * + * Default function: Camera azimuth / yaw + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); + + +/** + * Roll scaling factor + * + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); + +/** + * Pitch scaling factor + * + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); + +/** + * Yaw scaling factor + * + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); -PARAM_DEFINE_INT32(RC_FS_CH, 0); /**< RC failsafe channel, 0 = disable */ -PARAM_DEFINE_INT32(RC_FS_MODE, 0); /**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */ -PARAM_DEFINE_FLOAT(RC_FS_THR, 800); /**< RC failsafe PWM threshold */ + +/** + * Failsafe channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_FS_CH, 0); + +/** + * Failsafe channel mode. + * + * 0 = too low means signal loss, + * 1 = too high means signal loss + * + * @min 0 + * @max 1 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_FS_MODE, 0); + +/** + * Failsafe channel PWM threshold. + * + * @min 0 + * @max 1 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC_FS_THR, 800); diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 75be090f8..cb35a2541 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -40,8 +40,23 @@ #include <nuttx/config.h> #include <systemlib/param/param.h> -// Auto-start script with index #n +/** + * Auto-start script index. + * + * Defines the auto-start script used to bootstrap the system. + * + * @group System + */ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); -// Automatically configure default values +/** + * Automatically configure default values. + * + * Set to 1 to set platform-specific parameters to their default + * values on next system startup. + * + * @min 0 + * @max 1 + * @group System + */ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 828fb31cc..5192d4d58 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -58,10 +58,10 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ - unsigned rssi; /**< local signal strength */ - unsigned remote_rssi; /**< remote signal strength */ - unsigned rxerrors; /**< receive errors */ - unsigned fixed; /**< count of error corrected packets */ + uint8_t rssi; /**< local signal strength */ + uint8_t remote_rssi; /**< remote signal strength */ + uint16_t rxerrors; /**< receive errors */ + uint16_t fixed; /**< count of error corrected packets */ uint8_t noise; /**< background noise level */ uint8_t remote_noise; /**< remote background noise level */ uint8_t txbuf; /**< how full the tx buffer is as a percentage */ @@ -73,4 +73,4 @@ struct telemetry_status_s { ORB_DECLARE(telemetry_status); -#endif /* TOPIC_TELEMETRY_STATUS_H */
\ No newline at end of file +#endif /* TOPIC_TELEMETRY_STATUS_H */ 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) { |