From 88fe2d3052eccd542ffb5d3473d720b66b8679fd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 24 Feb 2014 19:04:26 +0100 Subject: [PATCH 01/67] mission feasibility checker: add check for waypoint altitude relative to home position altitude --- .../navigator/mission_feasibility_checker.cpp | 43 +++++++++++++++---- .../navigator/mission_feasibility_checker.h | 7 +-- src/modules/navigator/navigator_main.cpp | 2 +- 3 files changed, 40 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index eaafa217de..41670e2474 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -62,7 +62,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { /* Init if not done yet */ init(); @@ -75,24 +75,24 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence); + return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); else - return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence); + return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { - return checkGeofence(dm_current, nMissionItems, geofence); + return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); // warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); - return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence)); + return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -108,7 +108,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return false; } - if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude + if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); return false; } @@ -118,6 +118,33 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return true; } +bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error) +{ + /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ + for (size_t i = 0; i < nMissionItems; i++) { + static struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + if (throw_error) { + return false; + } else { + return true; + } + } + + if (home_alt > missionitem.altitude) { + mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); + if (throw_error) { + return false; + } else { + return true; + } + } + } +} + bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) { /* Go through all mission items and search for a landing waypoint diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 7a0b2a2966..819dcf9c3a 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -59,14 +59,15 @@ private: /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems); void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); public: MissionFeasibilityChecker(); @@ -75,7 +76,7 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c572972b58..11181ff642 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -521,7 +521,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence, _home_pos.alt); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_current_offboard_mission_index(offboard_mission.current_index); From 053ad5b6388ef653d1dfe255ea4f3eb00aeccaba Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 24 Feb 2014 22:19:21 +0100 Subject: [PATCH 02/67] mission feasibility checker: remove audio warning for waypoint below home altitude --- src/modules/navigator/mission_feasibility_checker.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 41670e2474..b41e9d355d 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -135,10 +135,11 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } if (home_alt > missionitem.altitude) { - mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); if (throw_error) { + mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i); return false; } else { + mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); return true; } } From c6e2ad918b50f01ec9e26ccd7fdb88c7c0d2a60c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Feb 2014 00:34:51 +0100 Subject: [PATCH 03/67] mission feasibility checker: add missing default return in checkHomePositionAltitude --- src/modules/navigator/mission_feasibility_checker.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index b41e9d355d..02e35f1a8b 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -144,6 +144,8 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } } + + return true; } bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) From af1af1e22ddd4bcd55fe9eaaf98f4a640329a4c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Apr 2014 18:38:30 +0200 Subject: [PATCH 04/67] Port expander driver first hacky version --- makefiles/config_px4fmu-v2_default.mk | 1 + makefiles/config_px4fmu-v2_test.mk | 1 + src/drivers/drv_io_expander.h | 65 +++ src/drivers/pca8574/module.mk | 6 + src/drivers/pca8574/pca8574.cpp | 569 ++++++++++++++++++++++++++ 5 files changed, 642 insertions(+) create mode 100644 src/drivers/drv_io_expander.h create mode 100644 src/drivers/pca8574/module.mk create mode 100644 src/drivers/pca8574/pca8574.cpp diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e13421acc5..602b0af3fb 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -40,6 +40,7 @@ MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl +MODULES += drivers/pca8574 # Needs to be burned to the ground and re-written; for now, diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 79922374d7..0dcfd04f22 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -24,6 +24,7 @@ MODULES += drivers/lsm303d MODULES += drivers/l3gd20 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 +MODULES += drivers/pca8574 MODULES += systemcmds/perf MODULES += systemcmds/reboot MODULES += systemcmds/tests diff --git a/src/drivers/drv_io_expander.h b/src/drivers/drv_io_expander.h new file mode 100644 index 0000000000..2705d6f9e1 --- /dev/null +++ b/src/drivers/drv_io_expander.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * 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_io_expander.h + * + * IO expander device API + */ + +#pragma once + +#include +#include + +/* + * ioctl() definitions + */ + +#define _IOXIOCBASE (0x2800) +#define _IOXIOC(_n) (_IOC(_IOXIOCBASE, _n)) + +/** enable the device */ +#define IOX_ENABLE _IOXIOC(1) + +/** set constant values */ +#define IOX_SET_VALUE _IOXIOC(2) + +/** set constant values */ +#define IOX_SET_MODE _IOXIOC(3) + +/* enum passed to RGBLED_SET_MODE ioctl()*/ +enum IOX_MODE { + IOX_MODE_OFF, + IOX_MODE_ON +}; diff --git a/src/drivers/pca8574/module.mk b/src/drivers/pca8574/module.mk new file mode 100644 index 0000000000..825ee9bb7a --- /dev/null +++ b/src/drivers/pca8574/module.mk @@ -0,0 +1,6 @@ +# +# PCA8574 driver for RGB LED +# + +MODULE_COMMAND = pca8574 +SRCS = pca8574.cpp diff --git a/src/drivers/pca8574/pca8574.cpp b/src/drivers/pca8574/pca8574.cpp new file mode 100644 index 0000000000..ce882b6367 --- /dev/null +++ b/src/drivers/pca8574/pca8574.cpp @@ -0,0 +1,569 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Julian Oes + * Anton Babushkin + * + * 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 pca8574.cpp + * + * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#include + +#define PCA8574_ONTIME 120 +#define PCA8574_OFFTIME 120 +#define PCA8574_DEVICE_PATH "/dev/pca8574" + +#define ADDR 0x20 /**< I2C adress of PCA8574 (default, A0-A2 pulled to GND) */ +#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ +#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ +#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ +#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */ +#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/ + +#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */ +#define SETTING_ENABLE 0x02 /**< on */ + + +class PCA8574 : public device::I2C +{ +public: + PCA8574(int bus, int pca8574); + virtual ~PCA8574(); + + + virtual int init(); + virtual int probe(); + virtual int info(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + work_s _work; + + uint8_t _values[8]; + float _brightness; + + enum IOX_MODE _mode; + bool _running; + int _led_interval; + bool _should_run; + int _counter; + + static void led_trampoline(void *arg); + void led(); + + int send_led_enable(bool enable); + int send_led_values(); +}; + +/* for now, we only support one PCA8574 */ +namespace +{ +PCA8574 *g_pca8574; +} + +void pca8574_usage(); + +extern "C" __EXPORT int pca8574_main(int argc, char *argv[]); + +PCA8574::PCA8574(int bus, int pca8574) : + I2C("pca8574", PCA8574_DEVICE_PATH, bus, pca8574, 100000), + _mode(IOX_MODE_OFF), + _values({}), + _brightness(1.0f), + _running(false), + _led_interval(0), + _should_run(false), + _counter(0) +{ + memset(&_work, 0, sizeof(_work)); +} + +PCA8574::~PCA8574() +{ +} + +int +PCA8574::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + /* switch off LED on start */ + send_led_enable(false); + + /* kick it in */ + _should_run = true; + _led_interval = 80; + work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); + + return OK; +} + +int +PCA8574::probe() +{ + + return send_led_enable(false); +} + +int +PCA8574::info() +{ + int ret = OK; + + return ret; +} + +int +PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = ENOTTY; + + switch (cmd) { + case IOX_SET_VALUE ... (IOX_SET_VALUE + 8): + { + /* set the specified color */ + unsigned prev = _values[cmd - IOX_SET_VALUE]; + _values[cmd - IOX_SET_VALUE] = arg; + if (_values[cmd - IOX_SET_VALUE] != prev) { + // XXX will be done with a change flag + send_led_values(); + } + return OK; + } + + case IOX_ENABLE: + send_led_enable(arg); + return OK; + + default: + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + + +void +PCA8574::led_trampoline(void *arg) +{ + PCA8574 *rgbl = reinterpret_cast(arg); + + rgbl->led(); +} + +/** + * Main loop function + */ +void +PCA8574::led() +{ + if (!_should_run) { + _running = false; + return; + } + + // switch (_mode) { + // case PCA8574_MODE_BLINK_SLOW: + // case PCA8574_MODE_BLINK_NORMAL: + // case PCA8574_MODE_BLINK_FAST: + // if (_counter >= 2) + // _counter = 0; + + // send_led_enable(_counter == 0); + + // break; + + // case PCA8574_MODE_BREATHE: + + // if (_counter >= 62) + // _counter = 0; + + // int n; + + // if (_counter < 32) { + // n = _counter; + + // } else { + // n = 62 - _counter; + // } + + // _brightness = n * n / (31.0f * 31.0f); + // send_led_rgb(); + // break; + + // case PCA8574_MODE_PATTERN: + + // /* don't run out of the pattern array and stop if the next frame is 0 */ + // if (_counter >= PCA8574_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) + // _counter = 0; + + // set_color(_pattern.color[_counter]); + // send_led_rgb(); + // _led_interval = _pattern.duration[_counter]; + // break; + + // default: + // break; + // } + + + // we count only seven states + _counter &= 0xF; + _counter++; + + for (int i = 0; i < 8; i++) { + if (i < _counter) { + _values[i] = 1; + } else { + _values[i] = 0; + } + } + + send_led_values(); + + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, _led_interval); +} + +// /** +// * Set mode, if mode not changed has no any effect (doesn't reset blinks phase) +// */ +// void +// PCA8574::set_mode(pca8574_mode_t mode) +// { +// if (mode != _mode) { +// _mode = mode; + +// switch (mode) { +// // case PCA8574_MODE_OFF: +// // _should_run = false; +// // send_led_enable(false); +// // break; + +// // case PCA8574_MODE_ON: +// // _brightness = 1.0f; +// // send_led_rgb(); +// // send_led_enable(true); +// // break; + +// // case PCA8574_MODE_BLINK_SLOW: +// // _should_run = true; +// // _counter = 0; +// // _led_interval = 2000; +// // _brightness = 1.0f; +// // send_led_rgb(); +// // break; + +// // case PCA8574_MODE_BLINK_NORMAL: +// // _should_run = true; +// // _counter = 0; +// // _led_interval = 500; +// // _brightness = 1.0f; +// // send_led_rgb(); +// // break; + +// // case PCA8574_MODE_BLINK_FAST: +// // _should_run = true; +// // _counter = 0; +// // _led_interval = 100; +// // _brightness = 1.0f; +// // send_led_rgb(); +// // break; + +// // case PCA8574_MODE_BREATHE: +// // _should_run = true; +// // _counter = 0; +// // _led_interval = 25; +// // send_led_enable(true); +// // break; + +// // case PCA8574_MODE_PATTERN: +// // _should_run = true; +// // _counter = 0; +// // _brightness = 1.0f; +// // send_led_enable(true); +// // break; + +// default: +// warnx("mode unknown"); +// break; +// } + +// /* if it should run now, start the workq */ +// if (_should_run && !_running) { +// _running = true; +// work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); +// } + +// } +// } + +/** + * Sent ENABLE flag to LED driver + */ +int +PCA8574::send_led_enable(bool enable) +{ + uint8_t msg; + + if (enable) { + /* active low */ + msg = 0x00; + } else { + /* active low, so off */ + msg = 0xFF; + } + + int ret = transfer(&msg, sizeof(msg), nullptr, 0); + + return ret; +} + +/** + * Send RGB PWM settings to LED driver according to current color and brightness + */ +int +PCA8574::send_led_values() +{ + uint8_t msg = 0; + + for (int i = 0; i < 8; i++) { + if (_values[i]) { + msg |= (1 << i); + } + } + + int ret = transfer(&msg, sizeof(msg), nullptr, 0); +} + +// int +// PCA8574::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) +// { +// uint8_t result[2]; +// int ret; + +// ret = transfer(nullptr, 0, &result[0], 2); + +// if (ret == OK) { +// on = result[0] & SETTING_ENABLE; +// powersave = !(result[0] & SETTING_NOT_POWERSAVE); +// /* XXX check, looks wrong */ +// r = (result[0] & 0x0f) << 4; +// g = (result[1] & 0xf0); +// b = (result[1] & 0x0f) << 4; +// } + +// return ret; +// } + +void +pca8574_usage() +{ + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'val 0 100'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + warnx(" -a addr (0x%x)", ADDR); +} + +int +pca8574_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int pca8574adr = ADDR; /* 7bit */ + + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + pca8574adr = strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + + default: + pca8574_usage(); + exit(0); + } + } + + if (optind >= argc) { + pca8574_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int fd; + int ret; + + if (!strcmp(verb, "start")) { + if (g_pca8574 != nullptr) + errx(1, "already started"); + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_pca8574 = new PCA8574(PX4_I2C_BUS_EXPANSION, pca8574adr); + + if (g_pca8574 != nullptr && OK != g_pca8574->init()) { + delete g_pca8574; + g_pca8574 = nullptr; + } + + if (g_pca8574 == nullptr) { + // fall back to default bus + if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { + errx(1, "init failed"); + } + i2cdevice = PX4_I2C_BUS_LED; + } + } + + if (g_pca8574 == nullptr) { + g_pca8574 = new PCA8574(i2cdevice, pca8574adr); + + if (g_pca8574 == nullptr) + errx(1, "new failed"); + + if (OK != g_pca8574->init()) { + delete g_pca8574; + g_pca8574 = nullptr; + errx(1, "init failed"); + } + } + + exit(0); + } + + /* need the driver past this point */ + if (g_pca8574 == nullptr) { + warnx("not started"); + pca8574_usage(); + exit(1); + } + + if (!strcmp(verb, "test")) { + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_VALUE, 255); + // ret = ioctl(fd, PCA8574_SET_MODE, (unsigned long)PCA8574_MODE_PATTERN); + + close(fd); + exit(ret); + } + + if (!strcmp(verb, "info")) { + g_pca8574->info(); + exit(0); + } + + if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); + close(fd); + exit(ret); + } + + if (!strcmp(verb, "stop")) { + delete g_pca8574; + g_pca8574 = nullptr; + exit(0); + } + + if (!strcmp(verb, "val")) { + if (argc < 4) { + errx(1, "Usage: pca8574 val "); + } + + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + unsigned channel = strtol(argv[2], NULL, 0); + unsigned val = strtol(argv[3], NULL, 0); + ret = ioctl(fd, (IOX_SET_VALUE+channel), val); + ret = ioctl(fd, IOX_ENABLE, 1); + close(fd); + exit(ret); + } + + pca8574_usage(); + exit(0); +} From bd9d58f565383598db785908a7cc08f87f6a07f1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 12 May 2014 23:06:45 +0200 Subject: [PATCH 05/67] attitude_estimator_ekf: auto detect mag declination using GPS coordinates --- src/lib/geo/geo.h | 2 +- .../attitude_estimator_ekf_main.cpp | 26 +++++++++++++++---- 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index e2f3da6f80..87c1cf4605 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -50,7 +50,7 @@ __BEGIN_DECLS -#include "geo/geo_mag_declination.h" +#include "geo_mag_declination.h" #include diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index c61b6ff3fe..e10c7de560 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -64,6 +64,7 @@ #include #include +#include #include #include @@ -292,6 +293,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; unsigned offset_count = 0; + /* magnetic declination, in radians */ + float mag_decl = 0.0f; + /* rotation matrix for magnetic declination */ math::Matrix<3, 3> R_decl; R_decl.identity(); @@ -330,9 +334,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* update parameters */ parameters_update(&ekf_param_handles, &ekf_params); - - /* update mag declination rotation matrix */ - R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl); } /* only run filter if sensor values changed */ @@ -345,6 +346,13 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_check(sub_gps, &gps_updated); if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); + + if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { + mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); + + /* update mag declination rotation matrix */ + R_decl.from_euler(0.0f, 0.0f, mag_decl); + } } bool global_pos_updated; @@ -474,7 +482,15 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds parameters_update(&ekf_param_handles, &ekf_params); /* update mag declination rotation matrix */ - R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl); + if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { + mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); + + } else { + mag_decl = ekf_params.mag_decl; + } + + /* update mag declination rotation matrix */ + R_decl.from_euler(0.0f, 0.0f, mag_decl); x_aposteriori_k[0] = z_k[0]; x_aposteriori_k[1] = z_k[1]; @@ -522,7 +538,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.roll = euler[0]; att.pitch = euler[1]; - att.yaw = euler[2] + ekf_params.mag_decl; + att.yaw = euler[2] + mag_decl; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; From fbb3adde06e5ecf88a4c39e332a539fa12d173b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 May 2014 16:04:02 +0200 Subject: [PATCH 06/67] mavlink app: Clean up allocations --- src/modules/mavlink/mavlink_commands.cpp | 29 +- src/modules/mavlink/mavlink_main.cpp | 112 +- src/modules/mavlink/mavlink_main.h | 10 +- src/modules/mavlink/mavlink_messages.cpp | 1928 +++++++++-------- src/modules/mavlink/mavlink_messages.h | 15 +- .../mavlink/mavlink_orb_subscription.cpp | 19 +- .../mavlink/mavlink_orb_subscription.h | 18 +- src/modules/mavlink/mavlink_receiver.cpp | 1 - src/modules/mavlink/mavlink_stream.h | 22 +- 9 files changed, 1080 insertions(+), 1074 deletions(-) diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp index 1c1e097a43..5760d75121 100644 --- a/src/modules/mavlink/mavlink_commands.cpp +++ b/src/modules/mavlink/mavlink_commands.cpp @@ -43,7 +43,6 @@ MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel) { _cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); - _cmd = (struct vehicle_command_s *)_cmd_sub->get_data(); } MavlinkCommandsStream::~MavlinkCommandsStream() @@ -53,21 +52,23 @@ MavlinkCommandsStream::~MavlinkCommandsStream() void MavlinkCommandsStream::update(const hrt_abstime t) { - if (_cmd_sub->update(t)) { + struct vehicle_command_s cmd; + + if (_cmd_sub->update(t, &cmd)) { /* only send commands for other systems/components */ - if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) { + if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { mavlink_msg_command_long_send(_channel, - _cmd->target_system, - _cmd->target_component, - _cmd->command, - _cmd->confirmation, - _cmd->param1, - _cmd->param2, - _cmd->param3, - _cmd->param4, - _cmd->param5, - _cmd->param6, - _cmd->param7); + cmd.target_system, + cmd.target_component, + cmd.command, + cmd.confirmation, + cmd.param1, + cmd.param2, + cmd.param3, + cmd.param4, + cmd.param5, + cmd.param6, + cmd.param7); } } } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6c97bfca71..340b20e1b2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -234,6 +234,11 @@ Mavlink::Mavlink() : _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), _message_buffer({}), + _param_initialized(false), + _param_system_id(0), + _param_component_id(0), + _param_system_type(0), + _param_use_hil_gps(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) @@ -493,44 +498,39 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) void Mavlink::mavlink_update_system(void) { - static bool initialized = false; - static param_t param_system_id; - static param_t param_component_id; - static param_t param_system_type; - static param_t param_use_hil_gps; - if (!initialized) { - param_system_id = param_find("MAV_SYS_ID"); - param_component_id = param_find("MAV_COMP_ID"); - param_system_type = param_find("MAV_TYPE"); - param_use_hil_gps = param_find("MAV_USEHILGPS"); - initialized = true; + if (!_param_initialized) { + _param_system_id = param_find("MAV_SYS_ID"); + _param_component_id = param_find("MAV_COMP_ID"); + _param_system_type = param_find("MAV_TYPE"); + _param_use_hil_gps = param_find("MAV_USEHILGPS"); + _param_initialized = true; } /* update system and component id */ int32_t system_id; - param_get(param_system_id, &system_id); + param_get(_param_system_id, &system_id); if (system_id > 0 && system_id < 255) { mavlink_system.sysid = system_id; } int32_t component_id; - param_get(param_component_id, &component_id); + param_get(_param_component_id, &component_id); if (component_id > 0 && component_id < 255) { mavlink_system.compid = component_id; } int32_t system_type; - param_get(param_system_type, &system_type); + param_get(_param_system_type, &system_type); if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { mavlink_system.type = system_type; } int32_t use_hil_gps; - param_get(param_use_hil_gps, &use_hil_gps); + param_get(_param_use_hil_gps, &use_hil_gps); _use_hil_gps = (bool)use_hil_gps; } @@ -791,7 +791,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { /* Start sending parameters */ mavlink_pm_start_queued_send(); - mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + mavlink_missionlib_send_gcs_string("[pm] sending list"); } break; case MAVLINK_MSG_ID_PARAM_SET: { @@ -813,7 +813,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; - sprintf(buf, "[mavlink pm] unknown: %s", name); + sprintf(buf, "[pm] unknown: %s", name); mavlink_missionlib_send_gcs_string(buf); } else { @@ -1001,8 +1001,6 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) } else { mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); - - if (_verbose) { warnx("ERROR: index out of bounds"); } } } @@ -1073,8 +1071,6 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u } else { mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - - if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); } } } @@ -1105,8 +1101,6 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) mavlink_missionlib_send_gcs_string("Operation timeout"); - if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); } - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; _wpm->current_partner_sysid = 0; _wpm->current_partner_compid = 0; @@ -1137,8 +1131,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); - - if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); } } break; @@ -1162,21 +1154,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - - if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); } } } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - - if (_verbose) { warnx("IGN WP CURR CMD: Busy"); } - } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); } } break; @@ -1206,14 +1191,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - - if (_verbose) { warnx("IGN REQUEST LIST: Busy"); } } } else { mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); - - if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); } } break; @@ -1230,8 +1211,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); } - break; } @@ -1242,15 +1221,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpr.seq == 0) { - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u changing to STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); - if (_verbose) { warnx("REJ. WP CMD: First id != 0"); } - break; } @@ -1258,17 +1235,15 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpr.seq == _wpm->current_wp_id) { - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u (again) from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } } else if (wpr.seq == _wpm->current_wp_id + 1) { - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, _wpm->current_wp_id, _wpm->current_wp_id + 1); } - break; } @@ -1276,8 +1251,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", _wpm->current_state); } - break; } @@ -1291,7 +1264,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); } + mavlink_missionlib_send_gcs_string("ERROR: Waypoint out of bounds"); } @@ -1301,13 +1274,9 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); } - } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } @@ -1331,15 +1300,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } if (wpc.count == 0) { - mavlink_missionlib_send_gcs_string("COUNT 0"); - - if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); } + mavlink_missionlib_send_gcs_string("WP COUNT 0"); break; } - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST; _wpm->current_wp_id = 0; _wpm->current_partner_sysid = msg->sysid; @@ -1353,25 +1318,17 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_wpm->current_wp_id == 0) { mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); } - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", _wpm->current_wp_id); } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy with WP"); } } else { mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); - - if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); } } } else { mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } break; @@ -1393,7 +1350,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wp.seq != 0) { mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); break; } @@ -1401,12 +1357,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wp.seq >= _wpm->current_count) { mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq); break; } if (wp.seq != _wpm->current_wp_id) { - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, _wpm->current_wp_id); + mavlink_missionlib_send_gcs_string("IGN: waypoint ID mismatch"); mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); break; } @@ -1473,8 +1428,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1515,8 +1468,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1535,8 +1486,7 @@ Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); - - mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len); + mavlink_send_uart_bytes(_channel, buf, len); } @@ -1619,6 +1569,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate) if (interval > 0) { /* search for stream with specified name in supported streams list */ for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { /* create new instance */ stream = streams_list[i]->new_instance(); @@ -1924,7 +1875,7 @@ Mavlink::task_main(int argc, char *argv[]) /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ if (_passing_on) { /* initialize message buffer if multiplexing is on */ - if (OK != message_buffer_init(500)) { + if (OK != message_buffer_init(300)) { errx(1, "can't allocate message buffer, exiting"); } @@ -1956,7 +1907,8 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); - struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); + struct vehicle_status_s status; + status_sub->update(0, &status); MavlinkCommandsStream commands_stream(this, _channel); @@ -2013,14 +1965,14 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); - if (param_sub->update(t)) { + if (param_sub->update(t, nullptr)) { /* parameters updated */ mavlink_update_system(); } - if (status_sub->update(t)) { + if (status_sub->update(t, &status)) { /* switch HIL mode if required */ - set_hil_enabled(status->hil_state == HIL_STATE_ON); + set_hil_enabled(status.hil_state == HIL_STATE_ON); } /* update commands stream */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c7a7d32f84..1f0445cb6f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -278,11 +278,15 @@ private: int size; char *data; }; - mavlink_message_buffer _message_buffer; - - pthread_mutex_t _message_buffer_mutex; + mavlink_message_buffer _message_buffer; + pthread_mutex_t _message_buffer_mutex; + bool _param_initialized; + param_t _param_system_id; + param_t _param_component_id; + param_t _param_system_type; + param_t _param_use_hil_gps; /** * Send one parameter. diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 79dd88657b..4bb827116d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -74,6 +74,8 @@ #include #include +#include + #include "mavlink_messages.h" @@ -189,42 +191,51 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set class MavlinkStreamHeartbeat : public MavlinkStream { public: - const char *get_name() + + ~MavlinkStreamHeartbeat() {}; + + const char *get_name() const + { + return MavlinkStreamHeartbeat::get_name_static(); + } + + static const char *get_name_static() { return "HEARTBEAT"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamHeartbeat(); } private: MavlinkOrbSubscription *status_sub; - struct vehicle_status_s *status; - MavlinkOrbSubscription *pos_sp_triplet_sub; - struct position_setpoint_triplet_s *pos_sp_triplet; + protected: + + explicit MavlinkStreamHeartbeat() {}; + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - status = (struct vehicle_status_s *)status_sub->get_data(); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } void send(const hrt_abstime t) { - (void)status_sub->update(t); - (void)pos_sp_triplet_sub->update(t); + struct vehicle_status_s status; + struct position_setpoint_triplet_s pos_sp_triplet; + + (void)status_sub->update(t, &status); + (void)pos_sp_triplet_sub->update(t, &pos_sp_triplet); uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); mavlink_msg_heartbeat_send(_channel, mavlink_system.type, @@ -240,12 +251,19 @@ protected: class MavlinkStreamSysStatus : public MavlinkStream { public: - const char *get_name() + ~MavlinkStreamSysStatus() {}; + + const char *get_name() const + { + return MavlinkStreamSysStatus::get_name_static(); + } + + static const char *get_name_static () { return "SYS_STATUS"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamSysStatus(); } @@ -255,29 +273,31 @@ private: struct vehicle_status_s *status; protected: + explicit MavlinkStreamSysStatus() {}; + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - status = (struct vehicle_status_s *)status_sub->get_data(); } void send(const hrt_abstime t) { - status_sub->update(t); + struct vehicle_status_s status; + (void)status_sub->update(t, &status); mavlink_msg_sys_status_send(_channel, - status->onboard_control_sensors_present, - status->onboard_control_sensors_enabled, - status->onboard_control_sensors_health, - status->load * 1000.0f, - status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, - status->battery_remaining * 100.0f, - status->drop_rate_comm, - status->errors_comm, - status->errors_count1, - status->errors_count2, - status->errors_count3, - status->errors_count4); + status.onboard_control_sensors_present, + status.onboard_control_sensors_enabled, + status.onboard_control_sensors_health, + status.load * 1000.0f, + status.battery_voltage * 1000.0f, + status.battery_current * 1000.0f, + status.battery_remaining * 100.0f, + status.drop_rate_comm, + status.errors_comm, + status.errors_count1, + status.errors_count2, + status.errors_count3, + status.errors_count4); } }; @@ -285,23 +305,25 @@ protected: class MavlinkStreamHighresIMU : public MavlinkStream { public: - MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0) + ~MavlinkStreamHighresIMU(); + + const char *get_name() const { + return MavlinkStreamHighresIMU::get_name_static(); } - const char *get_name() + static const char *get_name_static() { return "HIGHRES_IMU"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamHighresIMU(); } private: MavlinkOrbSubscription *sensor_sub; - struct sensor_combined_s *sensor; uint64_t accel_timestamp; uint64_t gyro_timestamp; @@ -309,48 +331,52 @@ private: uint64_t baro_timestamp; protected: + explicit MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0) + { + } + void subscribe(Mavlink *mavlink) { sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); - sensor = (struct sensor_combined_s *)sensor_sub->get_data(); } void send(const hrt_abstime t) { - if (sensor_sub->update(t)) { + struct sensor_combined_s sensor; + if (sensor_sub->update(t, &sensor)) { uint16_t fields_updated = 0; - if (accel_timestamp != sensor->accelerometer_timestamp) { + if (accel_timestamp != sensor.accelerometer_timestamp) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_timestamp = sensor->accelerometer_timestamp; + accel_timestamp = sensor.accelerometer_timestamp; } - if (gyro_timestamp != sensor->timestamp) { + if (gyro_timestamp != sensor.timestamp) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_timestamp = sensor->timestamp; + gyro_timestamp = sensor.timestamp; } - if (mag_timestamp != sensor->magnetometer_timestamp) { + if (mag_timestamp != sensor.magnetometer_timestamp) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_timestamp = sensor->magnetometer_timestamp; + mag_timestamp = sensor.magnetometer_timestamp; } - if (baro_timestamp != sensor->baro_timestamp) { + if (baro_timestamp != sensor.baro_timestamp) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_timestamp = sensor->baro_timestamp; + baro_timestamp = sensor.baro_timestamp; } mavlink_msg_highres_imu_send(_channel, - sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, + sensor.timestamp, + sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2], + sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2], + sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2], + sensor.baro_pres_mbar, sensor.differential_pressure_pa, + sensor.baro_alt_meter, sensor.baro_temp_celcius, fields_updated); } } @@ -360,12 +386,17 @@ protected: class MavlinkStreamAttitude : public MavlinkStream { public: - const char *get_name() + const char *get_name() const + { + return MavlinkStreamAttitude::get_name_static(); + } + + static const char *get_name_static() { return "ATTITUDE"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamAttitude(); } @@ -378,16 +409,17 @@ protected: void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - att = (struct vehicle_attitude_s *)att_sub->get_data(); } void send(const hrt_abstime t) { - if (att_sub->update(t)) { + struct vehicle_attitude_s att; + + if (att_sub->update(t, &att)) { mavlink_msg_attitude_send(_channel, - att->timestamp / 1000, - att->roll, att->pitch, att->yaw, - att->rollspeed, att->pitchspeed, att->yawspeed); + att.timestamp / 1000, + att.roll, att.pitch, att.yaw, + att.rollspeed, att.pitchspeed, att.yawspeed); } } }; @@ -396,39 +428,44 @@ protected: class MavlinkStreamAttitudeQuaternion : public MavlinkStream { public: - const char *get_name() + const char *get_name() const + { + return MavlinkStreamAttitudeQuaternion::get_name_static(); + } + + static const char *get_name_static() { return "ATTITUDE_QUATERNION"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamAttitudeQuaternion(); } private: MavlinkOrbSubscription *att_sub; - struct vehicle_attitude_s *att; protected: void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - att = (struct vehicle_attitude_s *)att_sub->get_data(); } void send(const hrt_abstime t) { - if (att_sub->update(t)) { + struct vehicle_attitude_s att; + + if (att_sub->update(t, &att)) { mavlink_msg_attitude_quaternion_send(_channel, - att->timestamp / 1000, - att->q[0], - att->q[1], - att->q[2], - att->q[3], - att->rollspeed, - att->pitchspeed, - att->yawspeed); + att.timestamp / 1000, + att.q[0], + att.q[1], + att.q[2], + att.q[3], + att.rollspeed, + att.pitchspeed, + att.yawspeed); } } }; @@ -437,12 +474,18 @@ protected: class MavlinkStreamVFRHUD : public MavlinkStream { public: - const char *get_name() + + const char *get_name() const + { + return MavlinkStreamVFRHUD::get_name_static(); + } + + static const char *get_name_static() { return "VFR_HUD"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamVFRHUD(); } @@ -467,41 +510,38 @@ protected: void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); - att = (struct vehicle_attitude_s *)att_sub->get_data(); - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - pos = (struct vehicle_global_position_s *)pos_sub->get_data(); - armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); - armed = (struct actuator_armed_s *)armed_sub->get_data(); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); - act = (struct actuator_controls_s *)act_sub->get_data(); - airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); - airspeed = (struct airspeed_s *)airspeed_sub->get_data(); } void send(const hrt_abstime t) { - bool updated = att_sub->update(t); - updated |= pos_sub->update(t); - updated |= armed_sub->update(t); - updated |= act_sub->update(t); - updated |= airspeed_sub->update(t); + struct vehicle_attitude_s att; + struct vehicle_global_position_s pos; + struct actuator_armed_s armed; + struct actuator_controls_s act; + struct airspeed_s airspeed; + + bool updated = att_sub->update(t, &att); + updated |= pos_sub->update(t, &pos); + updated |= armed_sub->update(t, &armed); + updated |= act_sub->update(t, &act); + updated |= airspeed_sub->update(t, &airspeed); if (updated) { - float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e); - uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F; - float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; + float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); + uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; + float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; mavlink_msg_vfr_hud_send(_channel, - airspeed->true_airspeed_m_s, + airspeed.true_airspeed_m_s, groundspeed, heading, throttle, - pos->alt, - -pos->vel_d); + pos.alt, + pos.vel_d); } } }; @@ -510,12 +550,17 @@ protected: class MavlinkStreamGPSRawInt : public MavlinkStream { public: - const char *get_name() + const char *get_name() const + { + return MavlinkStreamGPSRawInt::get_name_static(); + } + + static const char *get_name_static() { return "GPS_RAW_INT"; } - MavlinkStream *new_instance() + static MavlinkStream *new_instance() { return new MavlinkStreamGPSRawInt(); } @@ -528,864 +573,865 @@ protected: void subscribe(Mavlink *mavlink) { gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); - gps = (struct vehicle_gps_position_s *)gps_sub->get_data(); } void send(const hrt_abstime t) { - if (gps_sub->update(t)) { + struct vehicle_gps_position_s gps; + + if (gps_sub->update(t, &gps)) { mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + gps.timestamp_position, + gps.fix_type, + gps.lat, + gps.lon, + gps.alt, + cm_uint16_from_m_float(gps.eph_m), + cm_uint16_from_m_float(gps.epv_m), + gps.vel_m_s * 100.0f, + _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps.satellites_visible); } } }; -class MavlinkStreamGlobalPositionInt : public MavlinkStream -{ -public: - const char *get_name() - { - return "GLOBAL_POSITION_INT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamGlobalPositionInt(); - } - -private: - MavlinkOrbSubscription *pos_sub; - struct vehicle_global_position_s *pos; - - MavlinkOrbSubscription *home_sub; - struct home_position_s *home; - -protected: - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); - pos = (struct vehicle_global_position_s *)pos_sub->get_data(); - - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - home = (struct home_position_s *)home_sub->get_data(); - } - - void send(const hrt_abstime t) - { - bool updated = pos_sub->update(t); - updated |= home_sub->update(t); - - if (updated) { - mavlink_msg_global_position_int_send(_channel, - pos->timestamp / 1000, - pos->lat * 1e7, - pos->lon * 1e7, - pos->alt * 1000.0f, - (pos->alt - home->alt) * 1000.0f, - pos->vel_n * 100.0f, - pos->vel_e * 100.0f, - pos->vel_d * 100.0f, - _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); - } - } -}; - - -class MavlinkStreamLocalPositionNED : public MavlinkStream -{ -public: - const char *get_name() - { - return "LOCAL_POSITION_NED"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamLocalPositionNED(); - } - -private: - MavlinkOrbSubscription *pos_sub; - struct vehicle_local_position_s *pos; - -protected: - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); - pos = (struct vehicle_local_position_s *)pos_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (pos_sub->update(t)) { - mavlink_msg_local_position_ned_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->vx, - pos->vy, - pos->vz); - } - } -}; - - - -class MavlinkStreamViconPositionEstimate : public MavlinkStream -{ -public: - const char *get_name() - { - return "VICON_POSITION_ESTIMATE"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamViconPositionEstimate(); - } - -private: - MavlinkOrbSubscription *pos_sub; - struct vehicle_vicon_position_s *pos; - -protected: - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); - pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (pos_sub->update(t)) { - mavlink_msg_vicon_position_estimate_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->roll, - pos->pitch, - pos->yaw); - } - } -}; - - -class MavlinkStreamGPSGlobalOrigin : public MavlinkStream -{ -public: - const char *get_name() - { - return "GPS_GLOBAL_ORIGIN"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamGPSGlobalOrigin(); - } - -private: - MavlinkOrbSubscription *home_sub; - struct home_position_s *home; - -protected: - void subscribe(Mavlink *mavlink) - { - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); - home = (struct home_position_s *)home_sub->get_data(); - } - - void send(const hrt_abstime t) - { - - /* we're sending the GPS home periodically to ensure the - * the GCS does pick it up at one point */ - if (home_sub->is_published()) { - home_sub->update(t); - - mavlink_msg_gps_global_origin_send(_channel, - (int32_t)(home->lat * 1e7), - (int32_t)(home->lon * 1e7), - (int32_t)(home->alt) * 1000.0f); - } - } -}; - - -class MavlinkStreamServoOutputRaw : public MavlinkStream -{ -public: - MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n) - { - sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n); - } - - const char *get_name() - { - return _name; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamServoOutputRaw(_n); - } - -private: - MavlinkOrbSubscription *act_sub; - struct actuator_outputs_s *act; - - char _name[20]; - unsigned int _n; - -protected: - void subscribe(Mavlink *mavlink) - { - orb_id_t act_topics[] = { - ORB_ID(actuator_outputs_0), - ORB_ID(actuator_outputs_1), - ORB_ID(actuator_outputs_2), - ORB_ID(actuator_outputs_3) - }; - - act_sub = mavlink->add_orb_subscription(act_topics[_n]); - act = (struct actuator_outputs_s *)act_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (act_sub->update(t)) { - mavlink_msg_servo_output_raw_send(_channel, - act->timestamp / 1000, - _n, - act->output[0], - act->output[1], - act->output[2], - act->output[3], - act->output[4], - act->output[5], - act->output[6], - act->output[7]); - } - } -}; - - -class MavlinkStreamHILControls : public MavlinkStream -{ -public: - const char *get_name() - { - return "HIL_CONTROLS"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamHILControls(); - } - -private: - MavlinkOrbSubscription *status_sub; - struct vehicle_status_s *status; - - MavlinkOrbSubscription *pos_sp_triplet_sub; - struct position_setpoint_triplet_s *pos_sp_triplet; - - MavlinkOrbSubscription *act_sub; - struct actuator_outputs_s *act; - -protected: - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - status = (struct vehicle_status_s *)status_sub->get_data(); - - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); - - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); - act = (struct actuator_outputs_s *)act_sub->get_data(); - } - - void send(const hrt_abstime t) - { - bool updated = act_sub->update(t); - (void)pos_sp_triplet_sub->update(t); - (void)status_sub->update(t); - - if (updated && (status->arming_state == ARMING_STATE_ARMED)) { - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state; - uint8_t mavlink_base_mode; - uint32_t mavlink_custom_mode; - get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - if (mavlink_system.type == MAV_TYPE_QUADROTOR || - mavlink_system.type == MAV_TYPE_HEXAROTOR || - mavlink_system.type == MAV_TYPE_OCTOROTOR) { - /* set number of valid outputs depending on vehicle type */ - unsigned n; - - switch (mavlink_system.type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; - - case MAV_TYPE_HEXAROTOR: - n = 6; - break; - - default: - n = 8; - break; - } - - /* scale / assign outputs depending on system type */ - float out[8]; - - for (unsigned i = 0; i < 8; i++) { - if (i < n) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ - out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); - - } else { - /* send 0 when disarmed */ - out[i] = 0.0f; - } - - } else { - out[i] = -1.0f; - } - } - - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); - } else { - - /* fixed wing: scale all channels except throttle -1 .. 1 - * because we know that we set the mixers up this way - */ - - float out[8]; - - const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; - - for (unsigned i = 0; i < 8; i++) { - if (i != 3) { - /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ - out[i] = (act->output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); - - } else { - - /* scale fake PWM out 900..2100 us to 0..1 for throttle */ - out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); - } - - } - - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); - } - } - } -}; - - -class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream -{ -public: - const char *get_name() - { - return "GLOBAL_POSITION_SETPOINT_INT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamGlobalPositionSetpointInt(); - } - -private: - MavlinkOrbSubscription *pos_sp_triplet_sub; - struct position_setpoint_triplet_s *pos_sp_triplet; - -protected: - void subscribe(Mavlink *mavlink) - { - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); - pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (pos_sp_triplet_sub->update(t)) { - mavlink_msg_global_position_setpoint_int_send(_channel, - MAV_FRAME_GLOBAL, - (int32_t)(pos_sp_triplet->current.lat * 1e7), - (int32_t)(pos_sp_triplet->current.lon * 1e7), - (int32_t)(pos_sp_triplet->current.alt * 1000), - (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); - } - } -}; - - -class MavlinkStreamLocalPositionSetpoint : public MavlinkStream -{ -public: - const char *get_name() - { - return "LOCAL_POSITION_SETPOINT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamLocalPositionSetpoint(); - } - -private: - MavlinkOrbSubscription *pos_sp_sub; - struct vehicle_local_position_setpoint_s *pos_sp; - -protected: - void subscribe(Mavlink *mavlink) - { - pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); - pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (pos_sp_sub->update(t)) { - mavlink_msg_local_position_setpoint_send(_channel, - MAV_FRAME_LOCAL_NED, - pos_sp->x, - pos_sp->y, - pos_sp->z, - pos_sp->yaw); - } - } -}; - - -class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() - { - return "ROLL_PITCH_YAW_THRUST_SETPOINT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamRollPitchYawThrustSetpoint(); - } - -private: - MavlinkOrbSubscription *att_sp_sub; - struct vehicle_attitude_setpoint_s *att_sp; - -protected: - void subscribe(Mavlink *mavlink) - { - att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); - att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (att_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, - att_sp->timestamp / 1000, - att_sp->roll_body, - att_sp->pitch_body, - att_sp->yaw_body, - att_sp->thrust); - } - } -}; - - -class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream -{ -public: - const char *get_name() - { - return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); - } - -private: - MavlinkOrbSubscription *att_rates_sp_sub; - struct vehicle_rates_setpoint_s *att_rates_sp; - -protected: - void subscribe(Mavlink *mavlink) - { - att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); - att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (att_rates_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, - att_rates_sp->timestamp / 1000, - att_rates_sp->roll, - att_rates_sp->pitch, - att_rates_sp->yaw, - att_rates_sp->thrust); - } - } -}; - - -class MavlinkStreamRCChannelsRaw : public MavlinkStream -{ -public: - const char *get_name() - { - return "RC_CHANNELS_RAW"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamRCChannelsRaw(); - } - -private: - MavlinkOrbSubscription *rc_sub; - struct rc_input_values *rc; - -protected: - void subscribe(Mavlink *mavlink) - { - rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); - rc = (struct rc_input_values *)rc_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (rc_sub->update(t)) { - const unsigned port_width = 8; - - for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(_channel, - rc->timestamp_publication / 1000, - i, - (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, - rc->rssi); - } - } - } -}; - - -class MavlinkStreamManualControl : public MavlinkStream -{ -public: - const char *get_name() - { - return "MANUAL_CONTROL"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamManualControl(); - } - -private: - MavlinkOrbSubscription *manual_sub; - struct manual_control_setpoint_s *manual; - -protected: - void subscribe(Mavlink *mavlink) - { - manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); - manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (manual_sub->update(t)) { - mavlink_msg_manual_control_send(_channel, - mavlink_system.sysid, - manual->x * 1000, - manual->y * 1000, - manual->z * 1000, - manual->r * 1000, - 0); - } - } -}; - - -class MavlinkStreamOpticalFlow : public MavlinkStream -{ -public: - const char *get_name() - { - return "OPTICAL_FLOW"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamOpticalFlow(); - } - -private: - MavlinkOrbSubscription *flow_sub; - struct optical_flow_s *flow; - -protected: - void subscribe(Mavlink *mavlink) - { - flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); - flow = (struct optical_flow_s *)flow_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (flow_sub->update(t)) { - mavlink_msg_optical_flow_send(_channel, - flow->timestamp, - flow->sensor_id, - flow->flow_raw_x, flow->flow_raw_y, - flow->flow_comp_x_m, flow->flow_comp_y_m, - flow->quality, - flow->ground_distance_m); - } - } -}; - -class MavlinkStreamAttitudeControls : public MavlinkStream -{ -public: - const char *get_name() - { - return "ATTITUDE_CONTROLS"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamAttitudeControls(); - } - -private: - MavlinkOrbSubscription *att_ctrl_sub; - struct actuator_controls_s *att_ctrl; - -protected: - void subscribe(Mavlink *mavlink) - { - att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (att_ctrl_sub->update(t)) { - /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "rll ctrl ", - att_ctrl->control[0]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "ptch ctrl ", - att_ctrl->control[1]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "yaw ctrl ", - att_ctrl->control[2]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "thr ctrl ", - att_ctrl->control[3]); - } - } -}; - -class MavlinkStreamNamedValueFloat : public MavlinkStream -{ -public: - const char *get_name() - { - return "NAMED_VALUE_FLOAT"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamNamedValueFloat(); - } - -private: - MavlinkOrbSubscription *debug_sub; - struct debug_key_value_s *debug; - -protected: - void subscribe(Mavlink *mavlink) - { - debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); - debug = (struct debug_key_value_s *)debug_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (debug_sub->update(t)) { - /* enforce null termination */ - debug->key[sizeof(debug->key) - 1] = '\0'; - - mavlink_msg_named_value_float_send(_channel, - debug->timestamp_ms, - debug->key, - debug->value); - } - } -}; - -class MavlinkStreamCameraCapture : public MavlinkStream -{ -public: - const char *get_name() - { - return "CAMERA_CAPTURE"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamCameraCapture(); - } - -private: - MavlinkOrbSubscription *status_sub; - struct vehicle_status_s *status; - -protected: - void subscribe(Mavlink *mavlink) - { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); - status = (struct vehicle_status_s *)status_sub->get_data(); - } - - void send(const hrt_abstime t) - { - (void)status_sub->update(t); - - if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { - - /* send camera capture on */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); - - } else { - /* send camera capture off */ - mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); - } - } -}; - -class MavlinkStreamDistanceSensor : public MavlinkStream -{ -public: - const char *get_name() - { - return "DISTANCE_SENSOR"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamDistanceSensor(); - } - -private: - MavlinkOrbSubscription *range_sub; - struct range_finder_report *range; - -protected: - void subscribe(Mavlink *mavlink) - { - range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); - range = (struct range_finder_report *)range_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (range_sub->update(t)) { - - uint8_t type; - - switch (range->type) { - case RANGE_FINDER_TYPE_LASER: - type = MAV_DISTANCE_SENSOR_LASER; - break; - } - - uint8_t id = 0; - uint8_t orientation = 0; - uint8_t covariance = 20; - - mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, - range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); - } - } -}; - -MavlinkStream *streams_list[] = { - new MavlinkStreamHeartbeat(), - new MavlinkStreamSysStatus(), - new MavlinkStreamHighresIMU(), - new MavlinkStreamAttitude(), - new MavlinkStreamAttitudeQuaternion(), - new MavlinkStreamVFRHUD(), - new MavlinkStreamGPSRawInt(), - new MavlinkStreamGlobalPositionInt(), - new MavlinkStreamLocalPositionNED(), - new MavlinkStreamGPSGlobalOrigin(), - new MavlinkStreamServoOutputRaw(0), - new MavlinkStreamServoOutputRaw(1), - new MavlinkStreamServoOutputRaw(2), - new MavlinkStreamServoOutputRaw(3), - new MavlinkStreamHILControls(), - new MavlinkStreamGlobalPositionSetpointInt(), - new MavlinkStreamLocalPositionSetpoint(), - new MavlinkStreamRollPitchYawThrustSetpoint(), - new MavlinkStreamRollPitchYawRatesThrustSetpoint(), - new MavlinkStreamRCChannelsRaw(), - new MavlinkStreamManualControl(), - new MavlinkStreamOpticalFlow(), - new MavlinkStreamAttitudeControls(), - new MavlinkStreamNamedValueFloat(), - new MavlinkStreamCameraCapture(), - new MavlinkStreamDistanceSensor(), - new MavlinkStreamViconPositionEstimate(), +// class MavlinkStreamGlobalPositionInt : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "GLOBAL_POSITION_INT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamGlobalPositionInt(); +// } + +// private: +// MavlinkOrbSubscription *pos_sub; +// struct vehicle_global_position_s *pos; + +// MavlinkOrbSubscription *home_sub; +// struct home_position_s *home; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); +// pos = (struct vehicle_global_position_s *)pos_sub->get_data(); + +// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// home = (struct home_position_s *)home_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// bool updated = pos_sub->update(t); +// updated |= home_sub->update(t); + +// if (updated) { +// mavlink_msg_global_position_int_send(_channel, +// pos->timestamp / 1000, +// pos->lat * 1e7, +// pos->lon * 1e7, +// pos->alt * 1000.0f, +// (pos->alt - home->alt) * 1000.0f, +// pos->vel_n * 100.0f, +// pos->vel_e * 100.0f, +// pos->vel_d * 100.0f, +// _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); +// } +// } +// }; + + +// class MavlinkStreamLocalPositionNED : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "LOCAL_POSITION_NED"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamLocalPositionNED(); +// } + +// private: +// MavlinkOrbSubscription *pos_sub; +// struct vehicle_local_position_s *pos; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); +// pos = (struct vehicle_local_position_s *)pos_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (pos_sub->update(t)) { +// mavlink_msg_local_position_ned_send(_channel, +// pos->timestamp / 1000, +// pos->x, +// pos->y, +// pos->z, +// pos->vx, +// pos->vy, +// pos->vz); +// } +// } +// }; + + + +// class MavlinkStreamViconPositionEstimate : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "VICON_POSITION_ESTIMATE"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamViconPositionEstimate(); +// } + +// private: +// MavlinkOrbSubscription *pos_sub; +// struct vehicle_vicon_position_s *pos; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); +// pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (pos_sub->update(t)) { +// mavlink_msg_vicon_position_estimate_send(_channel, +// pos->timestamp / 1000, +// pos->x, +// pos->y, +// pos->z, +// pos->roll, +// pos->pitch, +// pos->yaw); +// } +// } +// }; + + +// class MavlinkStreamGPSGlobalOrigin : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "GPS_GLOBAL_ORIGIN"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamGPSGlobalOrigin(); +// } + +// private: +// MavlinkOrbSubscription *home_sub; +// struct home_position_s *home; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); +// home = (struct home_position_s *)home_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { + +// /* we're sending the GPS home periodically to ensure the +// * the GCS does pick it up at one point */ +// if (home_sub->is_published()) { +// home_sub->update(t); + +// mavlink_msg_gps_global_origin_send(_channel, +// (int32_t)(home->lat * 1e7), +// (int32_t)(home->lon * 1e7), +// (int32_t)(home->alt) * 1000.0f); +// } +// } +// }; + + +// class MavlinkStreamServoOutputRaw : public MavlinkStream +// { +// public: +// MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n) +// { +// sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n); +// } + +// const char *get_name() +// { +// return _name; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamServoOutputRaw(_n); +// } + +// private: +// MavlinkOrbSubscription *act_sub; +// struct actuator_outputs_s *act; + +// char _name[20]; +// unsigned int _n; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// orb_id_t act_topics[] = { +// ORB_ID(actuator_outputs_0), +// ORB_ID(actuator_outputs_1), +// ORB_ID(actuator_outputs_2), +// ORB_ID(actuator_outputs_3) +// }; + +// act_sub = mavlink->add_orb_subscription(act_topics[_n]); +// act = (struct actuator_outputs_s *)act_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (act_sub->update(t)) { +// mavlink_msg_servo_output_raw_send(_channel, +// act->timestamp / 1000, +// _n, +// act->output[0], +// act->output[1], +// act->output[2], +// act->output[3], +// act->output[4], +// act->output[5], +// act->output[6], +// act->output[7]); +// } +// } +// }; + + +// class MavlinkStreamHILControls : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "HIL_CONTROLS"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamHILControls(); +// } + +// private: +// MavlinkOrbSubscription *status_sub; +// struct vehicle_status_s *status; + +// MavlinkOrbSubscription *pos_sp_triplet_sub; +// struct position_setpoint_triplet_s *pos_sp_triplet; + +// MavlinkOrbSubscription *act_sub; +// struct actuator_outputs_s *act; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); +// status = (struct vehicle_status_s *)status_sub->get_data(); + +// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); + +// act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); +// act = (struct actuator_outputs_s *)act_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// bool updated = act_sub->update(t); +// (void)pos_sp_triplet_sub->update(t); +// (void)status_sub->update(t); + +// if (updated && (status->arming_state == ARMING_STATE_ARMED)) { +// /* translate the current syste state to mavlink state and mode */ +// uint8_t mavlink_state; +// uint8_t mavlink_base_mode; +// uint32_t mavlink_custom_mode; +// get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + +// if (mavlink_system.type == MAV_TYPE_QUADROTOR || +// mavlink_system.type == MAV_TYPE_HEXAROTOR || +// mavlink_system.type == MAV_TYPE_OCTOROTOR) { +// /* set number of valid outputs depending on vehicle type */ +// unsigned n; + +// switch (mavlink_system.type) { +// case MAV_TYPE_QUADROTOR: +// n = 4; +// break; + +// case MAV_TYPE_HEXAROTOR: +// n = 6; +// break; + +// default: +// n = 8; +// break; +// } + +// /* scale / assign outputs depending on system type */ +// float out[8]; + +// for (unsigned i = 0; i < 8; i++) { +// if (i < n) { +// if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { +// /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ +// out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + +// } else { +// /* send 0 when disarmed */ +// out[i] = 0.0f; +// } + +// } else { +// out[i] = -1.0f; +// } +// } + +// mavlink_msg_hil_controls_send(_channel, +// hrt_absolute_time(), +// out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], +// mavlink_base_mode, +// 0); +// } else { + +// /* fixed wing: scale all channels except throttle -1 .. 1 +// * because we know that we set the mixers up this way +// */ + +// float out[8]; + +// const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + +// for (unsigned i = 0; i < 8; i++) { +// if (i != 3) { +// /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ +// out[i] = (act->output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + +// } else { + +// /* scale fake PWM out 900..2100 us to 0..1 for throttle */ +// out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); +// } + +// } + +// mavlink_msg_hil_controls_send(_channel, +// hrt_absolute_time(), +// out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], +// mavlink_base_mode, +// 0); +// } +// } +// } +// }; + + +// class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "GLOBAL_POSITION_SETPOINT_INT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamGlobalPositionSetpointInt(); +// } + +// private: +// MavlinkOrbSubscription *pos_sp_triplet_sub; +// struct position_setpoint_triplet_s *pos_sp_triplet; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); +// pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (pos_sp_triplet_sub->update(t)) { +// mavlink_msg_global_position_setpoint_int_send(_channel, +// MAV_FRAME_GLOBAL, +// (int32_t)(pos_sp_triplet->current.lat * 1e7), +// (int32_t)(pos_sp_triplet->current.lon * 1e7), +// (int32_t)(pos_sp_triplet->current.alt * 1000), +// (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); +// } +// } +// }; + + +// class MavlinkStreamLocalPositionSetpoint : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "LOCAL_POSITION_SETPOINT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamLocalPositionSetpoint(); +// } + +// private: +// MavlinkOrbSubscription *pos_sp_sub; +// struct vehicle_local_position_setpoint_s *pos_sp; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); +// pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (pos_sp_sub->update(t)) { +// mavlink_msg_local_position_setpoint_send(_channel, +// MAV_FRAME_LOCAL_NED, +// pos_sp->x, +// pos_sp->y, +// pos_sp->z, +// pos_sp->yaw); +// } +// } +// }; + + +// class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "ROLL_PITCH_YAW_THRUST_SETPOINT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamRollPitchYawThrustSetpoint(); +// } + +// private: +// MavlinkOrbSubscription *att_sp_sub; +// struct vehicle_attitude_setpoint_s *att_sp; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); +// att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (att_sp_sub->update(t)) { +// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, +// att_sp->timestamp / 1000, +// att_sp->roll_body, +// att_sp->pitch_body, +// att_sp->yaw_body, +// att_sp->thrust); +// } +// } +// }; + + +// class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); +// } + +// private: +// MavlinkOrbSubscription *att_rates_sp_sub; +// struct vehicle_rates_setpoint_s *att_rates_sp; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); +// att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (att_rates_sp_sub->update(t)) { +// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, +// att_rates_sp->timestamp / 1000, +// att_rates_sp->roll, +// att_rates_sp->pitch, +// att_rates_sp->yaw, +// att_rates_sp->thrust); +// } +// } +// }; + + +// class MavlinkStreamRCChannelsRaw : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "RC_CHANNELS_RAW"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamRCChannelsRaw(); +// } + +// private: +// MavlinkOrbSubscription *rc_sub; +// struct rc_input_values *rc; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); +// rc = (struct rc_input_values *)rc_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (rc_sub->update(t)) { +// const unsigned port_width = 8; + +// for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { +// /* Channels are sent in MAVLink main loop at a fixed interval */ +// mavlink_msg_rc_channels_raw_send(_channel, +// rc->timestamp_publication / 1000, +// i, +// (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, +// (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, +// rc->rssi); +// } +// } +// } +// }; + + +// class MavlinkStreamManualControl : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "MANUAL_CONTROL"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamManualControl(); +// } + +// private: +// MavlinkOrbSubscription *manual_sub; +// struct manual_control_setpoint_s *manual; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); +// manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (manual_sub->update(t)) { +// mavlink_msg_manual_control_send(_channel, +// mavlink_system.sysid, +// manual->x * 1000, +// manual->y * 1000, +// manual->z * 1000, +// manual->r * 1000, +// 0); +// } +// } +// }; + + +// class MavlinkStreamOpticalFlow : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "OPTICAL_FLOW"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamOpticalFlow(); +// } + +// private: +// MavlinkOrbSubscription *flow_sub; +// struct optical_flow_s *flow; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); +// flow = (struct optical_flow_s *)flow_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (flow_sub->update(t)) { +// mavlink_msg_optical_flow_send(_channel, +// flow->timestamp, +// flow->sensor_id, +// flow->flow_raw_x, flow->flow_raw_y, +// flow->flow_comp_x_m, flow->flow_comp_y_m, +// flow->quality, +// flow->ground_distance_m); +// } +// } +// }; + +// class MavlinkStreamAttitudeControls : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "ATTITUDE_CONTROLS"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamAttitudeControls(); +// } + +// private: +// MavlinkOrbSubscription *att_ctrl_sub; +// struct actuator_controls_s *att_ctrl; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); +// att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (att_ctrl_sub->update(t)) { +// /* send, add spaces so that string buffer is at least 10 chars long */ +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl->timestamp / 1000, +// "rll ctrl ", +// att_ctrl->control[0]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl->timestamp / 1000, +// "ptch ctrl ", +// att_ctrl->control[1]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl->timestamp / 1000, +// "yaw ctrl ", +// att_ctrl->control[2]); +// mavlink_msg_named_value_float_send(_channel, +// att_ctrl->timestamp / 1000, +// "thr ctrl ", +// att_ctrl->control[3]); +// } +// } +// }; + +// class MavlinkStreamNamedValueFloat : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "NAMED_VALUE_FLOAT"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamNamedValueFloat(); +// } + +// private: +// MavlinkOrbSubscription *debug_sub; +// struct debug_key_value_s *debug; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); +// debug = (struct debug_key_value_s *)debug_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (debug_sub->update(t)) { +// /* enforce null termination */ +// debug->key[sizeof(debug->key) - 1] = '\0'; + +// mavlink_msg_named_value_float_send(_channel, +// debug->timestamp_ms, +// debug->key, +// debug->value); +// } +// } +// }; + +// class MavlinkStreamCameraCapture : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "CAMERA_CAPTURE"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamCameraCapture(); +// } + +// private: +// MavlinkOrbSubscription *status_sub; +// struct vehicle_status_s *status; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); +// status = (struct vehicle_status_s *)status_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// (void)status_sub->update(t); + +// if (status->arming_state == ARMING_STATE_ARMED +// || status->arming_state == ARMING_STATE_ARMED_ERROR) { + +// /* send camera capture on */ +// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + +// } else { +// /* send camera capture off */ +// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); +// } +// } +// }; + +// class MavlinkStreamDistanceSensor : public MavlinkStream +// { +// public: +// const char *get_name() +// { +// return "DISTANCE_SENSOR"; +// } + +// MavlinkStream *new_instance() +// { +// return new MavlinkStreamDistanceSensor(); +// } + +// private: +// MavlinkOrbSubscription *range_sub; +// struct range_finder_report *range; + +// protected: +// void subscribe(Mavlink *mavlink) +// { +// range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); +// range = (struct range_finder_report *)range_sub->get_data(); +// } + +// void send(const hrt_abstime t) +// { +// if (range_sub->update(t)) { + +// uint8_t type; + +// switch (range->type) { +// case RANGE_FINDER_TYPE_LASER: +// type = MAV_DISTANCE_SENSOR_LASER; +// break; +// } + +// uint8_t id = 0; +// uint8_t orientation = 0; +// uint8_t covariance = 20; + +// mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, +// range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); +// } +// } +// }; + +StreamListItem *streams_list[] = { + new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), + new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static), + new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static), + new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static), + new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), + new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), + new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), + // new MavlinkStreamGlobalPositionInt(), + // new MavlinkStreamLocalPositionNED(), + // new MavlinkStreamGPSGlobalOrigin(), + // new MavlinkStreamServoOutputRaw(0), + // new MavlinkStreamServoOutputRaw(1), + // new MavlinkStreamServoOutputRaw(2), + // new MavlinkStreamServoOutputRaw(3), + // new MavlinkStreamHILControls(), + // new MavlinkStreamGlobalPositionSetpointInt(), + // new MavlinkStreamLocalPositionSetpoint(), + // new MavlinkStreamRollPitchYawThrustSetpoint(), + // new MavlinkStreamRollPitchYawRatesThrustSetpoint(), + // new MavlinkStreamRCChannelsRaw(), + // new MavlinkStreamManualControl(), + // new MavlinkStreamOpticalFlow(), + // new MavlinkStreamAttitudeControls(), + // new MavlinkStreamNamedValueFloat(), + // new MavlinkStreamCameraCapture(), + // new MavlinkStreamDistanceSensor(), + // new MavlinkStreamViconPositionEstimate(), nullptr }; diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index b8823263a8..ee64d0e424 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -43,6 +43,19 @@ #include "mavlink_stream.h" -extern MavlinkStream *streams_list[]; +class StreamListItem { + +public: + MavlinkStream* (*new_instance)(); + const char* (*get_name)(); + + StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) : + new_instance(inst), + get_name(name) {}; + + ~StreamListItem() {}; +}; + +extern StreamListItem *streams_list[]; #endif /* MAVLINK_MESSAGES_H_ */ diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index d432edd2b4..0a23fb01e5 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -53,30 +53,21 @@ MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _last_check(0), next(nullptr) { - _data = malloc(topic->o_size); - memset(_data, 0, topic->o_size); } MavlinkOrbSubscription::~MavlinkOrbSubscription() { close(_fd); - free(_data); } -const orb_id_t -MavlinkOrbSubscription::get_topic() +orb_id_t +MavlinkOrbSubscription::get_topic() const { return _topic; } -void * -MavlinkOrbSubscription::get_data() -{ - return _data; -} - bool -MavlinkOrbSubscription::update(const hrt_abstime t) +MavlinkOrbSubscription::update(const hrt_abstime t, void* data) { if (_last_check == t) { /* already checked right now, return result of the check */ @@ -86,8 +77,8 @@ MavlinkOrbSubscription::update(const hrt_abstime t) _last_check = t; orb_check(_fd, &_updated); - if (_updated) { - orb_copy(_topic, _fd, _data); + if (_updated && data) { + orb_copy(_topic, _fd, data); _published = true; return true; } diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 5c6543e813..abd4031bdc 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -48,12 +48,12 @@ class MavlinkOrbSubscription { public: - MavlinkOrbSubscription *next; /*< pointer to next subscription in list */ + MavlinkOrbSubscription *next; ///< pointer to next subscription in list MavlinkOrbSubscription(const orb_id_t topic); ~MavlinkOrbSubscription(); - bool update(const hrt_abstime t); + bool update(const hrt_abstime t, void* data); /** * Check if the topic has been published. @@ -62,16 +62,14 @@ public: * @return true if the topic has been published at least once. */ bool is_published(); - void *get_data(); - const orb_id_t get_topic(); + orb_id_t get_topic() const; private: - const orb_id_t _topic; /*< topic metadata */ - int _fd; /*< subscription handle */ - bool _published; /*< topic was ever published */ - void *_data; /*< pointer to data buffer */ - hrt_abstime _last_check; /*< time of last check */ - bool _updated; /*< updated on last check */ + const orb_id_t _topic; ///< topic metadata + int _fd; ///< subscription handle + bool _published; ///< topic was ever published + hrt_abstime _last_check; ///< time of last check + bool _updated; ///< updated on last check }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 72b9ee83a3..666b3a8cd1 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -950,7 +950,6 @@ MavlinkReceiver::receive_start(Mavlink *parent) (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2900); - pthread_t thread; pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index def40d9adc..eb881edd75 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -50,14 +50,6 @@ class MavlinkStream; class MavlinkStream { -private: - hrt_abstime _last_sent; - -protected: - mavlink_channel_t _channel; - unsigned int _interval; - - virtual void send(const hrt_abstime t) = 0; public: MavlinkStream *next; @@ -67,9 +59,19 @@ public: void set_interval(const unsigned int interval); void set_channel(mavlink_channel_t channel); int update(const hrt_abstime t); - virtual MavlinkStream *new_instance() = 0; + static MavlinkStream *new_instance(); + static const char *get_name_static(); virtual void subscribe(Mavlink *mavlink) = 0; - virtual const char *get_name() = 0; + virtual const char *get_name() const = 0; + +protected: + mavlink_channel_t _channel; + unsigned int _interval; + + virtual void send(const hrt_abstime t) = 0; + +private: + hrt_abstime _last_sent; }; From 7926a1f8aed851d8ac22538279c11660dc645f20 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 May 2014 18:22:03 +0200 Subject: [PATCH 07/67] Converted streams to new API, saving a bunch of RAM --- src/modules/mavlink/mavlink_messages.cpp | 378 +++++++++++++---------- 1 file changed, 210 insertions(+), 168 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4bb827116d..3a82f1f050 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -596,55 +596,57 @@ protected: }; -// class MavlinkStreamGlobalPositionInt : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "GLOBAL_POSITION_INT"; -// } +class MavlinkStreamGlobalPositionInt : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamGlobalPositionInt::get_name_static(); + } -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamGlobalPositionInt(); -// } + static const char *get_name_static() + { + return "GLOBAL_POSITION_INT"; + } -// private: -// MavlinkOrbSubscription *pos_sub; -// struct vehicle_global_position_s *pos; + static MavlinkStream *new_instance() + { + return new MavlinkStreamGlobalPositionInt(); + } -// MavlinkOrbSubscription *home_sub; -// struct home_position_s *home; +private: + MavlinkOrbSubscription *pos_sub; + MavlinkOrbSubscription *home_sub; -// protected: -// void subscribe(Mavlink *mavlink) -// { -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); -// pos = (struct vehicle_global_position_s *)pos_sub->get_data(); +protected: + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); + } -// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); -// home = (struct home_position_s *)home_sub->get_data(); -// } + void send(const hrt_abstime t) + { + struct vehicle_global_position_s pos; + struct home_position_s home; -// void send(const hrt_abstime t) -// { -// bool updated = pos_sub->update(t); -// updated |= home_sub->update(t); + bool updated = pos_sub->update(t, &pos); + updated |= home_sub->update(t, &home); -// if (updated) { -// mavlink_msg_global_position_int_send(_channel, -// pos->timestamp / 1000, -// pos->lat * 1e7, -// pos->lon * 1e7, -// pos->alt * 1000.0f, -// (pos->alt - home->alt) * 1000.0f, -// pos->vel_n * 100.0f, -// pos->vel_e * 100.0f, -// pos->vel_d * 100.0f, -// _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); -// } -// } -// }; + if (updated) { + mavlink_msg_global_position_int_send(_channel, + pos.timestamp / 1000, + pos.lat * 1e7, + pos.lon * 1e7, + pos.alt * 1000.0f, + (pos.alt - home.alt) * 1000.0f, + pos.vel_n * 100.0f, + pos.vel_e * 100.0f, + pos.vel_d * 100.0f, + _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f); + } + } +}; // class MavlinkStreamLocalPositionNED : public MavlinkStream @@ -675,13 +677,13 @@ protected: // { // if (pos_sub->update(t)) { // mavlink_msg_local_position_ned_send(_channel, -// pos->timestamp / 1000, -// pos->x, -// pos->y, -// pos->z, -// pos->vx, -// pos->vy, -// pos->vz); +// pos.timestamp / 1000, +// pos.x, +// pos.y, +// pos.z, +// pos.vx, +// pos.vy, +// pos.vz); // } // } // }; @@ -716,13 +718,13 @@ protected: // { // if (pos_sub->update(t)) { // mavlink_msg_vicon_position_estimate_send(_channel, -// pos->timestamp / 1000, -// pos->x, -// pos->y, -// pos->z, -// pos->roll, -// pos->pitch, -// pos->yaw); +// pos.timestamp / 1000, +// pos.x, +// pos.y, +// pos.z, +// pos.roll, +// pos.pitch, +// pos.yaw); // } // } // }; @@ -1106,131 +1108,171 @@ protected: // }; -// class MavlinkStreamRCChannelsRaw : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "RC_CHANNELS_RAW"; -// } +class MavlinkStreamRCChannelsRaw : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRCChannelsRaw::get_name_static(); + } -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamRCChannelsRaw(); -// } + static const char *get_name_static() + { + return "RC_CHANNELS_RAW"; + } -// private: -// MavlinkOrbSubscription *rc_sub; -// struct rc_input_values *rc; + static MavlinkStream *new_instance() + { + return new MavlinkStreamRCChannelsRaw(); + } -// protected: -// void subscribe(Mavlink *mavlink) -// { -// rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); -// rc = (struct rc_input_values *)rc_sub->get_data(); -// } +private: + MavlinkOrbSubscription *rc_sub; -// void send(const hrt_abstime t) -// { -// if (rc_sub->update(t)) { -// const unsigned port_width = 8; +protected: + void subscribe(Mavlink *mavlink) + { + rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); + } -// for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { -// /* Channels are sent in MAVLink main loop at a fixed interval */ -// mavlink_msg_rc_channels_raw_send(_channel, -// rc->timestamp_publication / 1000, -// i, -// (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, -// (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, -// (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, -// (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, -// (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, -// (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, -// (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, -// (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, -// rc->rssi); -// } -// } -// } -// }; + void send(const hrt_abstime t) + { + struct rc_input_values rc; + + if (rc_sub->update(t, &rc)) { + const unsigned port_width = 8; + + // Deprecated message (but still needed for compatibility!) + for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) { + /* Channels are sent in MAVLink main loop at a fixed interval */ + mavlink_msg_rc_channels_raw_send(_channel, + rc.timestamp_publication / 1000, + i, + (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX, + (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX, + (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX, + (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX, + (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX, + (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX, + (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX, + (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX, + rc.rssi); + } + + // New message + mavlink_msg_rc_channels_send(_channel, + rc.timestamp_publication / 1000, + ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX), + ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX), + ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX), + ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX), + ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX), + ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX), + ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX), + ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX), + ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX), + ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX), + ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX), + ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX), + ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX), + ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX), + ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX), + ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX), + ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX), + ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX), + rc.channel_count, + rc.rssi); + } + } +}; -// class MavlinkStreamManualControl : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "MANUAL_CONTROL"; -// } +class MavlinkStreamManualControl : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamManualControl::get_name_static(); + } -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamManualControl(); -// } + static const char *get_name_static() + { + return "MANUAL_CONTROL"; + } -// private: -// MavlinkOrbSubscription *manual_sub; -// struct manual_control_setpoint_s *manual; + static MavlinkStream *new_instance() + { + return new MavlinkStreamManualControl(); + } -// protected: -// void subscribe(Mavlink *mavlink) -// { -// manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); -// manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); -// } +private: + MavlinkOrbSubscription *manual_sub; -// void send(const hrt_abstime t) -// { -// if (manual_sub->update(t)) { -// mavlink_msg_manual_control_send(_channel, -// mavlink_system.sysid, -// manual->x * 1000, -// manual->y * 1000, -// manual->z * 1000, -// manual->r * 1000, -// 0); -// } -// } -// }; +protected: + void subscribe(Mavlink *mavlink) + { + manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); + } + + void send(const hrt_abstime t) + { + struct manual_control_setpoint_s manual; + + if (manual_sub->update(t, &manual)) { + mavlink_msg_manual_control_send(_channel, + mavlink_system.sysid, + manual.x * 1000, + manual.y * 1000, + manual.z * 1000, + manual.r * 1000, + 0); + } + } +}; -// class MavlinkStreamOpticalFlow : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "OPTICAL_FLOW"; -// } +class MavlinkStreamOpticalFlow : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamOpticalFlow::get_name_static(); + } -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamOpticalFlow(); -// } + static const char *get_name_static() + { + return "OPTICAL_FLOW"; + } -// private: -// MavlinkOrbSubscription *flow_sub; -// struct optical_flow_s *flow; + static MavlinkStream *new_instance() + { + return new MavlinkStreamOpticalFlow(); + } -// protected: -// void subscribe(Mavlink *mavlink) -// { -// flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); -// flow = (struct optical_flow_s *)flow_sub->get_data(); -// } +private: + MavlinkOrbSubscription *flow_sub; -// void send(const hrt_abstime t) -// { -// if (flow_sub->update(t)) { -// mavlink_msg_optical_flow_send(_channel, -// flow->timestamp, -// flow->sensor_id, -// flow->flow_raw_x, flow->flow_raw_y, -// flow->flow_comp_x_m, flow->flow_comp_y_m, -// flow->quality, -// flow->ground_distance_m); -// } -// } -// }; +protected: + void subscribe(Mavlink *mavlink) + { + flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); + } + + void send(const hrt_abstime t) + { + struct optical_flow_s flow; + + if (flow_sub->update(t, &flow)) { + mavlink_msg_optical_flow_send(_channel, + flow.timestamp, + flow.sensor_id, + flow.flow_raw_x, flow.flow_raw_y, + flow.flow_comp_x_m, flow.flow_comp_y_m, + flow.quality, + flow.ground_distance_m); + } + } +}; // class MavlinkStreamAttitudeControls : public MavlinkStream // { @@ -1413,7 +1455,7 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), - // new MavlinkStreamGlobalPositionInt(), + new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), // new MavlinkStreamLocalPositionNED(), // new MavlinkStreamGPSGlobalOrigin(), // new MavlinkStreamServoOutputRaw(0), @@ -1425,9 +1467,9 @@ StreamListItem *streams_list[] = { // new MavlinkStreamLocalPositionSetpoint(), // new MavlinkStreamRollPitchYawThrustSetpoint(), // new MavlinkStreamRollPitchYawRatesThrustSetpoint(), - // new MavlinkStreamRCChannelsRaw(), - // new MavlinkStreamManualControl(), - // new MavlinkStreamOpticalFlow(), + new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), + new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), + new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), // new MavlinkStreamAttitudeControls(), // new MavlinkStreamNamedValueFloat(), // new MavlinkStreamCameraCapture(), From 9b2370e3873d65b65fd2dc2257beed50df6ac892 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 17:10:11 +0200 Subject: [PATCH 08/67] mavlink app: Fix compile error --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 340b20e1b2..e9932aac37 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1483,9 +1483,9 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) void Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) { - uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); + uint16_t len = mavlink_msg_to_send_buffer(buf, msg); mavlink_send_uart_bytes(_channel, buf, len); } From 7bfcaafc1631cab603191777e2e63f69755e334d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 May 2014 17:10:33 +0200 Subject: [PATCH 09/67] mavlink app: Finish porting all messages to new scheme --- src/modules/mavlink/mavlink_messages.cpp | 1308 ++++++++++++---------- 1 file changed, 690 insertions(+), 618 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 3a82f1f050..748c1f69a3 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -649,463 +649,514 @@ protected: }; -// class MavlinkStreamLocalPositionNED : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "LOCAL_POSITION_NED"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamLocalPositionNED(); -// } - -// private: -// MavlinkOrbSubscription *pos_sub; -// struct vehicle_local_position_s *pos; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); -// pos = (struct vehicle_local_position_s *)pos_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (pos_sub->update(t)) { -// mavlink_msg_local_position_ned_send(_channel, -// pos.timestamp / 1000, -// pos.x, -// pos.y, -// pos.z, -// pos.vx, -// pos.vy, -// pos.vz); -// } -// } -// }; - - - -// class MavlinkStreamViconPositionEstimate : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "VICON_POSITION_ESTIMATE"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamViconPositionEstimate(); -// } - -// private: -// MavlinkOrbSubscription *pos_sub; -// struct vehicle_vicon_position_s *pos; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); -// pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (pos_sub->update(t)) { -// mavlink_msg_vicon_position_estimate_send(_channel, -// pos.timestamp / 1000, -// pos.x, -// pos.y, -// pos.z, -// pos.roll, -// pos.pitch, -// pos.yaw); -// } -// } -// }; - - -// class MavlinkStreamGPSGlobalOrigin : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "GPS_GLOBAL_ORIGIN"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamGPSGlobalOrigin(); -// } - -// private: -// MavlinkOrbSubscription *home_sub; -// struct home_position_s *home; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); -// home = (struct home_position_s *)home_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { - -// /* we're sending the GPS home periodically to ensure the -// * the GCS does pick it up at one point */ -// if (home_sub->is_published()) { -// home_sub->update(t); - -// mavlink_msg_gps_global_origin_send(_channel, -// (int32_t)(home->lat * 1e7), -// (int32_t)(home->lon * 1e7), -// (int32_t)(home->alt) * 1000.0f); -// } -// } -// }; - - -// class MavlinkStreamServoOutputRaw : public MavlinkStream -// { -// public: -// MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n) -// { -// sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n); -// } - -// const char *get_name() -// { -// return _name; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamServoOutputRaw(_n); -// } - -// private: -// MavlinkOrbSubscription *act_sub; -// struct actuator_outputs_s *act; - -// char _name[20]; -// unsigned int _n; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// orb_id_t act_topics[] = { -// ORB_ID(actuator_outputs_0), -// ORB_ID(actuator_outputs_1), -// ORB_ID(actuator_outputs_2), -// ORB_ID(actuator_outputs_3) -// }; - -// act_sub = mavlink->add_orb_subscription(act_topics[_n]); -// act = (struct actuator_outputs_s *)act_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (act_sub->update(t)) { -// mavlink_msg_servo_output_raw_send(_channel, -// act->timestamp / 1000, -// _n, -// act->output[0], -// act->output[1], -// act->output[2], -// act->output[3], -// act->output[4], -// act->output[5], -// act->output[6], -// act->output[7]); -// } -// } -// }; - - -// class MavlinkStreamHILControls : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "HIL_CONTROLS"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamHILControls(); -// } - -// private: -// MavlinkOrbSubscription *status_sub; -// struct vehicle_status_s *status; - -// MavlinkOrbSubscription *pos_sp_triplet_sub; -// struct position_setpoint_triplet_s *pos_sp_triplet; - -// MavlinkOrbSubscription *act_sub; -// struct actuator_outputs_s *act; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); -// status = (struct vehicle_status_s *)status_sub->get_data(); - -// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); -// pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); - -// act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); -// act = (struct actuator_outputs_s *)act_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// bool updated = act_sub->update(t); -// (void)pos_sp_triplet_sub->update(t); -// (void)status_sub->update(t); - -// if (updated && (status->arming_state == ARMING_STATE_ARMED)) { -// /* translate the current syste state to mavlink state and mode */ -// uint8_t mavlink_state; -// uint8_t mavlink_base_mode; -// uint32_t mavlink_custom_mode; -// get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - -// if (mavlink_system.type == MAV_TYPE_QUADROTOR || -// mavlink_system.type == MAV_TYPE_HEXAROTOR || -// mavlink_system.type == MAV_TYPE_OCTOROTOR) { -// /* set number of valid outputs depending on vehicle type */ -// unsigned n; - -// switch (mavlink_system.type) { -// case MAV_TYPE_QUADROTOR: -// n = 4; -// break; - -// case MAV_TYPE_HEXAROTOR: -// n = 6; -// break; - -// default: -// n = 8; -// break; -// } - -// /* scale / assign outputs depending on system type */ -// float out[8]; - -// for (unsigned i = 0; i < 8; i++) { -// if (i < n) { -// if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { -// /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ -// out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); - -// } else { -// /* send 0 when disarmed */ -// out[i] = 0.0f; -// } - -// } else { -// out[i] = -1.0f; -// } -// } - -// mavlink_msg_hil_controls_send(_channel, -// hrt_absolute_time(), -// out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], -// mavlink_base_mode, -// 0); -// } else { - -// /* fixed wing: scale all channels except throttle -1 .. 1 -// * because we know that we set the mixers up this way -// */ - -// float out[8]; - -// const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; - -// for (unsigned i = 0; i < 8; i++) { -// if (i != 3) { -// /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ -// out[i] = (act->output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); - -// } else { - -// /* scale fake PWM out 900..2100 us to 0..1 for throttle */ -// out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); -// } - -// } - -// mavlink_msg_hil_controls_send(_channel, -// hrt_absolute_time(), -// out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], -// mavlink_base_mode, -// 0); -// } -// } -// } -// }; - - -// class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "GLOBAL_POSITION_SETPOINT_INT"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamGlobalPositionSetpointInt(); -// } - -// private: -// MavlinkOrbSubscription *pos_sp_triplet_sub; -// struct position_setpoint_triplet_s *pos_sp_triplet; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); -// pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (pos_sp_triplet_sub->update(t)) { -// mavlink_msg_global_position_setpoint_int_send(_channel, -// MAV_FRAME_GLOBAL, -// (int32_t)(pos_sp_triplet->current.lat * 1e7), -// (int32_t)(pos_sp_triplet->current.lon * 1e7), -// (int32_t)(pos_sp_triplet->current.alt * 1000), -// (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); -// } -// } -// }; - - -// class MavlinkStreamLocalPositionSetpoint : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "LOCAL_POSITION_SETPOINT"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamLocalPositionSetpoint(); -// } - -// private: -// MavlinkOrbSubscription *pos_sp_sub; -// struct vehicle_local_position_setpoint_s *pos_sp; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); -// pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (pos_sp_sub->update(t)) { -// mavlink_msg_local_position_setpoint_send(_channel, -// MAV_FRAME_LOCAL_NED, -// pos_sp->x, -// pos_sp->y, -// pos_sp->z, -// pos_sp->yaw); -// } -// } -// }; - - -// class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "ROLL_PITCH_YAW_THRUST_SETPOINT"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamRollPitchYawThrustSetpoint(); -// } - -// private: -// MavlinkOrbSubscription *att_sp_sub; -// struct vehicle_attitude_setpoint_s *att_sp; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); -// att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (att_sp_sub->update(t)) { -// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, -// att_sp->timestamp / 1000, -// att_sp->roll_body, -// att_sp->pitch_body, -// att_sp->yaw_body, -// att_sp->thrust); -// } -// } -// }; - - -// class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; -// } - -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); -// } - -// private: -// MavlinkOrbSubscription *att_rates_sp_sub; -// struct vehicle_rates_setpoint_s *att_rates_sp; - -// protected: -// void subscribe(Mavlink *mavlink) -// { -// att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); -// att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); -// } - -// void send(const hrt_abstime t) -// { -// if (att_rates_sp_sub->update(t)) { -// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, -// att_rates_sp->timestamp / 1000, -// att_rates_sp->roll, -// att_rates_sp->pitch, -// att_rates_sp->yaw, -// att_rates_sp->thrust); -// } -// } -// }; +class MavlinkStreamLocalPositionNED : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamLocalPositionNED::get_name_static(); + } + + static const char *get_name_static() + { + return "LOCAL_POSITION_NED"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamLocalPositionNED(); + } + +private: + MavlinkOrbSubscription *pos_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); + } + + void send(const hrt_abstime t) + { + struct vehicle_local_position_s pos; + + if (pos_sub->update(t, &pos)) { + mavlink_msg_local_position_ned_send(_channel, + pos.timestamp / 1000, + pos.x, + pos.y, + pos.z, + pos.vx, + pos.vy, + pos.vz); + } + } +}; + + + +class MavlinkStreamViconPositionEstimate : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamViconPositionEstimate::get_name_static(); + } + + static const char *get_name_static() + { + return "VICON_POSITION_ESTIMATE"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamViconPositionEstimate(); + } + +private: + MavlinkOrbSubscription *pos_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); + } + + void send(const hrt_abstime t) + { + struct vehicle_vicon_position_s pos; + + if (pos_sub->update(t, &pos)) { + mavlink_msg_vicon_position_estimate_send(_channel, + pos.timestamp / 1000, + pos.x, + pos.y, + pos.z, + pos.roll, + pos.pitch, + pos.yaw); + } + } +}; + + +class MavlinkStreamGPSGlobalOrigin : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamGPSGlobalOrigin::get_name_static(); + } + + static const char *get_name_static() + { + return "GPS_GLOBAL_ORIGIN"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamGPSGlobalOrigin(); + } + +private: + MavlinkOrbSubscription *home_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); + } + + void send(const hrt_abstime t) + { + /* we're sending the GPS home periodically to ensure the + * the GCS does pick it up at one point */ + if (home_sub->is_published()) { + struct home_position_s home; + home_sub->update(t, &home); + + mavlink_msg_gps_global_origin_send(_channel, + (int32_t)(home.lat * 1e7), + (int32_t)(home.lon * 1e7), + (int32_t)(home.alt) * 1000.0f); + } + } +}; + + +class MavlinkStreamServoOutputRaw : public MavlinkStream +{ +public: + MavlinkStreamServoOutputRaw() : MavlinkStream() + { + _instance = _n++; + } + + const char *get_name() const + { + return get_name_static_int(_instance); + } + + static const char *get_name_static() + { + return get_name_static_int(_n); + } + + static const char *get_name_static_int(unsigned n) + { + switch (n) { + case 0: + return "SERVO_OUTPUT_RAW_0"; + + case 1: + return "SERVO_OUTPUT_RAW_1"; + + case 2: + return "SERVO_OUTPUT_RAW_2"; + + case 3: + return "SERVO_OUTPUT_RAW_3"; + } + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamServoOutputRaw(); + } + + static unsigned _n; + +private: + MavlinkOrbSubscription *_act_sub; + + unsigned _instance; + +protected: + void subscribe(Mavlink *mavlink) + { + orb_id_t act_topics[] = { + ORB_ID(actuator_outputs_0), + ORB_ID(actuator_outputs_1), + ORB_ID(actuator_outputs_2), + ORB_ID(actuator_outputs_3) + }; + + _act_sub = mavlink->add_orb_subscription(act_topics[_instance]); + } + + void send(const hrt_abstime t) + { + struct actuator_outputs_s act; + + if (_act_sub->update(t, &act)) { + mavlink_msg_servo_output_raw_send(_channel, + act.timestamp / 1000, + _instance, + act.output[0], + act.output[1], + act.output[2], + act.output[3], + act.output[4], + act.output[5], + act.output[6], + act.output[7]); + } + } +}; + + +class MavlinkStreamHILControls : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamHILControls::get_name_static(); + } + + static const char *get_name_static() + { + return "HIL_CONTROLS"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamHILControls(); + } + +private: + MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *pos_sp_triplet_sub; + MavlinkOrbSubscription *act_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); + act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); + } + + void send(const hrt_abstime t) + { + struct vehicle_status_s status; + struct position_setpoint_triplet_s pos_sp_triplet; + struct actuator_outputs_s act; + + bool updated = act_sub->update(t, &act); + (void)pos_sp_triplet_sub->update(t, &pos_sp_triplet); + (void)status_sub->update(t, &status); + + if (updated && (status.arming_state == ARMING_STATE_ARMED)) { + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state; + uint8_t mavlink_base_mode; + uint32_t mavlink_custom_mode; + get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + if (mavlink_system.type == MAV_TYPE_QUADROTOR || + mavlink_system.type == MAV_TYPE_HEXAROTOR || + mavlink_system.type == MAV_TYPE_OCTOROTOR) { + /* set number of valid outputs depending on vehicle type */ + unsigned n; + + switch (mavlink_system.type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; + + case MAV_TYPE_HEXAROTOR: + n = 6; + break; + + default: + n = 8; + break; + } + + /* scale / assign outputs depending on system type */ + float out[8]; + + for (unsigned i = 0; i < 8; i++) { + if (i < n) { + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ + out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + + } else { + /* send 0 when disarmed */ + out[i] = 0.0f; + } + + } else { + out[i] = -1.0f; + } + } + + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); + } else { + + /* fixed wing: scale all channels except throttle -1 .. 1 + * because we know that we set the mixers up this way + */ + + float out[8]; + + const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + + for (unsigned i = 0; i < 8; i++) { + if (i != 3) { + /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ + out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + + } else { + + /* scale fake PWM out 900..2100 us to 0..1 for throttle */ + out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + } + + } + + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); + } + } + } +}; + + +class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamGlobalPositionSetpointInt::get_name_static(); + } + + static const char *get_name_static() + { + return "GLOBAL_POSITION_SETPOINT_INT"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamGlobalPositionSetpointInt(); + } + +private: + MavlinkOrbSubscription *pos_sp_triplet_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); + } + + void send(const hrt_abstime t) + { + struct position_setpoint_triplet_s pos_sp_triplet; + if (pos_sp_triplet_sub->update(t, &pos_sp_triplet)) { + mavlink_msg_global_position_setpoint_int_send(_channel, + MAV_FRAME_GLOBAL, + (int32_t)(pos_sp_triplet.current.lat * 1e7), + (int32_t)(pos_sp_triplet.current.lon * 1e7), + (int32_t)(pos_sp_triplet.current.alt * 1000), + (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f)); + } + } +}; + + +class MavlinkStreamLocalPositionSetpoint : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamLocalPositionSetpoint::get_name_static(); + } + + static const char *get_name_static() + { + return "LOCAL_POSITION_SETPOINT"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamLocalPositionSetpoint(); + } + +private: + MavlinkOrbSubscription *pos_sp_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); + } + + void send(const hrt_abstime t) + { + struct vehicle_local_position_setpoint_s pos_sp; + if (pos_sp_sub->update(t, &pos_sp)) { + mavlink_msg_local_position_setpoint_send(_channel, + MAV_FRAME_LOCAL_NED, + pos_sp.x, + pos_sp.y, + pos_sp.z, + pos_sp.yaw); + } + } +}; + + +class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static(); + } + + static const char *get_name_static() + { + return "ROLL_PITCH_YAW_THRUST_SETPOINT"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamRollPitchYawThrustSetpoint(); + } + +private: + MavlinkOrbSubscription *att_sp_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); + } + + void send(const hrt_abstime t) + { + struct vehicle_attitude_setpoint_s att_sp; + if (att_sp_sub->update(t, &att_sp)) { + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, + att_sp.timestamp / 1000, + att_sp.roll_body, + att_sp.pitch_body, + att_sp.yaw_body, + att_sp.thrust); + } + } +}; + + +class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static(); + } + + static const char *get_name_static() + { + return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; + } + + static MavlinkStream *new_instance() + { + return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); + } + +private: + MavlinkOrbSubscription *att_rates_sp_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); + } + + void send(const hrt_abstime t) + { + struct vehicle_rates_setpoint_s att_rates_sp; + if (att_rates_sp_sub->update(t, &att_rates_sp)) { + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, + att_rates_sp.timestamp / 1000, + att_rates_sp.roll, + att_rates_sp.pitch, + att_rates_sp.yaw, + att_rates_sp.thrust); + } + } +}; class MavlinkStreamRCChannelsRaw : public MavlinkStream @@ -1274,178 +1325,199 @@ protected: } }; -// class MavlinkStreamAttitudeControls : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "ATTITUDE_CONTROLS"; -// } +class MavlinkStreamAttitudeControls : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamAttitudeControls::get_name_static(); + } -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamAttitudeControls(); -// } + static const char *get_name_static() + { + return "ATTITUDE_CONTROLS"; + } -// private: -// MavlinkOrbSubscription *att_ctrl_sub; -// struct actuator_controls_s *att_ctrl; + static MavlinkStream *new_instance() + { + return new MavlinkStreamAttitudeControls(); + } -// protected: -// void subscribe(Mavlink *mavlink) -// { -// att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); -// att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data(); -// } +private: + MavlinkOrbSubscription *att_ctrl_sub; -// void send(const hrt_abstime t) -// { -// if (att_ctrl_sub->update(t)) { -// /* send, add spaces so that string buffer is at least 10 chars long */ -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl->timestamp / 1000, -// "rll ctrl ", -// att_ctrl->control[0]); -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl->timestamp / 1000, -// "ptch ctrl ", -// att_ctrl->control[1]); -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl->timestamp / 1000, -// "yaw ctrl ", -// att_ctrl->control[2]); -// mavlink_msg_named_value_float_send(_channel, -// att_ctrl->timestamp / 1000, -// "thr ctrl ", -// att_ctrl->control[3]); -// } -// } -// }; +protected: + void subscribe(Mavlink *mavlink) + { + att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + } -// class MavlinkStreamNamedValueFloat : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "NAMED_VALUE_FLOAT"; -// } + void send(const hrt_abstime t) + { + struct actuator_controls_s att_ctrl; -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamNamedValueFloat(); -// } + if (att_ctrl_sub->update(t, &att_ctrl)) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(_channel, + att_ctrl.timestamp / 1000, + "rll ctrl ", + att_ctrl.control[0]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl.timestamp / 1000, + "ptch ctrl ", + att_ctrl.control[1]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl.timestamp / 1000, + "yaw ctrl ", + att_ctrl.control[2]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl.timestamp / 1000, + "thr ctrl ", + att_ctrl.control[3]); + } + } +}; -// private: -// MavlinkOrbSubscription *debug_sub; -// struct debug_key_value_s *debug; +class MavlinkStreamNamedValueFloat : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamNamedValueFloat::get_name_static(); + } -// protected: -// void subscribe(Mavlink *mavlink) -// { -// debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); -// debug = (struct debug_key_value_s *)debug_sub->get_data(); -// } + static const char *get_name_static() + { + return "NAMED_VALUE_FLOAT"; + } -// void send(const hrt_abstime t) -// { -// if (debug_sub->update(t)) { -// /* enforce null termination */ -// debug->key[sizeof(debug->key) - 1] = '\0'; + static MavlinkStream *new_instance() + { + return new MavlinkStreamNamedValueFloat(); + } -// mavlink_msg_named_value_float_send(_channel, -// debug->timestamp_ms, -// debug->key, -// debug->value); -// } -// } -// }; +private: + MavlinkOrbSubscription *debug_sub; -// class MavlinkStreamCameraCapture : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "CAMERA_CAPTURE"; -// } +protected: + void subscribe(Mavlink *mavlink) + { + debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); + } -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamCameraCapture(); -// } + void send(const hrt_abstime t) + { + struct debug_key_value_s debug; -// private: -// MavlinkOrbSubscription *status_sub; -// struct vehicle_status_s *status; + if (debug_sub->update(t, &debug)) { + /* enforce null termination */ + debug.key[sizeof(debug.key) - 1] = '\0'; -// protected: -// void subscribe(Mavlink *mavlink) -// { -// status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); -// status = (struct vehicle_status_s *)status_sub->get_data(); -// } + mavlink_msg_named_value_float_send(_channel, + debug.timestamp_ms, + debug.key, + debug.value); + } + } +}; -// void send(const hrt_abstime t) -// { -// (void)status_sub->update(t); +class MavlinkStreamCameraCapture : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamCameraCapture::get_name_static(); + } -// if (status->arming_state == ARMING_STATE_ARMED -// || status->arming_state == ARMING_STATE_ARMED_ERROR) { + static const char *get_name_static() + { + return "CAMERA_CAPTURE"; + } -// /* send camera capture on */ -// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + static MavlinkStream *new_instance() + { + return new MavlinkStreamCameraCapture(); + } -// } else { -// /* send camera capture off */ -// mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); -// } -// } -// }; +private: + MavlinkOrbSubscription *status_sub; -// class MavlinkStreamDistanceSensor : public MavlinkStream -// { -// public: -// const char *get_name() -// { -// return "DISTANCE_SENSOR"; -// } +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + } -// MavlinkStream *new_instance() -// { -// return new MavlinkStreamDistanceSensor(); -// } + void send(const hrt_abstime t) + { + struct vehicle_status_s status; + (void)status_sub->update(t, &status); -// private: -// MavlinkOrbSubscription *range_sub; -// struct range_finder_report *range; + if (status.arming_state == ARMING_STATE_ARMED + || status.arming_state == ARMING_STATE_ARMED_ERROR) { -// protected: -// void subscribe(Mavlink *mavlink) -// { -// range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); -// range = (struct range_finder_report *)range_sub->get_data(); -// } + /* send camera capture on */ + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); -// void send(const hrt_abstime t) -// { -// if (range_sub->update(t)) { + } else { + /* send camera capture off */ + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + } + } +}; -// uint8_t type; +class MavlinkStreamDistanceSensor : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamDistanceSensor::get_name_static(); + } -// switch (range->type) { -// case RANGE_FINDER_TYPE_LASER: -// type = MAV_DISTANCE_SENSOR_LASER; -// break; -// } + static const char *get_name_static() + { + return "DISTANCE_SENSOR"; + } -// uint8_t id = 0; -// uint8_t orientation = 0; -// uint8_t covariance = 20; + static MavlinkStream *new_instance() + { + return new MavlinkStreamDistanceSensor(); + } -// mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, -// range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); -// } -// } -// }; +private: + MavlinkOrbSubscription *range_sub; + +protected: + void subscribe(Mavlink *mavlink) + { + range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); + } + + void send(const hrt_abstime t) + { + struct range_finder_report range; + + if (range_sub->update(t, &range)) { + + uint8_t type; + + switch (range.type) { + case RANGE_FINDER_TYPE_LASER: + type = MAV_DISTANCE_SENSOR_LASER; + break; + } + + uint8_t id = 0; + uint8_t orientation = 0; + uint8_t covariance = 20; + + mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation, + range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance); + } + } +}; + +unsigned MavlinkStreamServoOutputRaw::_n = 0; StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), @@ -1456,24 +1528,24 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), - // new MavlinkStreamLocalPositionNED(), - // new MavlinkStreamGPSGlobalOrigin(), - // new MavlinkStreamServoOutputRaw(0), - // new MavlinkStreamServoOutputRaw(1), - // new MavlinkStreamServoOutputRaw(2), - // new MavlinkStreamServoOutputRaw(3), - // new MavlinkStreamHILControls(), - // new MavlinkStreamGlobalPositionSetpointInt(), - // new MavlinkStreamLocalPositionSetpoint(), - // new MavlinkStreamRollPitchYawThrustSetpoint(), - // new MavlinkStreamRollPitchYawRatesThrustSetpoint(), + new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), + new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw::new_instance, &MavlinkStreamServoOutputRaw::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw::new_instance, &MavlinkStreamServoOutputRaw::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw::new_instance, &MavlinkStreamServoOutputRaw::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw::new_instance, &MavlinkStreamServoOutputRaw::get_name_static), + new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), + new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), + new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), + new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), + new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), - // new MavlinkStreamAttitudeControls(), - // new MavlinkStreamNamedValueFloat(), - // new MavlinkStreamCameraCapture(), - // new MavlinkStreamDistanceSensor(), - // new MavlinkStreamViconPositionEstimate(), + new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), + new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), + new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), + new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static), + new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), nullptr }; From 4ee647015a5fa4c2d8e324e75a9f2d749a1b6ca6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 May 2014 19:52:22 +0200 Subject: [PATCH 10/67] navigator: takeoff fix, always reach takeoff altitude, even if first waypoint is lower --- src/modules/navigator/navigator_main.cpp | 32 ++++++++++++------------ 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 87c893079f..8eedcc90e3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1477,27 +1477,27 @@ Navigator::check_mission_item_reached() acceptance_radius = _parameters.acceptance_radius; } - float dist = -1.0f; - float dist_xy = -1.0f; - float dist_z = -1.0f; - - /* calculate AMSL altitude for this waypoint */ - float wp_alt_amsl = _mission_item.altitude; - - if (_mission_item.altitude_is_relative) - wp_alt_amsl += _home_pos.alt; - - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, - &dist_xy, &dist_z); - if (_do_takeoff) { - if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { - /* require only altitude for takeoff */ + /* require only altitude for takeoff */ + if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) { _waypoint_position_reached = true; } } else { + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + /* calculate AMSL altitude for this waypoint */ + float wp_alt_amsl = _mission_item.altitude; + + if (_mission_item.altitude_is_relative) + wp_alt_amsl += _home_pos.alt; + + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, + (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, + &dist_xy, &dist_z); + if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; } From 95a8414895d0f93bf92b8339ad15a1b3b4d1a7f2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 May 2014 19:55:24 +0200 Subject: [PATCH 11/67] rc.mc_defaults: set default acceptance radius (NAV_ACCEPT_RAD) to 2m --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index b6b0a5b4ed..65f1e38c6b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -41,6 +41,7 @@ then param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.0 + param set NAV_ACCEPT_RAD 2.0 fi set PWM_RATE 400 From 9a49636f6a6cfa631e8a4686e2cbb2f4e25770eb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 27 May 2014 13:15:17 +0200 Subject: [PATCH 12/67] position_estimator_inav: remove acceleration from state and INAV_W_XXX_ACC parameters, more NaN checks --- .../position_estimator_inav/inertial_filter.c | 15 ++-- .../position_estimator_inav/inertial_filter.h | 2 +- .../position_estimator_inav_main.c | 68 ++++++++----------- .../position_estimator_inav_params.c | 6 -- .../position_estimator_inav_params.h | 4 -- 5 files changed, 38 insertions(+), 57 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index 2f1b3c0145..a36a4688d0 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -9,15 +9,18 @@ #include "inertial_filter.h" -void inertial_filter_predict(float dt, float x[3]) +void inertial_filter_predict(float dt, float x[2], float acc) { if (isfinite(dt)) { - x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; - x[1] += x[2] * dt; + if (!isfinite(acc)) { + acc = 0.0f; + } + x[0] += x[1] * dt + acc * dt * dt / 2.0f; + x[1] += acc * dt; } } -void inertial_filter_correct(float e, float dt, float x[3], int i, float w) +void inertial_filter_correct(float e, float dt, float x[2], int i, float w) { if (isfinite(e) && isfinite(w) && isfinite(dt)) { float ewdt = e * w * dt; @@ -25,10 +28,6 @@ void inertial_filter_correct(float e, float dt, float x[3], int i, float w) if (i == 0) { x[1] += w * ewdt; - x[2] += w * w * ewdt / 3.0; - - } else if (i == 1) { - x[2] += w * ewdt; } } } diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h index 761c17097d..cdeb4cfc6c 100644 --- a/src/modules/position_estimator_inav/inertial_filter.h +++ b/src/modules/position_estimator_inav/inertial_filter.h @@ -8,6 +8,6 @@ #include #include -void inertial_filter_predict(float dt, float x[3]); +void inertial_filter_predict(float dt, float x[3], float acc); void inertial_filter_correct(float e, float dt, float x[3], int i, float w); 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 fdc3233e03..d7503e42da 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -168,15 +168,15 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(1); } -void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float x_est_prev[3], float y_est_prev[3], float z_est_prev[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) +void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { FILE *f = fopen("/fs/microsd/inav.log", "a"); if (f) { char *s = malloc(256); - unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f] x_est_prev=[%.5f %.5f %.5f] y_est_prev=[%.5f %.5f %.5f] z_est_prev=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2], x_est_prev[0], x_est_prev[1], x_est_prev[2], y_est_prev[0], y_est_prev[1], y_est_prev[2], z_est_prev[0], z_est_prev[1], z_est_prev[2]); + unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], y_est[0], y_est[1], z_est[0], z_est[1], x_est_prev[0], x_est_prev[1], y_est_prev[0], y_est_prev[1], z_est_prev[0], z_est_prev[1]); fwrite(s, 1, n, f); - n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); + n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", acc[0], acc[1], acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); fwrite(s, 1, n, f); free(s); } @@ -195,14 +195,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[inav] started"); - float x_est[3] = { 0.0f, 0.0f, 0.0f }; - float y_est[3] = { 0.0f, 0.0f, 0.0f }; - float z_est[3] = { 0.0f, 0.0f, 0.0f }; + float x_est[2] = { 0.0f, 0.0f }; // pos, vel + float y_est[2] = { 0.0f, 0.0f }; // pos, vel + float z_est[2] = { 0.0f, 0.0f }; // pos, vel float eph = 1.0; float epv = 1.0; - float x_est_prev[3], y_est_prev[3], z_est_prev[3]; + float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); memset(z_est_prev, 0, sizeof(z_est_prev)); @@ -241,7 +241,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ - float corr_acc[] = { 0.0f, 0.0f, 0.0f }; // N E D + float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame float corr_baro = 0.0f; // D float corr_gps[3][2] = { @@ -341,8 +341,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { - baro_offset += sensor.baro_alt_meter; - baro_init_cnt++; + if (isfinite(sensor.baro_alt_meter)) { + baro_offset += sensor.baro_alt_meter; + baro_init_cnt++; + } } else { wait_baro = false; @@ -418,19 +420,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { - accel_NED[i] = 0.0f; + acc[i] = 0.0f; for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; + acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; } } - corr_acc[0] = accel_NED[0] - x_est[2]; - corr_acc[1] = accel_NED[1] - y_est[2]; - corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; + acc[2] += CONSTANTS_ONE_G; } else { - memset(corr_acc, 0, sizeof(corr_acc)); + memset(acc, 0, sizeof(acc)); } accel_timestamp = sensor.accelerometer_timestamp; @@ -628,11 +628,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* set position estimate to (0, 0, 0), use GPS velocity for XY */ x_est[0] = 0.0f; x_est[1] = gps.vel_n_m_s; - x_est[2] = accel_NED[0]; y_est[0] = 0.0f; y_est[1] = gps.vel_e_m_s; z_est[0] = 0.0f; - y_est[2] = accel_NED[1]; local_pos.ref_lat = lat; local_pos.ref_lon = lon; @@ -655,10 +653,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (reset_est) { x_est[0] = gps_proj[0]; x_est[1] = gps.vel_n_m_s; - x_est[2] = accel_NED[0]; y_est[0] = gps_proj[1]; y_est[1] = gps.vel_e_m_s; - y_est[2] = accel_NED[1]; } /* calculate correction for position */ @@ -796,26 +792,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) c += att.R[j][i] * accel_bias_corr[j]; } - acc_bias[i] += c * params.w_acc_bias * dt; + if (isfinite(c)) { + acc_bias[i] += c * params.w_acc_bias * dt; + } } /* inertial filter prediction for altitude */ - inertial_filter_predict(dt, z_est); + inertial_filter_predict(dt, z_est, acc[2]); - if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) { - write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { + write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(z_est, z_est_prev, sizeof(z_est)); } /* inertial filter correction for altitude */ inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); - inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); - if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) { - write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { + write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(z_est, z_est_prev, sizeof(z_est)); - memset(corr_acc, 0, sizeof(corr_acc)); memset(corr_gps, 0, sizeof(corr_gps)); corr_baro = 0; @@ -825,19 +821,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (can_estimate_xy) { /* inertial filter prediction for position */ - inertial_filter_predict(dt, x_est); - inertial_filter_predict(dt, y_est); + inertial_filter_predict(dt, x_est, acc[0]); + inertial_filter_predict(dt, y_est, acc[1]); - if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) { - write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { + write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); } /* inertial filter correction for position */ - inertial_filter_correct(corr_acc[0], dt, x_est, 2, params.w_xy_acc); - inertial_filter_correct(corr_acc[1], dt, y_est, 2, params.w_xy_acc); - if (use_flow) { inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); @@ -853,11 +846,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) { - write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) { + write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); - memset(corr_acc, 0, sizeof(corr_acc)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_flow, 0, sizeof(corr_flow)); 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 2e4f26661f..8aa08b6f2c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -42,11 +42,9 @@ 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); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); -PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); @@ -62,11 +60,9 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); - h->w_z_acc = param_find("INAV_W_Z_ACC"); h->w_z_sonar = param_find("INAV_W_Z_SONAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); - h->w_xy_acc = param_find("INAV_W_XY_ACC"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); @@ -85,11 +81,9 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str { param_get(h->w_z_baro, &(p->w_z_baro)); param_get(h->w_z_gps_p, &(p->w_z_gps_p)); - param_get(h->w_z_acc, &(p->w_z_acc)); param_get(h->w_z_sonar, &(p->w_z_sonar)); param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); - param_get(h->w_xy_acc, &(p->w_xy_acc)); param_get(h->w_xy_flow, &(p->w_xy_flow)); param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_acc_bias, &(p->w_acc_bias)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index e2be079d35..08ec996a13 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -43,11 +43,9 @@ struct position_estimator_inav_params { float w_z_baro; float w_z_gps_p; - float w_z_acc; float w_z_sonar; float w_xy_gps_p; float w_xy_gps_v; - float w_xy_acc; float w_xy_flow; float w_gps_flow; float w_acc_bias; @@ -63,11 +61,9 @@ struct position_estimator_inav_params { struct position_estimator_inav_param_handles { param_t w_z_baro; param_t w_z_gps_p; - param_t w_z_acc; param_t w_z_sonar; param_t w_xy_gps_p; param_t w_xy_gps_v; - param_t w_xy_acc; param_t w_xy_flow; param_t w_gps_flow; param_t w_acc_bias; From 7e7d78f50655f2f5f1291cf79a249e06f6a08017 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 May 2014 21:53:21 +0200 Subject: [PATCH 13/67] ekf_att_pos_estimator: Default printing to off --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 23ecd89ac3..5db1adbb3c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -5,7 +5,7 @@ // Define EKF_DEBUG here to enable the debug print calls // if the macro is not set, these will be completely // optimized out by the compiler. -#define EKF_DEBUG +//#define EKF_DEBUG #ifdef EKF_DEBUG #include From 00d40d095ddb4a5ad645044bed1ffdc1f977dba3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 May 2014 00:12:59 +0200 Subject: [PATCH 14/67] mavlink mission item takeoff: read correct param for minimal pitch --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 28dd97fca1..bb1ad86ef6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -415,7 +415,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) void Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) { - + Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { @@ -886,7 +886,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item switch (mavlink_mission_item->command) { case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param2; + mission_item->pitch_min = mavlink_mission_item->param1; break; default: From fcb890553329b4092c7dca319f5f538865734b3a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 28 May 2014 08:33:04 +0200 Subject: [PATCH 15/67] navigator: autocontinue and RTL autolanding fixes --- src/modules/navigator/navigator_main.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 87c893079f..310bdf9eab 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1458,7 +1458,6 @@ Navigator::check_mission_item_reached() /* XXX TODO count turns */ if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && _mission_item.loiter_radius > 0.01f) { @@ -1567,7 +1566,14 @@ Navigator::on_mission_item_reached() } if (_mission.current_mission_available()) { - set_mission_item(); + if (_mission_item.autocontinue) { + /* continue mission */ + set_mission_item(); + + } else { + /* autocontinue disabled for this item */ + request_loiter_or_ready(); + } } else { /* if no more mission items available then finish mission */ From b84cfea455ea801593b4ee04419417dcc7c4d4f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 May 2014 08:46:45 +0200 Subject: [PATCH 16/67] romfs: Added back mixer readme, now gets filtered out --- ROMFS/px4fmu_common/mixers/README | 154 ++++++++++++++++++++++++++++++ 1 file changed, 154 insertions(+) create mode 100644 ROMFS/px4fmu_common/mixers/README diff --git a/ROMFS/px4fmu_common/mixers/README b/ROMFS/px4fmu_common/mixers/README new file mode 100644 index 0000000000..60311232e8 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/README @@ -0,0 +1,154 @@ +PX4 mixer definitions +===================== + +Files in this directory implement example mixers that can be used as a basis +for customisation, or for general testing purposes. + +Mixer basics +------------ + +Mixers combine control values from various sources (control tasks, user inputs, +etc.) and produce output values suitable for controlling actuators; servos, +motors, switches and so on. + +An actuator derives its value from the combination of one or more control +values. Each of the control values is scaled according to the actuator's +configuration and then combined to produce the actuator value, which may then be +further scaled to suit the specific output type. + +Internally, all scaling is performed using floating point values. Inputs and +outputs are clamped to the range -1.0 to 1.0. + +control control control + | | | + v v v + scale scale scale + | | | + | v | + +-------> mix <------+ + | + scale + | + v + out + +Scaling +------- + +Basic scalers provide linear scaling of the input to the output. + +Each scaler allows the input value to be scaled independently for inputs +greater/less than zero. An offset can be applied to the output, and lower and +upper boundary constraints can be applied. Negative scaling factors cause the +output to be inverted (negative input produces positive output). + +Scaler pseudocode: + +if (input < 0) + output = (input * NEGATIVE_SCALE) + OFFSET +else + output = (input * POSITIVE_SCALE) + OFFSET + +if (output < LOWER_LIMIT) + output = LOWER_LIMIT +if (output > UPPER_LIMIT) + output = UPPER_LIMIT + +Syntax +------ + +Mixer definitions are text files; lines beginning with a single capital letter +followed by a colon are significant. All other lines are ignored, meaning that +explanatory text can be freely mixed with the definitions. + +Each file may define more than one mixer; the allocation of mixers to actuators +is specific to the device reading the mixer definition, and the number of +actuator outputs generated by a mixer is specific to the mixer. + +A mixer begins with a line of the form + + : + +The tag selects the mixer type; 'M' for a simple summing mixer, 'R' for a +multirotor mixer, etc. + +Null Mixer +.......... + +A null mixer consumes no controls and generates a single actuator output whose +value is always zero. Typically a null mixer is used as a placeholder in a +collection of mixers in order to achieve a specific pattern of actuator outputs. + +The null mixer definition has the form: + + Z: + +Simple Mixer +............ + +A simple mixer combines zero or more control inputs into a single actuator +output. Inputs are scaled, and the mixing function sums the result before +applying an output scaler. + +A simple mixer definition begins with: + + M: + O: <-ve scale> <+ve scale> + +If is zero, the sum is effectively zero and the mixer will +output a fixed value that is constrained by and . + +The second line defines the output scaler with scaler parameters as discussed +above. Whilst the calculations are performed as floating-point operations, the +values stored in the definition file are scaled by a factor of 10000; i.e. an +offset of -0.5 is encoded as -5000. + +The definition continues with entries describing the control +inputs and their scaling, in the form: + + S: <-ve scale> <+ve scale> + +The value identifies the control group from which the scaler will read, +and the value an offset within that group. These values are specific to +the device reading the mixer definition. + +When used to mix vehicle controls, mixer group zero is the vehicle attitude +control group, and index values zero through three are normally roll, pitch, +yaw and thrust respectively. + +The remaining fields on the line configure the control scaler with parameters as +discussed above. Whilst the calculations are performed as floating-point +operations, the values stored in the definition file are scaled by a factor of +10000; i.e. an offset of -0.5 is encoded as -5000. + +Multirotor Mixer +................ + +The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust) +into a set of actuator outputs intended to drive motor speed controllers. + +The mixer definition is a single line of the form: + +R: + +The supported geometries include: + + 4x - quadrotor in X configuration + 4+ - quadrotor in + configuration + 6x - hexcopter in X configuration + 6+ - hexcopter in + configuration + 8x - octocopter in X configuration + 8+ - octocopter in + configuration + +Each of the roll, pitch and yaw scale values determine scaling of the roll, +pitch and yaw controls relative to the thrust control. Whilst the calculations +are performed as floating-point operations, the values stored in the definition +file are scaled by a factor of 10000; i.e. an factor of 0.5 is encoded as 5000. + +Roll, pitch and yaw inputs are expected to range from -1.0 to 1.0, whilst the +thrust input ranges from 0.0 to 1.0. Output for each actuator is in the +range -1.0 to 1.0. + +In the case where an actuator saturates, all actuator values are rescaled so that +the saturating actuator is limited to 1.0. \ No newline at end of file From 366e9c633d7d37894ab71d46951605ebe951c062 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 May 2014 17:46:16 +0200 Subject: [PATCH 17/67] FMUv1 board prettifying --- src/drivers/boards/px4fmu-v1/board_config.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 58273f2d29..c944007a56 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -86,6 +86,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) #define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_EXT 2 /* * Use these in place of the spi_dev_e enumeration to @@ -98,7 +99,7 @@ __BEGIN_DECLS /* * Optional devices on IO's external port */ -#define PX4_SPIDEV_ACCEL_MAG 2 +#define PX4_SPIDEV_ACCEL_MAG 2 /* * I2C busses From e1309f239104a7493b40bebc5da3aa1e7572bd09 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 May 2014 17:51:00 +0200 Subject: [PATCH 18/67] Enable SPI4 for FMUv2 --- nuttx-configs/px4fmu-v2/include/board.h | 4 ++++ nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index 3bede8a1f7..3b3c6fa707 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -268,6 +268,10 @@ #define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) #define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz) +#define GPIO_SPI4_MISO (GPIO_SPI4_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI4_MOSI (GPIO_SPI4_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI4_SCK (GPIO_SPI4_SCK_1|GPIO_SPEED_50MHz) + /************************************************************************************ * Public Data ************************************************************************************/ diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index a982040c6d..7d5e6e9df5 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -235,7 +235,7 @@ CONFIG_STM32_SDIO=y CONFIG_STM32_SPI1=y CONFIG_STM32_SPI2=y # CONFIG_STM32_SPI3 is not set -# CONFIG_STM32_SPI4 is not set +CONFIG_STM32_SPI4=y # CONFIG_STM32_SPI5 is not set # CONFIG_STM32_SPI6 is not set CONFIG_STM32_SYSCFG=y From 096a4673e90240ea31fc414dbc09d3a00f24b180 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 May 2014 17:51:18 +0200 Subject: [PATCH 19/67] Add SPI4 init bits for FMUv2 --- src/drivers/boards/px4fmu-v2/board_config.h | 5 +++ src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 12 +++++++ src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 35 +++++++++++++++++++++ 3 files changed, 52 insertions(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index c2de1bfba0..3ed9888e45 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -106,14 +106,19 @@ __BEGIN_DECLS #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_EXT0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) +#define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) #define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_EXT 4 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 #define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_BARO 3 #define PX4_SPIDEV_MPU 4 +#define PX4_SPIDEV_EXT0 5 +#define PX4_SPIDEV_EXT1 6 /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 71414d62c7..bf41bb1fed 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -192,6 +192,7 @@ stm32_boardinitialize(void) static struct spi_dev_s *spi1; static struct spi_dev_s *spi2; +static struct spi_dev_s *spi4; static struct sdio_dev_s *sdio; #include @@ -305,6 +306,17 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n"); + spi4 = up_spiinitialize(4); + + /* Default SPI4 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi4, 10000000); + SPI_SETBITS(spi4, 8); + SPI_SETMODE(spi4, SPIDEV_MODE3); + SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false); + SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false); + + message("[boot] Initialized SPI port 4\n"); + #ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index c66c490a7f..9033290520 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -94,6 +94,13 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_configgpio(GPIO_SPI_CS_FRAM); stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); #endif + +#ifdef CONFIG_STM32_SPI4 + stm32_configgpio(GPIO_SPI_CS_EXT0); + stm32_configgpio(GPIO_SPI_CS_EXT1); + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); +#endif } __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) @@ -157,3 +164,31 @@ __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devi return SPI_STATUS_PRESENT; } #endif + +__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_EXT0: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(PX4_SPIDEV_EXT0, !selected); + stm32_gpiowrite(PX4_SPIDEV_EXT1, 1); + break; + + case PX4_SPIDEV_EXT1: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(PX4_SPIDEV_EXT1, !selected); + stm32_gpiowrite(PX4_SPIDEV_EXT0, 1); + break; + + default: + break; + + } +} + +__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} From 2bfde311db7daa0771c7be7f165b7601bf2946ca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 May 2014 10:45:11 +0200 Subject: [PATCH 20/67] Fix SPI select on port 4 to use the right defines --- src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 9033290520..01dbd6e774 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -172,14 +172,14 @@ __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_EXT0: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(PX4_SPIDEV_EXT0, !selected); - stm32_gpiowrite(PX4_SPIDEV_EXT1, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); break; case PX4_SPIDEV_EXT1: /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(PX4_SPIDEV_EXT1, !selected); - stm32_gpiowrite(PX4_SPIDEV_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); break; default: From 65344133a4a6944b71f2ca134c6c4443ff7c96cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 May 2014 10:45:38 +0200 Subject: [PATCH 21/67] Count devices on SPI4 / EXT from 1 as for the other buses --- src/drivers/boards/px4fmu-v2/board_config.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 3ed9888e45..36eb7bec4e 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -117,8 +117,10 @@ __BEGIN_DECLS #define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_BARO 3 #define PX4_SPIDEV_MPU 4 -#define PX4_SPIDEV_EXT0 5 -#define PX4_SPIDEV_EXT1 6 + +/* External bus */ +#define PX4_SPIDEV_EXT0 1 +#define PX4_SPIDEV_EXT1 2 /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 From b08a429d89cc10cb5d9dc953a218527dc20c230c Mon Sep 17 00:00:00 2001 From: Kynos Date: Wed, 28 May 2014 14:28:41 +0200 Subject: [PATCH 22/67] Updated AR.Drone parameters Flight tested May 7th, 2014. --- ROMFS/px4fmu_common/init.d/4008_ardrone | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index 14786f2106..e6007db0e7 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -14,19 +14,20 @@ sh /etc/init.d/rc.mc_defaults if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig - param set MC_ROLL_P 5.0 - param set MC_ROLLRATE_P 0.13 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.0 - param set MC_PITCH_P 5.0 - param set MC_PITCHRATE_P 0.13 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.0 - param set MC_YAW_P 1.0 - param set MC_YAWRATE_P 0.15 - param set MC_YAWRATE_I 0.0 + param set MC_ROLL_P 6.0 + param set MC_ROLLRATE_P 0.14 + param set MC_ROLLRATE_I 0.1 + param set MC_ROLLRATE_D 0.002 + param set MC_PITCH_P 6.0 + param set MC_PITCHRATE_P 0.14 + param set MC_PITCHRATE_I 0.1 + param set MC_PITCHRATE_D 0.002 + param set MC_YAW_P 2.0 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_D 0.0 - param set MC_YAW_FF 0.15 + param set MC_YAW_FF 0.8 + param set BAT_V_SCALING 0.00838095238 fi From fe28069effe77dcac143c0194b982028438068f3 Mon Sep 17 00:00:00 2001 From: Kynos Date: Wed, 28 May 2014 17:26:41 +0200 Subject: [PATCH 23/67] Increase UART1 & UART5 RX&Tx buffer sizes To fix MAVLink message garbling problems. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index bc0c120679..f2c7d22bfe 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -504,8 +504,8 @@ CONFIG_MTD_BYTE_WRITE=y # # USART1 Configuration # -CONFIG_USART1_RXBUFSIZE=256 -CONFIG_USART1_TXBUFSIZE=128 +CONFIG_USART1_RXBUFSIZE=300 +CONFIG_USART1_TXBUFSIZE=300 CONFIG_USART1_BAUD=57600 CONFIG_USART1_BITS=8 CONFIG_USART1_PARITY=0 @@ -528,8 +528,8 @@ CONFIG_USART2_OFLOWCONTROL=y # # UART5 Configuration # -CONFIG_UART5_RXBUFSIZE=256 -CONFIG_UART5_TXBUFSIZE=128 +CONFIG_UART5_RXBUFSIZE=300 +CONFIG_UART5_TXBUFSIZE=300 CONFIG_UART5_BAUD=57600 CONFIG_UART5_BITS=8 CONFIG_UART5_PARITY=0 From bb6b442ca66a8349eb934a43b5886f44c2b7ab87 Mon Sep 17 00:00:00 2001 From: Liio Chen Date: Thu, 29 May 2014 17:42:05 +0800 Subject: [PATCH 24/67] Magnetometer data is not update MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Magnetometer is not updated during a read operation, because the function "lsm303d_mag::measure" is not called. ”!!!JUST A GUESS!!!“ --- src/drivers/lsm303d/lsm303d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 4ca8b5e42d..8bf76dcc33 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -880,7 +880,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) /* manual measurement */ _mag_reports->flush(); - measure(); + _mag->measure(); /* measurement will have generated a report, copy it out */ if (_mag_reports->get(mrb)) From 17d8e2a1663b36504a432b120c3993e4912842b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 May 2014 12:24:50 +0200 Subject: [PATCH 25/67] PCA8574 driver: Cleanup, ready for final testing and production --- src/drivers/drv_io_expander.h | 18 +- src/drivers/pca8574/pca8574.cpp | 343 +++++++++++++------------------- 2 files changed, 149 insertions(+), 212 deletions(-) diff --git a/src/drivers/drv_io_expander.h b/src/drivers/drv_io_expander.h index 2705d6f9e1..106354377f 100644 --- a/src/drivers/drv_io_expander.h +++ b/src/drivers/drv_io_expander.h @@ -49,17 +49,23 @@ #define _IOXIOCBASE (0x2800) #define _IOXIOC(_n) (_IOC(_IOXIOCBASE, _n)) -/** enable the device */ -#define IOX_ENABLE _IOXIOC(1) +/** set a bitmask (non-blocking) */ +#define IOX_SET_MASK _IOXIOC(1) -/** set constant values */ -#define IOX_SET_VALUE _IOXIOC(2) +/** get a bitmask (blocking) */ +#define IOX_GET_MASK _IOXIOC(2) -/** set constant values */ +/** set device mode (non-blocking) */ #define IOX_SET_MODE _IOXIOC(3) +/** set constant values (non-blocking) */ +#define IOX_SET_VALUE _IOXIOC(4) + +/* ... to IOX_SET_VALUE + 8 */ + /* enum passed to RGBLED_SET_MODE ioctl()*/ enum IOX_MODE { IOX_MODE_OFF, - IOX_MODE_ON + IOX_MODE_ON, + IOX_MODE_TEST_OUT }; diff --git a/src/drivers/pca8574/pca8574.cpp b/src/drivers/pca8574/pca8574.cpp index ce882b6367..cd1ffc82c6 100644 --- a/src/drivers/pca8574/pca8574.cpp +++ b/src/drivers/pca8574/pca8574.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Julian Oes - * Anton Babushkin + * 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 @@ -36,8 +34,11 @@ /** * @file pca8574.cpp * - * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * Driver for an 8 I/O controller (PC8574) connected via I2C. * + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin */ #include @@ -68,16 +69,7 @@ #define PCA8574_OFFTIME 120 #define PCA8574_DEVICE_PATH "/dev/pca8574" -#define ADDR 0x20 /**< I2C adress of PCA8574 (default, A0-A2 pulled to GND) */ -#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ -#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ -#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ -#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */ -#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/ - -#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */ -#define SETTING_ENABLE 0x02 /**< on */ - +#define ADDR 0x20 ///< I2C adress of PCA8574 (default, A0-A2 pulled to GND) class PCA8574 : public device::I2C { @@ -94,20 +86,23 @@ public: private: work_s _work; - uint8_t _values[8]; - float _brightness; + uint8_t _values_out; + uint8_t _values_in; enum IOX_MODE _mode; bool _running; int _led_interval; bool _should_run; + bool _update_out; int _counter; static void led_trampoline(void *arg); void led(); - int send_led_enable(bool enable); + int send_led_enable(uint8_t arg); int send_led_values(); + + int get(uint8_t &vals); }; /* for now, we only support one PCA8574 */ @@ -122,12 +117,13 @@ extern "C" __EXPORT int pca8574_main(int argc, char *argv[]); PCA8574::PCA8574(int bus, int pca8574) : I2C("pca8574", PCA8574_DEVICE_PATH, bus, pca8574, 100000), + _values_out(0), + _values_in(0), _mode(IOX_MODE_OFF), - _values({}), - _brightness(1.0f), _running(false), _led_interval(0), _should_run(false), + _update_out(false), _counter(0) { memset(&_work, 0, sizeof(_work)); @@ -147,10 +143,10 @@ PCA8574::init() return ret; } - /* switch off LED on start */ - send_led_enable(false); + // switch off LED on start (active low on Pixhawk) + send_led_enable(0xFF); - /* kick it in */ + // kick it in _should_run = true; _led_interval = 80; work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); @@ -179,24 +175,69 @@ PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg) int ret = ENOTTY; switch (cmd) { - case IOX_SET_VALUE ... (IOX_SET_VALUE + 8): - { - /* set the specified color */ - unsigned prev = _values[cmd - IOX_SET_VALUE]; - _values[cmd - IOX_SET_VALUE] = arg; - if (_values[cmd - IOX_SET_VALUE] != prev) { - // XXX will be done with a change flag - send_led_values(); - } - return OK; - } + case IOX_SET_VALUE ...(IOX_SET_VALUE + 8): { + // set the specified on / off state + uint8_t position = (1 << (cmd - IOX_SET_VALUE)); + uint8_t prev = _values_out; - case IOX_ENABLE: + if (arg) { + _values_out |= position; + + } else { + _values_out &= ~(position); + } + + if (_values_out != prev) { + send_led_values(); + } + + return OK; + } + + case IOX_SET_MASK: send_led_enable(arg); return OK; + case IOX_GET_MASK: { + uint8_t val; + ret = get(val); + + if (ret == OK) { + return val; + + } else { + return -1; + } + } + + case IOX_SET_MODE: + + if (_mode != (IOX_MODE)arg) { + + switch ((IOX_MODE)arg) { + case IOX_MODE_OFF: + _values_out = 0xFF; + break; + + case IOX_MODE_ON: + _values_out = 0; + break; + + case IOX_MODE_TEST_OUT: + break; + + default: + return -1; + } + + _mode = (IOX_MODE)arg; + send_led_values(); + } + + return OK; + default: - /* see if the parent class can make any use of it */ + // see if the parent class can make any use of it ret = CDev::ioctl(filp, cmd, arg); break; } @@ -224,204 +265,93 @@ PCA8574::led() return; } - // switch (_mode) { - // case PCA8574_MODE_BLINK_SLOW: - // case PCA8574_MODE_BLINK_NORMAL: - // case PCA8574_MODE_BLINK_FAST: - // if (_counter >= 2) - // _counter = 0; + if (_mode == IOX_MODE_TEST_OUT) { - // send_led_enable(_counter == 0); + // we count only seven states + _counter &= 0xF; + _counter++; - // break; + for (int i = 0; i < 8; i++) { + if (i < _counter) { + _values_out |= (1 << i); - // case PCA8574_MODE_BREATHE: + } else { + _values_out &= ~(1 << i); + } + } - // if (_counter >= 62) - // _counter = 0; + _update_out = true; + } - // int n; + if (_update_out) { + uint8_t msg = _values_out; - // if (_counter < 32) { - // n = _counter; + int ret = transfer(&msg, sizeof(msg), nullptr, 0); - // } else { - // n = 62 - _counter; - // } - - // _brightness = n * n / (31.0f * 31.0f); - // send_led_rgb(); - // break; - - // case PCA8574_MODE_PATTERN: - - // /* don't run out of the pattern array and stop if the next frame is 0 */ - // if (_counter >= PCA8574_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) - // _counter = 0; - - // set_color(_pattern.color[_counter]); - // send_led_rgb(); - // _led_interval = _pattern.duration[_counter]; - // break; - - // default: - // break; - // } - - - // we count only seven states - _counter &= 0xF; - _counter++; - - for (int i = 0; i < 8; i++) { - if (i < _counter) { - _values[i] = 1; - } else { - _values[i] = 0; + if (!ret) { + _update_out = false; } } - send_led_values(); + // check if any activity remains, else stp + if (!_update_out) { + _running = false; + return; + } - /* re-queue ourselves to run again later */ + // re-queue ourselves to run again later work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, _led_interval); } -// /** -// * Set mode, if mode not changed has no any effect (doesn't reset blinks phase) -// */ -// void -// PCA8574::set_mode(pca8574_mode_t mode) -// { -// if (mode != _mode) { -// _mode = mode; - -// switch (mode) { -// // case PCA8574_MODE_OFF: -// // _should_run = false; -// // send_led_enable(false); -// // break; - -// // case PCA8574_MODE_ON: -// // _brightness = 1.0f; -// // send_led_rgb(); -// // send_led_enable(true); -// // break; - -// // case PCA8574_MODE_BLINK_SLOW: -// // _should_run = true; -// // _counter = 0; -// // _led_interval = 2000; -// // _brightness = 1.0f; -// // send_led_rgb(); -// // break; - -// // case PCA8574_MODE_BLINK_NORMAL: -// // _should_run = true; -// // _counter = 0; -// // _led_interval = 500; -// // _brightness = 1.0f; -// // send_led_rgb(); -// // break; - -// // case PCA8574_MODE_BLINK_FAST: -// // _should_run = true; -// // _counter = 0; -// // _led_interval = 100; -// // _brightness = 1.0f; -// // send_led_rgb(); -// // break; - -// // case PCA8574_MODE_BREATHE: -// // _should_run = true; -// // _counter = 0; -// // _led_interval = 25; -// // send_led_enable(true); -// // break; - -// // case PCA8574_MODE_PATTERN: -// // _should_run = true; -// // _counter = 0; -// // _brightness = 1.0f; -// // send_led_enable(true); -// // break; - -// default: -// warnx("mode unknown"); -// break; -// } - -// /* if it should run now, start the workq */ -// if (_should_run && !_running) { -// _running = true; -// work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); -// } - -// } -// } - /** * Sent ENABLE flag to LED driver */ int -PCA8574::send_led_enable(bool enable) +PCA8574::send_led_enable(uint8_t arg) { - uint8_t msg; - if (enable) { - /* active low */ - msg = 0x00; - } else { - /* active low, so off */ - msg = 0xFF; - } - - int ret = transfer(&msg, sizeof(msg), nullptr, 0); + int ret = transfer(&arg, sizeof(arg), nullptr, 0); return ret; } /** - * Send RGB PWM settings to LED driver according to current color and brightness + * Send 8 outputs */ int PCA8574::send_led_values() { - uint8_t msg = 0; + _update_out = true; + _should_run = true; - for (int i = 0; i < 8; i++) { - if (_values[i]) { - msg |= (1 << i); - } + // if not active, kick it + if (!_running) { + work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); } - int ret = transfer(&msg, sizeof(msg), nullptr, 0); + return 0; } -// int -// PCA8574::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) -// { -// uint8_t result[2]; -// int ret; +int +PCA8574::get(uint8_t &vals) +{ + uint8_t result; + int ret; -// ret = transfer(nullptr, 0, &result[0], 2); + ret = transfer(nullptr, 0, &result, 1); -// if (ret == OK) { -// on = result[0] & SETTING_ENABLE; -// powersave = !(result[0] & SETTING_NOT_POWERSAVE); -// /* XXX check, looks wrong */ -// r = (result[0] & 0x0f) << 4; -// g = (result[1] & 0xf0); -// b = (result[1] & 0x0f) << 4; -// } + if (ret == OK) { + _values_in = result; + vals = result; + } -// return ret; -// } + return ret; +} void pca8574_usage() { - warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'val 0 100'"); + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'val 0 1'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); warnx(" -a addr (0x%x)", ADDR); @@ -431,11 +361,11 @@ int pca8574_main(int argc, char *argv[]) { int i2cdevice = -1; - int pca8574adr = ADDR; /* 7bit */ + int pca8574adr = ADDR; // 7bit int ch; - /* jump over start/off/etc and look at options first */ + // jump over start/off/etc and look at options first while ((ch = getopt(argc, argv, "a:b:")) != EOF) { switch (ch) { case 'a': @@ -452,10 +382,10 @@ pca8574_main(int argc, char *argv[]) } } - if (optind >= argc) { - pca8574_usage(); - exit(1); - } + if (optind >= argc) { + pca8574_usage(); + exit(1); + } const char *verb = argv[optind]; @@ -463,8 +393,9 @@ pca8574_main(int argc, char *argv[]) int ret; if (!strcmp(verb, "start")) { - if (g_pca8574 != nullptr) + if (g_pca8574 != nullptr) { errx(1, "already started"); + } if (i2cdevice == -1) { // try the external bus first @@ -481,6 +412,7 @@ pca8574_main(int argc, char *argv[]) if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { errx(1, "init failed"); } + i2cdevice = PX4_I2C_BUS_LED; } } @@ -488,8 +420,9 @@ pca8574_main(int argc, char *argv[]) if (g_pca8574 == nullptr) { g_pca8574 = new PCA8574(i2cdevice, pca8574adr); - if (g_pca8574 == nullptr) + if (g_pca8574 == nullptr) { errx(1, "new failed"); + } if (OK != g_pca8574->init()) { delete g_pca8574; @@ -501,7 +434,7 @@ pca8574_main(int argc, char *argv[]) exit(0); } - /* need the driver past this point */ + // need the driver past this point if (g_pca8574 == nullptr) { warnx("not started"); pca8574_usage(); @@ -515,8 +448,7 @@ pca8574_main(int argc, char *argv[]) errx(1, "Unable to open " PCA8574_DEVICE_PATH); } - ret = ioctl(fd, IOX_SET_VALUE, 255); - // ret = ioctl(fd, PCA8574_SET_MODE, (unsigned long)PCA8574_MODE_PATTERN); + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT); close(fd); exit(ret); @@ -547,7 +479,7 @@ pca8574_main(int argc, char *argv[]) if (!strcmp(verb, "val")) { if (argc < 4) { - errx(1, "Usage: pca8574 val "); + errx(1, "Usage: pca8574 val <0 or 1>"); } fd = open(PCA8574_DEVICE_PATH, 0); @@ -558,8 +490,7 @@ pca8574_main(int argc, char *argv[]) unsigned channel = strtol(argv[2], NULL, 0); unsigned val = strtol(argv[3], NULL, 0); - ret = ioctl(fd, (IOX_SET_VALUE+channel), val); - ret = ioctl(fd, IOX_ENABLE, 1); + ret = ioctl(fd, (IOX_SET_VALUE + channel), val); close(fd); exit(ret); } From 0c2e70d30f3e1d927a22fdd066a4a14ecc698de1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 May 2014 13:52:29 +0200 Subject: [PATCH 26/67] Enable the driver as default for boards having it --- ROMFS/px4fmu_common/init.d/rcS | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8c413e0878..87ec4293ea 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -93,14 +93,18 @@ then # if rgbled start then - echo "[init] Using external RGB Led" + echo "[init] RGB Led" else if blinkm start then - echo "[init] Using blinkm" + echo "[init] BlinkM" blinkm systemstate fi fi + + if pca8574 start + then + fi # # Set default values From eb02e74d30a215ff415983913bf569a23e916a2b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 May 2014 13:52:47 +0200 Subject: [PATCH 27/67] PCA cleanup: Full interfaces ready for AP use --- src/drivers/pca8574/pca8574.cpp | 108 ++++++++++++++++++++++++-------- 1 file changed, 81 insertions(+), 27 deletions(-) diff --git a/src/drivers/pca8574/pca8574.cpp b/src/drivers/pca8574/pca8574.cpp index cd1ffc82c6..904ce18e8a 100644 --- a/src/drivers/pca8574/pca8574.cpp +++ b/src/drivers/pca8574/pca8574.cpp @@ -82,12 +82,15 @@ public: virtual int probe(); virtual int info(); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + bool is_running() { return _running; } private: work_s _work; uint8_t _values_out; uint8_t _values_in; + uint8_t _blinking; + uint8_t _blink_phase; enum IOX_MODE _mode; bool _running; @@ -119,9 +122,11 @@ PCA8574::PCA8574(int bus, int pca8574) : I2C("pca8574", PCA8574_DEVICE_PATH, bus, pca8574, 100000), _values_out(0), _values_in(0), + _blinking(0), + _blink_phase(0), _mode(IOX_MODE_OFF), _running(false), - _led_interval(0), + _led_interval(80), _should_run(false), _update_out(false), _counter(0) @@ -143,22 +148,14 @@ PCA8574::init() return ret; } - // switch off LED on start (active low on Pixhawk) - send_led_enable(0xFF); - - // kick it in - _should_run = true; - _led_interval = 80; - work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); - return OK; } int PCA8574::probe() { - - return send_led_enable(false); + uint8_t val; + return get(val); } int @@ -188,6 +185,9 @@ PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg) } if (_values_out != prev) { + if (_values_out) { + _mode = IOX_MODE_ON; + } send_led_values(); } @@ -260,11 +260,6 @@ PCA8574::led_trampoline(void *arg) void PCA8574::led() { - if (!_should_run) { - _running = false; - return; - } - if (_mode == IOX_MODE_TEST_OUT) { // we count only seven states @@ -281,10 +276,37 @@ PCA8574::led() } _update_out = true; + _should_run = true; + } else if (_mode == IOX_MODE_OFF) { + _update_out = true; + _should_run = false; + } else { + + // Any of the normal modes + if (_blinking > 0) { + /* we need to be running to blink */ + _should_run = true; + } else { + _should_run = false; + } } if (_update_out) { - uint8_t msg = _values_out; + uint8_t msg; + + if (_blinking) { + msg = (_values_out & _blinking & _blink_phase); + + // wipe out all positions that are marked as blinking + msg &= ~(_blinking); + + // fill blink positions + msg |= ((_blink_phase) ? _blinking : 0); + + _blink_phase = !_blink_phase; + } else { + msg = _values_out; + } int ret = transfer(&msg, sizeof(msg), nullptr, 0); @@ -294,12 +316,13 @@ PCA8574::led() } // check if any activity remains, else stp - if (!_update_out) { + if (!_should_run) { _running = false; return; } // re-queue ourselves to run again later + _running = true; work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, _led_interval); } @@ -322,10 +345,10 @@ int PCA8574::send_led_values() { _update_out = true; - _should_run = true; // if not active, kick it if (!_running) { + _running = true; work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); } @@ -436,8 +459,7 @@ pca8574_main(int argc, char *argv[]) // need the driver past this point if (g_pca8574 == nullptr) { - warnx("not started"); - pca8574_usage(); + warnx("not started, run pca8574 start"); exit(1); } @@ -459,10 +481,10 @@ pca8574_main(int argc, char *argv[]) exit(0); } - if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { + if (!strcmp(verb, "off")) { fd = open(PCA8574_DEVICE_PATH, 0); - if (fd == -1) { + if (fd < 0) { errx(1, "Unable to open " PCA8574_DEVICE_PATH); } @@ -472,9 +494,36 @@ pca8574_main(int argc, char *argv[]) } if (!strcmp(verb, "stop")) { - delete g_pca8574; - g_pca8574 = nullptr; - exit(0); + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); + close(fd); + + // wait until we're not running any more + for (unsigned i = 0; i < 15; i++) { + if (!g_pca8574->is_running()) { + break; + } + + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + fflush(stdout); + + if (!g_pca8574->is_running()) { + delete g_pca8574; + g_pca8574 = nullptr; + exit(0); + } else { + warnx("stop failed."); + exit(1); + } } if (!strcmp(verb, "val")) { @@ -490,7 +539,12 @@ pca8574_main(int argc, char *argv[]) unsigned channel = strtol(argv[2], NULL, 0); unsigned val = strtol(argv[3], NULL, 0); - ret = ioctl(fd, (IOX_SET_VALUE + channel), val); + + if (channel < 8) { + ret = ioctl(fd, (IOX_SET_VALUE + channel), val); + } else { + ret = -1; + } close(fd); exit(ret); } From 46301f753d212b55e151a394bc9d4b3b787f35ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 May 2014 18:28:37 +0200 Subject: [PATCH 28/67] Minor fixes to MAVLink --- src/modules/mavlink/mavlink_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index bb1ad86ef6..28cdf65a3c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -190,8 +190,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length /* check if there is space in the buffer, let it overflow else */ if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { - if (desired > buf_free) { - desired = buf_free; + if (buf_free < desired) { + /* we don't want to send anything just in half, so return */ + return; } } @@ -222,6 +223,8 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), + _mode(MAVLINK_MODE_NORMAL), + _total_counter(0), _verbose(false), _forwarding_on(false), _passing_on(false), From ead91f325989cda312424ec3b2cc7fd11913f5c8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 29 May 2014 22:42:33 +0200 Subject: [PATCH 29/67] position_estimator_inav: GPS delay compensation --- .../position_estimator_inav_main.c | 50 ++++++++++++++++--- .../position_estimator_inav_params.c | 3 ++ .../position_estimator_inav_params.h | 2 + 3 files changed, 47 insertions(+), 8 deletions(-) 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 d7503e42da..d6a44304d2 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -71,6 +71,8 @@ #include "inertial_filter.h" #define MIN_VALID_W 0.00001f +#define PUB_INTERVAL 10000 // limit publish rate to 100 Hz +#define EST_BUF_SIZE 500000 / PUB_INTERVAL // buffer size is 0.5s static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -83,7 +85,6 @@ static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss static const uint32_t updates_counter_len = 1000000; -static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz static const float max_flow = 1.0f; // max flow value that can be used, rad/s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -92,6 +93,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]); static void usage(const char *reason); +static inline int min(int val1, int val2) +{ + return (val1 < val2) ? val1 : val2; +} + +static inline int max(int val1, int val2) +{ + return (val1 > val2) ? val1 : val2; +} + /** * Print the correct usage. */ @@ -199,6 +210,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float y_est[2] = { 0.0f, 0.0f }; // pos, vel float z_est[2] = { 0.0f, 0.0f }; // pos, vel + float est_buf[EST_BUF_SIZE][3][2]; + memset(est_buf, 0, sizeof(est_buf)); + int est_buf_ptr = 0; + float eph = 1.0; float epv = 1.0; @@ -657,16 +672,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) y_est[1] = gps.vel_e_m_s; } + /* calculate index of estimated values in buffer */ + int est_i = est_buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL))); + if (est_i < 0) { + est_i += EST_BUF_SIZE; + } + /* calculate correction for position */ - corr_gps[0][0] = gps_proj[0] - x_est[0]; - corr_gps[1][0] = gps_proj[1] - y_est[0]; - corr_gps[2][0] = local_pos.ref_alt - alt - z_est[0]; + corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0]; + corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0]; + corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0]; /* calculate correction for velocity */ if (gps.vel_ned_valid) { - corr_gps[0][1] = gps.vel_n_m_s - x_est[1]; - corr_gps[1][1] = gps.vel_e_m_s - y_est[1]; - corr_gps[2][1] = gps.vel_d_m_s - z_est[1]; + corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1]; + corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1]; + corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1]; } else { corr_gps[0][1] = 0.0f; @@ -910,8 +931,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (t > pub_last + pub_interval) { + if (t > pub_last + PUB_INTERVAL) { pub_last = t; + + /* push current estimate to FIFO buffer */ + est_buf[est_buf_ptr][0][0] = x_est[0]; + est_buf[est_buf_ptr][0][1] = x_est[1]; + est_buf[est_buf_ptr][1][0] = y_est[0]; + est_buf[est_buf_ptr][1][1] = y_est[1]; + est_buf[est_buf_ptr][2][0] = z_est[0]; + est_buf[est_buf_ptr][2][1] = z_est[1]; + est_buf_ptr++; + if (est_buf_ptr >= EST_BUF_SIZE) { + est_buf_ptr = 0; + } + /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; 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 8aa08b6f2c..8509d15cba 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -55,6 +55,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); +PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.1f); int parameters_init(struct position_estimator_inav_param_handles *h) { @@ -73,6 +74,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->land_t = param_find("INAV_LAND_T"); h->land_disp = param_find("INAV_LAND_DISP"); h->land_thr = param_find("INAV_LAND_THR"); + h->delay_gps = param_find("INAV_DELAY_GPS"); return OK; } @@ -94,6 +96,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->land_t, &(p->land_t)); param_get(h->land_disp, &(p->land_disp)); param_get(h->land_thr, &(p->land_thr)); + param_get(h->delay_gps, &(p->delay_gps)); return OK; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 08ec996a13..df1cfaa710 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -56,6 +56,7 @@ struct position_estimator_inav_params { float land_t; float land_disp; float land_thr; + float delay_gps; }; struct position_estimator_inav_param_handles { @@ -74,6 +75,7 @@ struct position_estimator_inav_param_handles { param_t land_t; param_t land_disp; param_t land_thr; + param_t delay_gps; }; /** From fdd5d7b8b8835dd3e2655cd104f7440c9f56ba27 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 May 2014 11:03:06 +0200 Subject: [PATCH 30/67] position_estimator_inav: add buffer for rotation matrix to do accel bias correction properly --- .../position_estimator_inav_main.c | 61 ++++++++++++++----- 1 file changed, 45 insertions(+), 16 deletions(-) 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 d6a44304d2..aa533c777f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -72,7 +72,7 @@ #define MIN_VALID_W 0.00001f #define PUB_INTERVAL 10000 // limit publish rate to 100 Hz -#define EST_BUF_SIZE 500000 / PUB_INTERVAL // buffer size is 0.5s +#define EST_BUF_SIZE 250000 / PUB_INTERVAL // buffer size is 0.5s static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -146,7 +146,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", - SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000, + SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); @@ -210,9 +210,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float y_est[2] = { 0.0f, 0.0f }; // pos, vel float z_est[2] = { 0.0f, 0.0f }; // pos, vel - float est_buf[EST_BUF_SIZE][3][2]; + float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer + float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer + float R_gps[3][3]; // rotation matrix for GPS correction moment memset(est_buf, 0, sizeof(est_buf)); - int est_buf_ptr = 0; + memset(R_buf, 0, sizeof(R_buf)); + memset(R_gps, 0, sizeof(R_gps)); + int buf_ptr = 0; float eph = 1.0; float epv = 1.0; @@ -673,7 +677,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* calculate index of estimated values in buffer */ - int est_i = est_buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL))); + int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL))); if (est_i < 0) { est_i += EST_BUF_SIZE; } @@ -695,6 +699,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } + /* save rotation matrix at this moment */ + memcpy(R_gps, R_buf[est_i], sizeof(R_gps)); + eph = fminf(eph, gps.eph_m); epv = fminf(epv, gps.epv_m); @@ -784,7 +791,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_baro += offs_corr; } - /* accelerometer bias correction */ + /* accelerometer bias correction for GPS (use buffered rotation matrix) */ float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; if (use_gps_xy) { @@ -798,6 +805,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; } + /* transform error vector from NED frame to body frame */ + for (int i = 0; i < 3; i++) { + float c = 0.0f; + + for (int j = 0; j < 3; j++) { + c += R_gps[j][i] * accel_bias_corr[j]; + } + + if (isfinite(c)) { + acc_bias[i] += c * params.w_acc_bias * dt; + } + } + + /* accelerometer bias correction for flow and baro (assume that there is no delay) */ + accel_bias_corr[0] = 0.0f; + accel_bias_corr[1] = 0.0f; + accel_bias_corr[2] = 0.0f; + if (use_flow) { accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow; accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow; @@ -934,16 +959,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t > pub_last + PUB_INTERVAL) { pub_last = t; - /* push current estimate to FIFO buffer */ - est_buf[est_buf_ptr][0][0] = x_est[0]; - est_buf[est_buf_ptr][0][1] = x_est[1]; - est_buf[est_buf_ptr][1][0] = y_est[0]; - est_buf[est_buf_ptr][1][1] = y_est[1]; - est_buf[est_buf_ptr][2][0] = z_est[0]; - est_buf[est_buf_ptr][2][1] = z_est[1]; - est_buf_ptr++; - if (est_buf_ptr >= EST_BUF_SIZE) { - est_buf_ptr = 0; + /* push current estimate to buffer */ + est_buf[buf_ptr][0][0] = x_est[0]; + est_buf[buf_ptr][0][1] = x_est[1]; + est_buf[buf_ptr][1][0] = y_est[0]; + est_buf[buf_ptr][1][1] = y_est[1]; + est_buf[buf_ptr][2][0] = z_est[0]; + est_buf[buf_ptr][2][1] = z_est[1]; + + /* push current rotation matrix to buffer */ + memcpy(R_buf[buf_ptr], att.R, sizeof(att.R)); + + buf_ptr++; + if (buf_ptr >= EST_BUF_SIZE) { + buf_ptr = 0; } /* publish local position */ From 718206bd6d4103d8d2b1ad6a111770f65622029a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 30 May 2014 14:37:23 +0200 Subject: [PATCH 31/67] ubx driver: fix unit of speed and position accuracy estimate --- src/drivers/gps/ubx.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 19cf5beecf..7502809bd7 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -447,8 +447,8 @@ UBX::handle_message() gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = packet->sAcc; - _gps_position->p_variance_m = packet->pAcc; + _gps_position->s_variance_m_s = (float)packet->sAcc * 1e-2f; // from cm/s to m/s + _gps_position->p_variance_m = (float)packet->pAcc * 1e-2f; // from cm to m _gps_position->timestamp_variance = hrt_absolute_time(); ret = 1; From 0bdde089243bbf7e3651f9535f5c847f894e0ac7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 May 2014 21:18:03 +0200 Subject: [PATCH 32/67] position_estimator_inav: default GPS delay changed to 0.2s --- .../position_estimator_inav/position_estimator_inav_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 8509d15cba..d88419c258 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -55,7 +55,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); -PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.1f); +PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); int parameters_init(struct position_estimator_inav_param_handles *h) { From 47c9d326209034dc373ab54647d29df4e63b5285 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 May 2014 21:23:26 +0200 Subject: [PATCH 33/67] ubx: disable all debug messages --- src/drivers/gps/ubx.cpp | 170 ++++++++++++++++++++-------------------- 1 file changed, 85 insertions(+), 85 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 7502809bd7..cbfb61d00a 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -219,19 +219,19 @@ UBX::configure(unsigned &baudrate) return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); - - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV SVINFO"); - return 1; - } - - configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); - - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: MON HW"); - return 1; - } +// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); +// +// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { +// warnx("MSG CFG FAIL: NAV SVINFO"); +// return 1; +// } +// +// configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); +// +// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { +// warnx("MSG CFG FAIL: MON HW"); +// return 1; +// } _configured = true; return 0; @@ -486,61 +486,61 @@ UBX::handle_message() break; } - case UBX_MESSAGE_NAV_SVINFO: { - //printf("GOT NAV_SVINFO\n"); - const int length_part1 = 8; - gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; - const int length_part2 = 12; - gps_bin_nav_svinfo_part2_packet_t *packet_part2; - - uint8_t satellites_used = 0; - int i; - - //printf("Number of Channels: %d\n", packet_part1->numCh); - for (i = 0; i < packet_part1->numCh; i++) { - /* set pointer to sattelite_i information */ - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); - - /* write satellite information to global storage */ - uint8_t sv_used = packet_part2->flags & 0x01; - - if (sv_used) { - /* count SVs used for NAV */ - satellites_used++; - } - - /* record info for all channels, whether or not the SV is used for NAV */ - _gps_position->satellite_used[i] = sv_used; - _gps_position->satellite_snr[i] = packet_part2->cno; - _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); - _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - _gps_position->satellite_prn[i] = packet_part2->svid; - //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); - } - - for (i = packet_part1->numCh; i < 20; i++) { - /* unused channels have to be set to zero for e.g. MAVLink */ - _gps_position->satellite_prn[i] = 0; - _gps_position->satellite_used[i] = 0; - _gps_position->satellite_snr[i] = 0; - _gps_position->satellite_elevation[i] = 0; - _gps_position->satellite_azimuth[i] = 0; - } - - _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - - if (packet_part1->numCh > 0) { - _gps_position->satellite_info_available = true; - - } else { - _gps_position->satellite_info_available = false; - } - - _gps_position->timestamp_satellites = hrt_absolute_time(); - - ret = 1; - break; - } +// case UBX_MESSAGE_NAV_SVINFO: { +// //printf("GOT NAV_SVINFO\n"); +// const int length_part1 = 8; +// gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; +// const int length_part2 = 12; +// gps_bin_nav_svinfo_part2_packet_t *packet_part2; +// +// uint8_t satellites_used = 0; +// int i; +// +// //printf("Number of Channels: %d\n", packet_part1->numCh); +// for (i = 0; i < packet_part1->numCh; i++) { +// /* set pointer to sattelite_i information */ +// packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); +// +// /* write satellite information to global storage */ +// uint8_t sv_used = packet_part2->flags & 0x01; +// +// if (sv_used) { +// /* count SVs used for NAV */ +// satellites_used++; +// } +// +// /* record info for all channels, whether or not the SV is used for NAV */ +// _gps_position->satellite_used[i] = sv_used; +// _gps_position->satellite_snr[i] = packet_part2->cno; +// _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); +// _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); +// _gps_position->satellite_prn[i] = packet_part2->svid; +// //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); +// } +// +// for (i = packet_part1->numCh; i < 20; i++) { +// /* unused channels have to be set to zero for e.g. MAVLink */ +// _gps_position->satellite_prn[i] = 0; +// _gps_position->satellite_used[i] = 0; +// _gps_position->satellite_snr[i] = 0; +// _gps_position->satellite_elevation[i] = 0; +// _gps_position->satellite_azimuth[i] = 0; +// } +// +// _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones +// +// if (packet_part1->numCh > 0) { +// _gps_position->satellite_info_available = true; +// +// } else { +// _gps_position->satellite_info_available = false; +// } +// +// _gps_position->timestamp_satellites = hrt_absolute_time(); +// +// ret = 1; +// break; +// } case UBX_MESSAGE_NAV_VELNED: { // printf("GOT NAV_VELNED\n"); @@ -573,23 +573,23 @@ UBX::handle_message() break; } - case UBX_CLASS_MON: { - switch (_message_id) { - case UBX_MESSAGE_MON_HW: { - - struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; - - _gps_position->noise_per_ms = p->noisePerMS; - _gps_position->jamming_indicator = p->jamInd; - - ret = 1; - break; - } - - default: - break; - } - } +// case UBX_CLASS_MON: { +// switch (_message_id) { +// case UBX_MESSAGE_MON_HW: { +// +// struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; +// +// _gps_position->noise_per_ms = p->noisePerMS; +// _gps_position->jamming_indicator = p->jamInd; +// +// ret = 1; +// break; +// } +// +// default: +// break; +// } +// } default: break; From efb44969d584e1b2613d96a795fc94a3ef728d9a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 May 2014 21:57:48 +0200 Subject: [PATCH 34/67] ubx: send update only if got POSLLH & VELNED & TIMEUTC --- src/drivers/gps/ubx.cpp | 184 +++++++++++++++++++++------------------- src/drivers/gps/ubx.h | 3 + 2 files changed, 100 insertions(+), 87 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index cbfb61d00a..c143eeb0c2 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -69,6 +69,9 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : _gps_position(gps_position), _configured(false), _waiting_for_ack(false), + _got_posllh(false), + _got_velned(false), + _got_timeutc(false), _disable_cmd_last(0) { decode_init(); @@ -219,19 +222,19 @@ UBX::configure(unsigned &baudrate) return 1; } -// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); -// -// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { -// warnx("MSG CFG FAIL: NAV SVINFO"); -// return 1; -// } -// -// configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); -// -// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { -// warnx("MSG CFG FAIL: MON HW"); -// return 1; -// } + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("MSG CFG FAIL: NAV SVINFO"); + return 1; + } + + configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("MSG CFG FAIL: MON HW"); + return 1; + } _configured = true; return 0; @@ -275,9 +278,10 @@ UBX::receive(unsigned timeout) bool handled = false; while (true) { + bool ready_to_return = _configured ? (_got_posllh && _got_velned && _got_timeutc) : handled; /* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */ - int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout); + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout); if (ret < 0) { /* something went wrong when polling */ @@ -286,7 +290,10 @@ UBX::receive(unsigned timeout) } else if (ret == 0) { /* return success after short delay after receiving a packet or timeout after long delay */ - if (handled) { + if (ready_to_return) { + _got_posllh = false; + _got_velned = false; + _got_timeutc = false; return 1; } else { @@ -438,6 +445,7 @@ UBX::handle_message() _rate_count_lat_lon++; + _got_posllh = true; ret = 1; break; } @@ -482,65 +490,66 @@ UBX::handle_message() _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); _gps_position->timestamp_time = hrt_absolute_time(); + _got_timeutc = true; ret = 1; break; } -// case UBX_MESSAGE_NAV_SVINFO: { -// //printf("GOT NAV_SVINFO\n"); -// const int length_part1 = 8; -// gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; -// const int length_part2 = 12; -// gps_bin_nav_svinfo_part2_packet_t *packet_part2; -// -// uint8_t satellites_used = 0; -// int i; -// -// //printf("Number of Channels: %d\n", packet_part1->numCh); -// for (i = 0; i < packet_part1->numCh; i++) { -// /* set pointer to sattelite_i information */ -// packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); -// -// /* write satellite information to global storage */ -// uint8_t sv_used = packet_part2->flags & 0x01; -// -// if (sv_used) { -// /* count SVs used for NAV */ -// satellites_used++; -// } -// -// /* record info for all channels, whether or not the SV is used for NAV */ -// _gps_position->satellite_used[i] = sv_used; -// _gps_position->satellite_snr[i] = packet_part2->cno; -// _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); -// _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); -// _gps_position->satellite_prn[i] = packet_part2->svid; -// //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); -// } -// -// for (i = packet_part1->numCh; i < 20; i++) { -// /* unused channels have to be set to zero for e.g. MAVLink */ -// _gps_position->satellite_prn[i] = 0; -// _gps_position->satellite_used[i] = 0; -// _gps_position->satellite_snr[i] = 0; -// _gps_position->satellite_elevation[i] = 0; -// _gps_position->satellite_azimuth[i] = 0; -// } -// -// _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones -// -// if (packet_part1->numCh > 0) { -// _gps_position->satellite_info_available = true; -// -// } else { -// _gps_position->satellite_info_available = false; -// } -// -// _gps_position->timestamp_satellites = hrt_absolute_time(); -// -// ret = 1; -// break; -// } + case UBX_MESSAGE_NAV_SVINFO: { + //printf("GOT NAV_SVINFO\n"); + const int length_part1 = 8; + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; + + uint8_t satellites_used = 0; + int i; + + //printf("Number of Channels: %d\n", packet_part1->numCh); + for (i = 0; i < packet_part1->numCh; i++) { + /* set pointer to sattelite_i information */ + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); + + /* write satellite information to global storage */ + uint8_t sv_used = packet_part2->flags & 0x01; + + if (sv_used) { + /* count SVs used for NAV */ + satellites_used++; + } + + /* record info for all channels, whether or not the SV is used for NAV */ + _gps_position->satellite_used[i] = sv_used; + _gps_position->satellite_snr[i] = packet_part2->cno; + _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); + _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + _gps_position->satellite_prn[i] = packet_part2->svid; + //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); + } + + for (i = packet_part1->numCh; i < 20; i++) { + /* unused channels have to be set to zero for e.g. MAVLink */ + _gps_position->satellite_prn[i] = 0; + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; + } + + _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones + + if (packet_part1->numCh > 0) { + _gps_position->satellite_info_available = true; + + } else { + _gps_position->satellite_info_available = false; + } + + _gps_position->timestamp_satellites = hrt_absolute_time(); + + ret = 1; + break; + } case UBX_MESSAGE_NAV_VELNED: { // printf("GOT NAV_VELNED\n"); @@ -557,6 +566,7 @@ UBX::handle_message() _rate_count_vel++; + _got_velned = true; ret = 1; break; } @@ -573,23 +583,23 @@ UBX::handle_message() break; } -// case UBX_CLASS_MON: { -// switch (_message_id) { -// case UBX_MESSAGE_MON_HW: { -// -// struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; -// -// _gps_position->noise_per_ms = p->noisePerMS; -// _gps_position->jamming_indicator = p->jamInd; -// -// ret = 1; -// break; -// } -// -// default: -// break; -// } -// } + case UBX_CLASS_MON: { + switch (_message_id) { + case UBX_MESSAGE_MON_HW: { + + struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; + + _gps_position->noise_per_ms = p->noisePerMS; + _gps_position->jamming_indicator = p->jamInd; + + ret = 1; + break; + } + + default: + break; + } + } default: break; diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 5cf47b60b5..43d6888936 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -397,6 +397,9 @@ private: struct vehicle_gps_position_s *_gps_position; bool _configured; bool _waiting_for_ack; + bool _got_posllh; + bool _got_velned; + bool _got_timeutc; uint8_t _message_class_needed; uint8_t _message_id_needed; ubx_decode_state_t _decode_state; From 138edca5445a66df3647c03529ad2a30a51fe083 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 31 May 2014 13:15:10 +0200 Subject: [PATCH 35/67] attempt to fix mavlink hardware flow control disable logic --- src/modules/mavlink/mavlink_main.cpp | 32 +++++++++++++++------------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 28cdf65a3c..540b886579 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -105,7 +105,8 @@ static struct file_operations fops; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); -static uint64_t last_write_times[6] = {0}; +static uint64_t last_write_success_times[6] = {0}; +static uint64_t last_write_try_times[6] = {0}; /* * Internal function to send the bytes through the right serial port @@ -166,26 +167,25 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (instance->get_flow_control_enabled() && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { - if (buf_free == 0) { - - if (last_write_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { - - warnx("DISABLING HARDWARE FLOW CONTROL"); - instance->enable_flow_control(false); - } - - } else { - - /* apparently there is space left, although we might be - * partially overflooding the buffer already */ - last_write_times[(unsigned)channel] = hrt_absolute_time(); + /* Disable hardware flow control: + * if no successful write since a defined time + * and if the last try was not the last successful write + */ + if (last_write_try_times[(unsigned)channel] != 0 && + hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && + last_write_success_times[(unsigned)channel] != + last_write_try_times[(unsigned)channel]) + { + warnx("DISABLING HARDWARE FLOW CONTROL"); + instance->enable_flow_control(false); } + } /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ if (instance->should_transmit()) { + last_write_try_times[(unsigned)channel] = hrt_absolute_time(); /* check if there is space in the buffer, let it overflow else */ if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { @@ -199,6 +199,8 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length ssize_t ret = write(uart, ch, desired); if (ret != desired) { warnx("TX FAIL"); + } else { + last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } } From 6b8248ee29f38ae0f2ff10ebc23b1816e1fc6829 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 31 May 2014 16:19:38 +0200 Subject: [PATCH 36/67] position_estimator_inav: more safe EPH/EPV estimation, minor changes --- .../position_estimator_inav_main.c | 47 ++++++++++++------- 1 file changed, 30 insertions(+), 17 deletions(-) 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 aa533c777f..f908d7a3be 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -79,7 +79,7 @@ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; -static const hrt_abstime gps_topic_timeout = 1000000; // GPS topic timeout = 1s +static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss @@ -218,8 +218,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(R_gps, 0, sizeof(R_gps)); int buf_ptr = 0; - float eph = 1.0; - float epv = 1.0; + static const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation + static const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation + + float eph = max_eph_epv; + float epv = 1.0f; + + float eph_flow = 1.0f; float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); @@ -276,9 +281,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float corr_flow[] = { 0.0f, 0.0f }; // N E float w_flow = 0.0f; - static float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation - static float max_eph_epv = 10.0f; // max EPH/EPV acceptable for estimation - float sonar_prev = 0.0f; hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) @@ -555,9 +557,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) w_flow *= 0.05f; } - flow_valid = true; + /* under ideal conditions, on 1m distance assume EPH = 10cm */ + eph_flow = 0.1 / w_flow; - eph = fminf(eph, 0.1 / att.R[2][2] / flow_q * fmaxf(1.0f, flow_dist)); // under ideal conditions, on 1m distance assume EPH = 10cm + flow_valid = true; } else { w_flow = 0.0f; @@ -616,13 +619,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* hysteresis for GPS quality */ if (gps_valid) { - if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) { + if (gps.eph_m > max_eph_epv || gps.epv_m > max_eph_epv || gps.fix_type < 3) { gps_valid = false; mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { - if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) { + if (gps.eph_m < max_eph_epv * 0.7f && gps.epv_m < max_eph_epv * 0.7f && gps.fix_type >= 3) { gps_valid = true; reset_est = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); @@ -702,9 +705,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* save rotation matrix at this moment */ memcpy(R_gps, R_buf[est_i], sizeof(R_gps)); - eph = fminf(eph, gps.eph_m); - epv = fminf(epv, gps.epv_m); - w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m); w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m); } @@ -745,8 +745,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) t_prev = t; /* increase EPH/EPV on each step */ - eph *= 1.0 + dt; - epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift) + if (eph < max_eph_epv) { + eph *= 1.0 + dt; + } + if (epv < max_eph_epv) { + epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift) + } /* use GPS if it's valid and reference position initialized */ bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; @@ -759,7 +763,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) xy_src_time = t; } - bool can_estimate_xy = eph < max_eph_epv * 1.5; + bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); @@ -853,7 +857,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* inertial filter correction for altitude */ inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); - inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); + + if (use_gps_z) { + epv = fminf(epv, gps.epv_m); + + inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); + } if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); @@ -878,11 +887,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* inertial filter correction for position */ if (use_flow) { + eph = fminf(eph, eph_flow); + inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); } if (use_gps_xy) { + eph = fminf(eph, gps.eph_m); + inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p); inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p); From 9ee42e8e5986dcdb2e5bc9ff5110ad7bed462f98 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 10:59:11 +0200 Subject: [PATCH 37/67] rcS: fix whitespace --- ROMFS/px4fmu_common/init.d/rcS | 82 +++++++++++++++++----------------- 1 file changed, 41 insertions(+), 41 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 87ec4293ea..6d06f897a3 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -65,12 +65,12 @@ then # Start CDC/ACM serial driver # sercon - + # # Start the ORB (first app to start) # uorb start - + # # Load parameters # @@ -79,7 +79,7 @@ then then set PARAM_FILE /fs/mtd_params fi - + param select $PARAM_FILE if param load then @@ -87,7 +87,7 @@ then else echo "[init] ERROR: Params loading failed: $PARAM_FILE" fi - + # # Start system state indicator # @@ -105,7 +105,7 @@ then if pca8574 start then fi - + # # Set default values # @@ -126,7 +126,7 @@ then set LOAD_DEFAULT_APPS yes set GPS yes set GPS_FAKE no - + # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts # @@ -136,7 +136,7 @@ then else set DO_AUTOCONFIG no fi - + # # Set USE_IO flag # @@ -146,7 +146,7 @@ then else set USE_IO no fi - + # # Set parameters and env variables for selected AUTOSTART # @@ -176,9 +176,9 @@ then param set SYS_AUTOCONFIG 0 param save fi - + set IO_PRESENT no - + if [ $USE_IO == yes ] then # @@ -190,19 +190,19 @@ then else set IO_FILE /etc/extras/px4io-v1_default.bin fi - + if px4io checkcrc $IO_FILE then echo "[init] PX4IO CRC OK" echo "PX4IO CRC OK" >> $LOG_FILE - + set IO_PRESENT yes else echo "[init] Trying to update" echo "PX4IO Trying to update" >> $LOG_FILE - + tone_alarm MLL32CP8MB - + if px4io forceupdate 14662 $IO_FILE then usleep 500000 @@ -211,7 +211,7 @@ then echo "[init] PX4IO CRC OK, update successful" echo "PX4IO CRC OK after updating" >> $LOG_FILE tone_alarm MLL8CDE - + set IO_PRESENT yes else echo "[init] ERROR: PX4IO update failed" @@ -224,14 +224,14 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - + if [ $IO_PRESENT == no ] then echo "[init] ERROR: PX4IO not found" tone_alarm $TUNE_OUT_ERROR fi fi - + # # Set default output if not set # @@ -250,7 +250,7 @@ then # Need IO for output but it not present, disable output set OUTPUT_MODE none echo "[init] ERROR: PX4IO not found, disabling output" - + # Avoid using ttyS0 for MAVLink on FMUv1 if ver hwcmp PX4FMU_V1 then @@ -274,17 +274,17 @@ then # Try to get an USB console nshterm /dev/ttyACM0 & - + # # Start the Commander (needs to be this early for in-air-restarts) # commander start - + # # Start primary output # set TTYS1_BUSY no - + # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output if [ $OUTPUT_MODE != none ] then @@ -300,7 +300,7 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - + if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ] then echo "[init] Use FMU as primary output" @@ -311,7 +311,7 @@ then echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi - + if ver hwcmp PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] @@ -324,7 +324,7 @@ then fi fi fi - + if [ $OUTPUT_MODE == mkblctrl ] then echo "[init] Use MKBLCTRL as primary output" @@ -337,7 +337,7 @@ then then set MKBLCTRL_ARG "-mkmode +" fi - + if mkblctrl $MKBLCTRL_ARG then echo "[init] MKBLCTRL started" @@ -345,9 +345,9 @@ then echo "[init] ERROR: MKBLCTRL start failed" tone_alarm $TUNE_OUT_ERROR fi - + fi - + if [ $OUTPUT_MODE == hil ] then echo "[init] Use HIL as primary output" @@ -359,7 +359,7 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - + # # Start IO or FMU for RC PPM input if needed # @@ -386,7 +386,7 @@ then echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi - + if ver hwcmp PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] @@ -401,7 +401,7 @@ then fi fi fi - + # # MAVLink # @@ -422,7 +422,7 @@ then fi mavlink start $MAVLINK_FLAGS - + # # Sensors, Logging, GPS # @@ -433,7 +433,7 @@ then echo "[init] Start logging" sh /etc/init.d/rc.logging fi - + if [ $GPS == yes ] then echo "[init] Start GPS" @@ -443,7 +443,7 @@ then gps start -f else gps start - fi + fi fi # @@ -460,24 +460,24 @@ then if [ $VEHICLE_TYPE == fw ] then echo "[init] Vehicle type: FIXED WING" - + if [ $MIXER == none ] then # Set default mixer for fixed wing if not defined set MIXER FMU_AERT fi - + if [ $MAV_TYPE == none ] then # Use MAV_TYPE = 1 (fixed wing) if not defined set MAV_TYPE 1 fi - + param set MAV_TYPE $MAV_TYPE - + # Load mixer and configure outputs sh /etc/init.d/rc.interface - + # Start standard fixedwing apps if [ $LOAD_DEFAULT_APPS == yes ] then @@ -525,7 +525,7 @@ then set MAV_TYPE 14 fi fi - + # Still no MAV_TYPE found if [ $MAV_TYPE == none ] then @@ -533,10 +533,10 @@ then else param set MAV_TYPE $MAV_TYPE fi - + # Load mixer and configure outputs sh /etc/init.d/rc.interface - + # Start standard multicopter apps if [ $LOAD_DEFAULT_APPS == yes ] then From 0c35b7a8ee5305b8cc4c2dda69222f137f3f3593 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Jun 2014 11:50:14 +0200 Subject: [PATCH 38/67] Fixed EKF initial param values --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index b6b0a5b4ed..97644d1b9a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -37,7 +37,7 @@ then param set MPC_LAND_SPEED 1.0 param set PE_VELNE_NOISE 0.5 - param set PE_VELNE_NOISE 0.7 + param set PE_VELD_NOISE 0.7 param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.0 From 480c41d83591354480a8fd81cfbc314fccaa6a20 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 13:18:56 +0200 Subject: [PATCH 39/67] do not read out home position in gcs (home position is still displayed) --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c0628a166..65922b2a5d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1408,7 +1408,7 @@ int commander_thread_main(int argc, char *argv[]) home.alt = global_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { From 794b853a3b94b5520b734933f18823885259427d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 14:04:16 +0200 Subject: [PATCH 40/67] mavlink: for takeoff mission items don't set the time inside field. Correctly set pitch min when converting from mission item to mavlink mission item --- src/modules/mavlink/mavlink_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 540b886579..304a02c291 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -896,6 +896,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item default: mission_item->acceptance_radius = mavlink_mission_item->param2; + mission_item->time_inside = mavlink_mission_item->param1; break; } @@ -904,7 +905,6 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; - mission_item->time_inside = mavlink_mission_item->param1; mission_item->autocontinue = mavlink_mission_item->autocontinue; // mission_item->index = mavlink_mission_item->seq; mission_item->origin = ORIGIN_MAVLINK; @@ -923,11 +923,12 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ switch (mission_item->nav_cmd) { case NAV_CMD_TAKEOFF: - mavlink_mission_item->param2 = mission_item->pitch_min; + mavlink_mission_item->param1 = mission_item->pitch_min; break; default: mavlink_mission_item->param2 = mission_item->acceptance_radius; + mavlink_mission_item->param1 = mission_item->time_inside; break; } @@ -938,7 +939,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; mavlink_mission_item->command = mission_item->nav_cmd; - mavlink_mission_item->param1 = mission_item->time_inside; mavlink_mission_item->autocontinue = mission_item->autocontinue; // mavlink_mission_item->seq = mission_item->index; From ad811304c89d00d6432ec871916b7a82c8b3cbe7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Jun 2014 15:36:26 +0200 Subject: [PATCH 41/67] actuator_armed: Fixed comments and doxygen, no C-level changes --- src/modules/uORB/topics/actuator_armed.h | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h index 6e944ffee3..a98d3fc3a4 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/modules/uORB/topics/actuator_armed.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,15 +44,25 @@ #include #include "../uORB.h" +/** + * @addtogroup topics + * @{ + */ + /** global 'actuator output is live' control. */ struct actuator_armed_s { - uint64_t timestamp; - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ + uint64_t timestamp; /**< Microseconds since system boot */ + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ }; +/** + * @} + */ + +/* register this as object request broker structure */ ORB_DECLARE(actuator_armed); #endif \ No newline at end of file From 325d5026bb6d36ab3557ab999f8db772f77ac7a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Jun 2014 13:37:31 +0200 Subject: [PATCH 42/67] Hotfix: Fix scaling for battery current --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 933478f560..fd41b723ab 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -274,7 +274,7 @@ protected: status->onboard_control_sensors_health, status->load * 1000.0f, status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, + status->battery_current * 100.0f, status->battery_remaining * 100.0f, status->drop_rate_comm, status->errors_comm, From 5624c1406aa78aa4bf4b5c0e20dca637c26478d5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Jun 2014 16:43:15 +0200 Subject: [PATCH 43/67] Hotfix: Better microSD reporting --- src/modules/mavlink/mavlink_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 304a02c291..264cfd875a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1054,6 +1054,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t } else { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD"); if (_verbose) { warnx("ERROR: could not read WP%u", seq); } } @@ -1440,6 +1441,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } From 33067373614e50e3be068d30f3ad3b718d16df5f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 6 Jun 2014 00:42:02 +0200 Subject: [PATCH 44/67] mavlink: send MISSION_REQUEST after short timeout when receiving mission, remove all "target id mismatch" warnings --- src/modules/mavlink/mavlink_main.cpp | 51 ++++++++-------------------- src/modules/mavlink/mavlink_main.h | 1 + 2 files changed, 15 insertions(+), 37 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 264cfd875a..e300be0747 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -790,9 +790,14 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { - /* Start sending parameters */ - mavlink_pm_start_queued_send(); - mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + mavlink_param_request_list_t req; + mavlink_msg_param_request_list_decode(msg, &req); + if (req.target_system == mavlink_system.sysid && + (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { + /* Start sending parameters */ + mavlink_pm_start_queued_send(); + mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + } } break; case MAVLINK_MSG_ID_PARAM_SET: { @@ -954,6 +959,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->current_partner_compid = 0; state->timestamp_lastaction = 0; state->timestamp_last_send_setpoint = 0; + state->timestamp_last_send_request = 0; state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->current_dataman_id = 0; } @@ -1070,6 +1076,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u wpr.seq = seq; mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr); mavlink_missionlib_send_message(&msg); + _wpm->timestamp_last_send_request = hrt_absolute_time(); if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } @@ -1112,6 +1119,10 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) _wpm->current_state = MAVLINK_WPM_STATE_IDLE; _wpm->current_partner_sysid = 0; _wpm->current_partner_compid = 0; + + } else if (now - _wpm->timestamp_last_send_request > 500000 && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + /* try to get WP again after short timeout */ + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } @@ -1174,11 +1185,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN WP CURR CMD: Busy"); } } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); } } break; @@ -1211,11 +1217,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN REQUEST LIST: Busy"); } } - - } else { - mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); - - if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); } } break; @@ -1304,12 +1305,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); } - - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } @@ -1368,12 +1363,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); } } - - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } break; @@ -1473,11 +1462,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1513,13 +1497,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); } } - - - } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - - mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 25c0da8206..40edc4b851 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -93,6 +93,7 @@ struct mavlink_wpm_storage { uint8_t current_partner_compid; uint64_t timestamp_lastaction; uint64_t timestamp_last_send_setpoint; + uint64_t timestamp_last_send_request; uint32_t timeout; int current_dataman_id; }; From c4e2232078f2f28cbf97b8280f40fd988a5c2a75 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 6 Jun 2014 08:13:05 +0200 Subject: [PATCH 45/67] Remove unused loiter radius parameter. Fixes #1042 --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 ------ .../fw_pos_control_l1/fw_pos_control_l1_params.c | 11 ----------- 2 files changed, 17 deletions(-) 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 5b877cd77d..116d3cc63d 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 @@ -232,8 +232,6 @@ private: float throttle_land_max; - float loiter_hold_radius; - float heightrate_p; float speedrate_p; @@ -277,8 +275,6 @@ private: param_t throttle_land_max; - param_t loiter_hold_radius; - param_t heightrate_p; param_t speedrate_p; @@ -441,7 +437,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); _parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); - _parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R"); _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); @@ -513,7 +508,6 @@ FixedwingPositionControl::parameters_update() /* L1 control parameters */ param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping)); param_get(_parameter_handles.l1_period, &(_parameters.l1_period)); - param_get(_parameter_handles.loiter_hold_radius, &(_parameters.loiter_hold_radius)); param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); 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 f258f77dab..52128e1b7a 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 @@ -71,17 +71,6 @@ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f); */ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); -/** - * Default Loiter Radius - * - * This radius is used when no other loiter radius is set. - * - * @min 10.0 - * @max 100.0 - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f); - /** * Cruise throttle * From 85b2dfa0c662298d49f584e6bd774954b04cec35 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Jun 2014 12:30:27 +0200 Subject: [PATCH 46/67] fix initialization of perfcounters in fw att controllers --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 4 ++-- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 4 ++-- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 0a909d02f5..a0a18bc2e4 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -61,9 +61,9 @@ ECL_PitchController::ECL_PitchController() : _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f) + _bodyrate_setpoint(0.0f), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input")) { - perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"); } ECL_PitchController::~ECL_PitchController() diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 82903ef5aa..d2a231694d 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -59,9 +59,9 @@ ECL_RollController::ECL_RollController() : _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f) + _bodyrate_setpoint(0.0f), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")) { - perf_alloc(PC_COUNT, "fw att control roll nonfinite input"); } ECL_RollController::~ECL_RollController() diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index e53ffc6448..79184e2cdd 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -58,9 +58,9 @@ ECL_YawController::ECL_YawController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _coordinated_min_speed(1.0f) + _coordinated_min_speed(1.0f), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input")) { - perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"); } ECL_YawController::~ECL_YawController() From 128389d3b14a43d48712b2bb5e32cac85b5dafcc Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Mon, 9 Jun 2014 16:01:44 -0700 Subject: [PATCH 47/67] Converted style to work with wiki. Cleaned up bad fields. --- .../fw_att_control/fw_att_control_params.c | 367 +++++++++++++----- 1 file changed, 270 insertions(+), 97 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index aa637e2074..7cae846785 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -50,149 +50,322 @@ f * Copyright (c) 2013 PX4 Development Team. All rights reserved. * Controller parameters, accessible via MAVLink * */ -// @DisplayName Attitude Time Constant -// @Description This defines the latency between a step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed. -// @Range 0.4 to 1.0 seconds, in tens of seconds + +/** + * Attitude Time Constant + * + * This defines the latency between a step input and the achieved setpoint + * (inverse to a P gain). Half a second is a good start value and fits for + * most average systems. Smaller systems may require smaller values, but as + * this will wear out servos faster, the value should only be decreased as + * needed. + * + * @unit seconds + * @min 0.4 + * @max 1.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); -// @DisplayName Pitch rate proportional gain. -// @Description This defines how much the elevator input will be commanded depending on the current body angular rate error. -// @Range 10 to 200, 1 increments +/** + * Pitch rate proportional gain. + * + * This defines how much the elevator input will be commanded depending on the + * current body angular rate error. + * + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f); -// @DisplayName Pitch rate integrator gain. -// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. -// @Range 0 to 50.0 +/** + * Pitch rate integrator gain. + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @min 0.0 + * @max 50.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f); -// @DisplayName Maximum positive / up pitch rate. -// @Description This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. -// @Range 0 to 90.0 degrees per seconds, in 1 increments +/** + * Maximum positive / up pitch rate. + * + * This limits the maximum pitch up angular rate the controller will output (in + * degrees per second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f); -// @DisplayName Maximum negative / down pitch rate. -// @Description This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. -// @Range 0 to 90.0 degrees per seconds, in 1 increments +/** + * Maximum negative / down pitch rate. + * + * This limits the maximum pitch down up angular rate the controller will + * output (in degrees per second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); -// @DisplayName Pitch rate integrator limit -// @Description The portion of the integrator part in the control surface deflection is limited to this value -// @Range 0.0 to 1 -// @Increment 0.1 +/** + * Pitch rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f); -// @DisplayName Roll to Pitch feedforward gain. -// @Description This compensates during turns and ensures the nose stays level. -// @Range 0.5 2.0 -// @Increment 0.05 -// @User User +/** + * Roll to Pitch feedforward gain. + * + * This compensates during turns and ensures the nose stays level. + * + * @min 0.0 + * @max 2.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...) -// @DisplayName Roll rate proportional Gain. -// @Description This defines how much the aileron input will be commanded depending on the current body angular rate error. -// @Range 10.0 200.0 -// @Increment 10.0 -// @User User +/** + * Roll rate proportional Gain + * + * This defines how much the aileron input will be commanded depending on the + * current body angular rate error. + * + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); -// @DisplayName Roll rate integrator Gain -// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. -// @Range 0.0 100.0 -// @Increment 5.0 -// @User User +/** + * Roll rate integrator Gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @min 0.0 + * @max 100.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f); -// @DisplayName Roll Integrator Anti-Windup -// @Description The portion of the integrator part in the control surface deflection is limited to this value. -// @Range 0.0 to 1.0 -// @Increment 0.1 +/** + * Roll Integrator Anti-Windup + * + * The portion of the integrator part in the control surface deflection is limited to this value. + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f); -// @DisplayName Maximum Roll Rate -// @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit. -// @Range 0 to 90.0 degrees per seconds -// @Increment 1.0 -PARAM_DEFINE_FLOAT(FW_R_RMAX, 0); +/** + * Maximum Roll Rate + * + * This limits the maximum roll rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f); -// @DisplayName Yaw rate proportional gain. -// @Description This defines how much the rudder input will be commanded depending on the current body angular rate error. -// @Range 10 to 200, 1 increments -PARAM_DEFINE_FLOAT(FW_YR_P, 0.05); +/** + * Yaw rate proportional gain + * + * This defines how much the rudder input will be commanded depending on the + * current body angular rate error. + * + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f); -// @DisplayName Yaw rate integrator gain. -// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. -// @Range 0 to 50.0 +/** + * Yaw rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @min 0.0 + * @max 50.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f); -// @DisplayName Yaw rate integrator limit -// @Description The portion of the integrator part in the control surface deflection is limited to this value -// @Range 0.0 to 1 -// @Increment 0.1 +/** + * Yaw rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); -// @DisplayName Maximum Yaw Rate -// @Description This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit. -// @Range 0 to 90.0 degrees per seconds -// @Increment 1.0 -PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0); +/** + * Maximum Yaw Rate + * + * This limits the maximum yaw rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); -// @DisplayName Roll rate feed forward -// @Description Direct feed forward from rate setpoint to control surface output -// @Range 0 to 10 -// @Increment 0.1 +/** + * Roll rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @min 0.0 + * @max 10.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f); -// @DisplayName Pitch rate feed forward -// @Description Direct feed forward from rate setpoint to control surface output -// @Range 0 to 10 -// @Increment 0.1 +/** + * Pitch rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @min 0.0 + * @max 10.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f); -// @DisplayName Yaw rate feed forward -// @Description Direct feed forward from rate setpoint to control surface output -// @Range 0 to 10 -// @Increment 0.1 +/** + * Yaw rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @min 0.0 + * @max 10.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); -// @DisplayName Minimal speed for yaw coordination -// @Description For airspeeds above this value the yaw rate is calculated for a coordinated turn. Set to a very high value to disable. -// @Range 0 to 90.0 degrees per seconds -// @Increment 1.0 +/** + * Minimal speed for yaw coordination + * + * For airspeeds above this value, the yaw rate is calculated for a coordinated + * turn. Set to a very high value to disable. + * + * @unit m/s + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f); -/* Airspeed parameters: the following parameters about airspeed are used by the attitude and the positon controller */ +/* Airspeed parameters: + * The following parameters about airspeed are used by the attitude and the + * position controller. + * */ -// @DisplayName Minimum Airspeed -// @Description If the airspeed falls below this value the TECS controller will try to increase airspeed more aggressively -// @Range 0.0 to 30 +/** + * Minimum Airspeed + * + * If the airspeed falls below this value, the TECS controller will try to + * increase airspeed more aggressively. + * + * @unit m/s + * @min 0.0 + * @max 30.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f); -// @DisplayName Trim Airspeed -// @Description The TECS controller tries to fly at this airspeed -// @Range 0.0 to 30 +/** + * Trim Airspeed + * + * The TECS controller tries to fly at this airspeed. + * + * @unit m/s + * @min 0.0 + * @max 30.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f); -// @DisplayName Maximum Airspeed -// @Description If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively -// @Range 0.0 to 30 +/** + * Maximum Airspeed + * + * If the airspeed is above this value, the TECS controller will try to decrease + * airspeed more aggressively. + * + * @unit m/s + * @min 0.0 + * @max 30.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f); -// @DisplayName Roll Setpoint Offset -// @Description An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe -// @Range -90.0 to 90.0 +/** + * Roll Setpoint Offset + * + * An airframe specific offset of the roll setpoint in degrees, the value is + * added to the roll setpoint and should correspond to the typical cruise speed + * of the airframe. + * + * @unit deg + * @min -90.0 + * @max 90.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f); -// @DisplayName Pitch Setpoint Offset -// @Description An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe -// @Range -90.0 to 90.0 +/** + * Pitch Setpoint Offset + * + * An airframe specific offset of the pitch setpoint in degrees, the value is + * added to the pitch setpoint and should correspond to the typical cruise + * speed of the airframe. + * + * @unit deg + * @min -90.0 + * @max 90.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); -// @DisplayName Max Manual Roll -// @Description Max roll for manual control in attitude stabilized mode -// @Range 0.0 to 90.0 +/** + * Max Manual Roll + * + * Max roll for manual control in attitude stabilized mode + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); -// @DisplayName Max Manual Pitch -// @Description Max pitch for manual control in attitude stabilized mode -// @Range 0.0 to 90.0 +/** + * Max Manual Pitch + * + * Max pitch for manual control in attitude stabilized mode + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); From d5c0933d6516741f432a8f259149384fa2a2f95b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 10 Jun 2014 14:29:17 +0200 Subject: [PATCH 48/67] mavlink: report global position setpoint and do this always no just when updated, otherwise the values are not visible in QGC --- ROMFS/px4fmu_common/init.d/rc.usb | 2 ++ src/modules/mavlink/mavlink_messages.cpp | 14 ++++++-------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 76593881db..d6e638c0ab 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -17,6 +17,8 @@ mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30 usleep 100000 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 usleep 100000 +mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20 +usleep 100000 # Exit shell to make it available to MAVLink exit diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fd41b723ab..c44bdc53dd 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -938,14 +938,12 @@ protected: void send(const hrt_abstime t) { - if (pos_sp_triplet_sub->update(t)) { - mavlink_msg_global_position_setpoint_int_send(_channel, - MAV_FRAME_GLOBAL, - (int32_t)(pos_sp_triplet->current.lat * 1e7), - (int32_t)(pos_sp_triplet->current.lon * 1e7), - (int32_t)(pos_sp_triplet->current.alt * 1000), - (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); - } + mavlink_msg_global_position_setpoint_int_send(_channel, + MAV_FRAME_GLOBAL, + (int32_t)(pos_sp_triplet->current.lat * 1e7), + (int32_t)(pos_sp_triplet->current.lon * 1e7), + (int32_t)(pos_sp_triplet->current.alt * 1000), + (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); } }; From 064a75a3c289a32fa4d5234e3874712e7981f238 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 10 Jun 2014 15:02:20 +0200 Subject: [PATCH 49/67] mavlink: put update call back in --- src/modules/mavlink/mavlink_messages.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c44bdc53dd..d94e7fc7e5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -938,6 +938,8 @@ protected: void send(const hrt_abstime t) { + /* always send this message, even if it has not been updated */ + pos_sp_triplet_sub->update(t); mavlink_msg_global_position_setpoint_int_send(_channel, MAV_FRAME_GLOBAL, (int32_t)(pos_sp_triplet->current.lat * 1e7), From 342e08977ae5bf49c5ba941866e44fddefca4cda Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 11 Jun 2014 14:00:44 +0200 Subject: [PATCH 50/67] MavlinkOrbSubscription API reworked --- src/modules/mavlink/mavlink_commands.cpp | 8 +- src/modules/mavlink/mavlink_commands.h | 2 +- src/modules/mavlink/mavlink_main.cpp | 8 +- src/modules/mavlink/mavlink_messages.cpp | 311 +++++++++++------- .../mavlink/mavlink_orb_subscription.cpp | 44 ++- .../mavlink/mavlink_orb_subscription.h | 18 +- 6 files changed, 251 insertions(+), 140 deletions(-) diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp index 5760d75121..fccd4d9a59 100644 --- a/src/modules/mavlink/mavlink_commands.cpp +++ b/src/modules/mavlink/mavlink_commands.cpp @@ -40,21 +40,17 @@ #include "mavlink_commands.h" -MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel) +MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0) { _cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); } -MavlinkCommandsStream::~MavlinkCommandsStream() -{ -} - void MavlinkCommandsStream::update(const hrt_abstime t) { struct vehicle_command_s cmd; - if (_cmd_sub->update(t, &cmd)) { + if (_cmd_sub->update(&_cmd_time, &cmd)) { /* only send commands for other systems/components */ if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { mavlink_msg_command_long_send(_channel, diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_commands.h index 6255d65dff..abdba34b98 100644 --- a/src/modules/mavlink/mavlink_commands.h +++ b/src/modules/mavlink/mavlink_commands.h @@ -55,10 +55,10 @@ private: MavlinkOrbSubscription *_cmd_sub; struct vehicle_command_s *_cmd; mavlink_channel_t _channel; + uint64_t _cmd_time; public: MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel); - ~MavlinkCommandsStream(); void update(const hrt_abstime t); }; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 39610fdd83..046f45bd9c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1900,10 +1900,12 @@ Mavlink::task_main(int argc, char *argv[]) _task_running = true; MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); + uint64_t param_time = 0; MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); + uint64_t status_time = 0; struct vehicle_status_s status; - status_sub->update(0, &status); + status_sub->update(&status_time, &status); MavlinkCommandsStream commands_stream(this, _channel); @@ -1960,12 +1962,12 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); - if (param_sub->update(t, nullptr)) { + if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ mavlink_update_system(); } - if (status_sub->update(t, &status)) { + if (status_sub->update(&status_time, &status)) { /* switch HIL mode if required */ set_hil_enabled(status.hil_state == HIL_STATE_ON); } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 184726fe85..0430987ec1 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -195,9 +195,6 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set class MavlinkStreamHeartbeat : public MavlinkStream { public: - - ~MavlinkStreamHeartbeat() {}; - const char *get_name() const { return MavlinkStreamHeartbeat::get_name_static(); @@ -216,12 +213,8 @@ public: private: MavlinkOrbSubscription *status_sub; MavlinkOrbSubscription *pos_sp_triplet_sub; - protected: - - explicit MavlinkStreamHeartbeat() {}; - void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); @@ -233,21 +226,19 @@ protected: struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; - (void)status_sub->update(t, &status); - (void)pos_sp_triplet_sub->update(t, &pos_sp_triplet); - - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - mavlink_msg_heartbeat_send(_channel, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); + if (status_sub->update(&status) && pos_sp_triplet_sub->update(&pos_sp_triplet)) { + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + mavlink_msg_heartbeat_send(_channel, + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_base_mode, + mavlink_custom_mode, + mavlink_state); + } } }; @@ -255,8 +246,6 @@ protected: class MavlinkStreamSysStatus : public MavlinkStream { public: - ~MavlinkStreamSysStatus() {}; - const char *get_name() const { return MavlinkStreamSysStatus::get_name_static(); @@ -274,11 +263,8 @@ public: private: MavlinkOrbSubscription *status_sub; - struct vehicle_status_s *status; protected: - explicit MavlinkStreamSysStatus() {}; - void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); @@ -287,21 +273,23 @@ protected: void send(const hrt_abstime t) { struct vehicle_status_s status; - (void)status_sub->update(t, &status); - mavlink_msg_sys_status_send(_channel, - status.onboard_control_sensors_present, - status.onboard_control_sensors_enabled, - status.onboard_control_sensors_health, - status.load * 1000.0f, - status.battery_voltage * 1000.0f, - status.battery_current * 100.0f, - status.battery_remaining * 100.0f, - status.drop_rate_comm, - status.errors_comm, - status.errors_count1, - status.errors_count2, - status.errors_count3, - status.errors_count4); + + if (status_sub->update(&status)) { + mavlink_msg_sys_status_send(_channel, + status.onboard_control_sensors_present, + status.onboard_control_sensors_enabled, + status.onboard_control_sensors_health, + status.load * 1000.0f, + status.battery_voltage * 1000.0f, + status.battery_current * 100.0f, + status.battery_remaining * 100.0f, + status.drop_rate_comm, + status.errors_comm, + status.errors_count1, + status.errors_count2, + status.errors_count3, + status.errors_count4); + } } }; @@ -309,8 +297,6 @@ protected: class MavlinkStreamHighresIMU : public MavlinkStream { public: - ~MavlinkStreamHighresIMU() {}; - const char *get_name() const { return MavlinkStreamHighresIMU::get_name_static(); @@ -328,6 +314,7 @@ public: private: MavlinkOrbSubscription *sensor_sub; + uint64_t sensor_time; uint64_t accel_timestamp; uint64_t gyro_timestamp; @@ -335,9 +322,13 @@ private: uint64_t baro_timestamp; protected: - explicit MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0) - { - } + explicit MavlinkStreamHighresIMU() : MavlinkStream(), + sensor_time(0), + accel_timestamp(0), + gyro_timestamp(0), + mag_timestamp(0), + baro_timestamp(0) + {} void subscribe(Mavlink *mavlink) { @@ -347,7 +338,8 @@ protected: void send(const hrt_abstime t) { struct sensor_combined_s sensor; - if (sensor_sub->update(t, &sensor)) { + + if (sensor_sub->update(&sensor_time, &sensor)) { uint16_t fields_updated = 0; if (accel_timestamp != sensor.accelerometer_timestamp) { @@ -407,9 +399,13 @@ public: private: MavlinkOrbSubscription *att_sub; - struct vehicle_attitude_s *att; + uint64_t att_time; protected: + explicit MavlinkStreamAttitude() : MavlinkStream(), + att_time(0) + {} + void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); @@ -419,7 +415,7 @@ protected: { struct vehicle_attitude_s att; - if (att_sub->update(t, &att)) { + if (att_sub->update(&att_time, &att)) { mavlink_msg_attitude_send(_channel, att.timestamp / 1000, att.roll, att.pitch, att.yaw, @@ -449,8 +445,13 @@ public: private: MavlinkOrbSubscription *att_sub; + uint64_t att_time; protected: + explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), + att_time(0) + {} + void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); @@ -460,7 +461,7 @@ protected: { struct vehicle_attitude_s att; - if (att_sub->update(t, &att)) { + if (att_sub->update(&att_time, &att)) { mavlink_msg_attitude_quaternion_send(_channel, att.timestamp / 1000, att.q[0], @@ -496,21 +497,29 @@ public: private: MavlinkOrbSubscription *att_sub; - struct vehicle_attitude_s *att; + uint64_t att_time; MavlinkOrbSubscription *pos_sub; - struct vehicle_global_position_s *pos; + uint64_t pos_time; MavlinkOrbSubscription *armed_sub; - struct actuator_armed_s *armed; + uint64_t armed_time; MavlinkOrbSubscription *act_sub; - struct actuator_controls_s *act; + uint64_t act_time; MavlinkOrbSubscription *airspeed_sub; - struct airspeed_s *airspeed; + uint64_t airspeed_time; protected: + explicit MavlinkStreamVFRHUD() : MavlinkStream(), + att_time(0), + pos_time(0), + armed_time(0), + act_time(0), + airspeed_time(0) + {} + void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); @@ -528,11 +537,11 @@ protected: struct actuator_controls_s act; struct airspeed_s airspeed; - bool updated = att_sub->update(t, &att); - updated |= pos_sub->update(t, &pos); - updated |= armed_sub->update(t, &armed); - updated |= act_sub->update(t, &act); - updated |= airspeed_sub->update(t, &airspeed); + bool updated = att_sub->update(&att_time, &att); + updated |= pos_sub->update(&pos_time, &pos); + updated |= armed_sub->update(&armed_time, &armed); + updated |= act_sub->update(&act_time, &act); + updated |= airspeed_sub->update(&airspeed_time, &airspeed); if (updated) { float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); @@ -571,9 +580,13 @@ public: private: MavlinkOrbSubscription *gps_sub; - struct vehicle_gps_position_s *gps; + uint64_t gps_time; protected: + explicit MavlinkStreamGPSRawInt() : MavlinkStream(), + gps_time(0) + {} + void subscribe(Mavlink *mavlink) { gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); @@ -583,7 +596,7 @@ protected: { struct vehicle_gps_position_s gps; - if (gps_sub->update(t, &gps)) { + if (gps_sub->update(&gps_time, &gps)) { mavlink_msg_gps_raw_int_send(_channel, gps.timestamp_position, gps.fix_type, @@ -620,9 +633,17 @@ public: private: MavlinkOrbSubscription *pos_sub; + uint64_t pos_time; + MavlinkOrbSubscription *home_sub; + uint64_t home_time; protected: + explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), + pos_time(0), + home_time(0) + {} + void subscribe(Mavlink *mavlink) { pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); @@ -634,8 +655,8 @@ protected: struct vehicle_global_position_s pos; struct home_position_s home; - bool updated = pos_sub->update(t, &pos); - updated |= home_sub->update(t, &home); + bool updated = pos_sub->update(&pos_time, &pos); + updated |= home_sub->update(&home_time, &home); if (updated) { mavlink_msg_global_position_int_send(_channel, @@ -673,8 +694,13 @@ public: private: MavlinkOrbSubscription *pos_sub; + uint64_t pos_time; protected: + explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), + pos_time(0) + {} + void subscribe(Mavlink *mavlink) { pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); @@ -684,7 +710,7 @@ protected: { struct vehicle_local_position_s pos; - if (pos_sub->update(t, &pos)) { + if (pos_sub->update(&pos_time, &pos)) { mavlink_msg_local_position_ned_send(_channel, pos.timestamp / 1000, pos.x, @@ -719,8 +745,13 @@ public: private: MavlinkOrbSubscription *pos_sub; + uint64_t pos_time; protected: + explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), + pos_time(0) + {} + void subscribe(Mavlink *mavlink) { pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); @@ -730,7 +761,7 @@ protected: { struct vehicle_vicon_position_s pos; - if (pos_sub->update(t, &pos)) { + if (pos_sub->update(&pos_time, &pos)) { mavlink_msg_vicon_position_estimate_send(_channel, pos.timestamp / 1000, pos.x, @@ -777,38 +808,29 @@ protected: * the GCS does pick it up at one point */ if (home_sub->is_published()) { struct home_position_s home; - home_sub->update(t, &home); - mavlink_msg_gps_global_origin_send(_channel, - (int32_t)(home.lat * 1e7), - (int32_t)(home.lon * 1e7), - (int32_t)(home.alt) * 1000.0f); + if (home_sub->update(&home)) { + mavlink_msg_gps_global_origin_send(_channel, + (int32_t)(home.lat * 1e7), + (int32_t)(home.lon * 1e7), + (int32_t)(home.alt) * 1000.0f); + } } } }; - +template class MavlinkStreamServoOutputRaw : public MavlinkStream { public: - MavlinkStreamServoOutputRaw() : MavlinkStream() - { - _instance = _n++; - } - const char *get_name() const { - return get_name_static_int(_instance); + return MavlinkStreamServoOutputRaw::get_name_static(); } static const char *get_name_static() { - return get_name_static_int(_n); - } - - static const char *get_name_static_int(unsigned n) - { - switch (n) { + switch (N) { case 0: return "SERVO_OUTPUT_RAW_0"; @@ -825,17 +847,18 @@ public: static MavlinkStream *new_instance() { - return new MavlinkStreamServoOutputRaw(); + return new MavlinkStreamServoOutputRaw(); } - static unsigned _n; - private: - MavlinkOrbSubscription *_act_sub; - - unsigned _instance; + MavlinkOrbSubscription *act_sub; + uint64_t act_time; protected: + explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), + act_time(0) + {} + void subscribe(Mavlink *mavlink) { orb_id_t act_topics[] = { @@ -845,17 +868,17 @@ protected: ORB_ID(actuator_outputs_3) }; - _act_sub = mavlink->add_orb_subscription(act_topics[_instance]); + act_sub = mavlink->add_orb_subscription(act_topics[N]); } void send(const hrt_abstime t) { struct actuator_outputs_s act; - if (_act_sub->update(t, &act)) { + if (act_sub->update(&act_time, &act)) { mavlink_msg_servo_output_raw_send(_channel, act.timestamp / 1000, - _instance, + N, act.output[0], act.output[1], act.output[2], @@ -889,10 +912,21 @@ public: private: MavlinkOrbSubscription *status_sub; + uint64_t status_time; + MavlinkOrbSubscription *pos_sp_triplet_sub; + uint64_t pos_sp_triplet_time; + MavlinkOrbSubscription *act_sub; + uint64_t act_time; protected: + explicit MavlinkStreamHILControls() : MavlinkStream(), + status_time(0), + pos_sp_triplet_time(0), + act_time(0) + {} + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); @@ -906,9 +940,9 @@ protected: struct position_setpoint_triplet_s pos_sp_triplet; struct actuator_outputs_s act; - bool updated = act_sub->update(t, &act); - (void)pos_sp_triplet_sub->update(t, &pos_sp_triplet); - (void)status_sub->update(t, &status); + bool updated = act_sub->update(&act_time, &act); + updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet); + updated |= status_sub->update(&status_time, &status); if (updated && (status.arming_state == ARMING_STATE_ARMED)) { /* translate the current syste state to mavlink state and mode */ @@ -1015,8 +1049,13 @@ public: private: MavlinkOrbSubscription *pos_sp_triplet_sub; + uint64_t pos_sp_triplet_time; protected: + explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), + pos_sp_triplet_time(0) + {} + void subscribe(Mavlink *mavlink) { pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); @@ -1025,7 +1064,8 @@ protected: void send(const hrt_abstime t) { struct position_setpoint_triplet_s pos_sp_triplet; - if (pos_sp_triplet_sub->update(t, &pos_sp_triplet)) { + + if (pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet)) { mavlink_msg_global_position_setpoint_int_send(_channel, MAV_FRAME_GLOBAL, (int32_t)(pos_sp_triplet.current.lat * 1e7), @@ -1057,8 +1097,13 @@ public: private: MavlinkOrbSubscription *pos_sp_sub; + uint64_t pos_sp_time; protected: + explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), + pos_sp_time(0) + {} + void subscribe(Mavlink *mavlink) { pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); @@ -1067,7 +1112,8 @@ protected: void send(const hrt_abstime t) { struct vehicle_local_position_setpoint_s pos_sp; - if (pos_sp_sub->update(t, &pos_sp)) { + + if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) { mavlink_msg_local_position_setpoint_send(_channel, MAV_FRAME_LOCAL_NED, pos_sp.x, @@ -1099,8 +1145,13 @@ public: private: MavlinkOrbSubscription *att_sp_sub; + uint64_t att_sp_time; protected: + explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), + att_sp_time(0) + {} + void subscribe(Mavlink *mavlink) { att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); @@ -1109,7 +1160,8 @@ protected: void send(const hrt_abstime t) { struct vehicle_attitude_setpoint_s att_sp; - if (att_sp_sub->update(t, &att_sp)) { + + if (att_sp_sub->update(&att_sp_time, &att_sp)) { mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, att_sp.timestamp / 1000, att_sp.roll_body, @@ -1141,8 +1193,13 @@ public: private: MavlinkOrbSubscription *att_rates_sp_sub; + uint64_t att_rates_sp_time; protected: + explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), + att_rates_sp_time(0) + {} + void subscribe(Mavlink *mavlink) { att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); @@ -1151,7 +1208,8 @@ protected: void send(const hrt_abstime t) { struct vehicle_rates_setpoint_s att_rates_sp; - if (att_rates_sp_sub->update(t, &att_rates_sp)) { + + if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) { mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, att_rates_sp.timestamp / 1000, att_rates_sp.roll, @@ -1183,8 +1241,13 @@ public: private: MavlinkOrbSubscription *rc_sub; + uint64_t rc_time; protected: + explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), + rc_time(0) + {} + void subscribe(Mavlink *mavlink) { rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); @@ -1194,7 +1257,7 @@ protected: { struct rc_input_values rc; - if (rc_sub->update(t, &rc)) { + if (rc_sub->update(&rc_time, &rc)) { const unsigned port_width = 8; // Deprecated message (but still needed for compatibility!) @@ -1262,8 +1325,13 @@ public: private: MavlinkOrbSubscription *manual_sub; + uint64_t manual_time; protected: + explicit MavlinkStreamManualControl() : MavlinkStream(), + manual_time(0) + {} + void subscribe(Mavlink *mavlink) { manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); @@ -1273,7 +1341,7 @@ protected: { struct manual_control_setpoint_s manual; - if (manual_sub->update(t, &manual)) { + if (manual_sub->update(&manual_time, &manual)) { mavlink_msg_manual_control_send(_channel, mavlink_system.sysid, manual.x * 1000, @@ -1306,8 +1374,13 @@ public: private: MavlinkOrbSubscription *flow_sub; + uint64_t flow_time; protected: + explicit MavlinkStreamOpticalFlow() : MavlinkStream(), + flow_time(0) + {} + void subscribe(Mavlink *mavlink) { flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); @@ -1317,7 +1390,7 @@ protected: { struct optical_flow_s flow; - if (flow_sub->update(t, &flow)) { + if (flow_sub->update(&flow_time, &flow)) { mavlink_msg_optical_flow_send(_channel, flow.timestamp, flow.sensor_id, @@ -1349,8 +1422,13 @@ public: private: MavlinkOrbSubscription *att_ctrl_sub; + uint64_t att_ctrl_time; protected: + explicit MavlinkStreamAttitudeControls() : MavlinkStream(), + att_ctrl_time(0) + {} + void subscribe(Mavlink *mavlink) { att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); @@ -1360,7 +1438,7 @@ protected: { struct actuator_controls_s att_ctrl; - if (att_ctrl_sub->update(t, &att_ctrl)) { + if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(_channel, att_ctrl.timestamp / 1000, @@ -1402,8 +1480,13 @@ public: private: MavlinkOrbSubscription *debug_sub; + uint64_t debug_time; protected: + explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), + debug_time(0) + {} + void subscribe(Mavlink *mavlink) { debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); @@ -1413,7 +1496,7 @@ protected: { struct debug_key_value_s debug; - if (debug_sub->update(t, &debug)) { + if (debug_sub->update(&debug_time, &debug)) { /* enforce null termination */ debug.key[sizeof(debug.key) - 1] = '\0'; @@ -1455,7 +1538,7 @@ protected: void send(const hrt_abstime t) { struct vehicle_status_s status; - (void)status_sub->update(t, &status); + (void)status_sub->update(&status); if (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) { @@ -1490,8 +1573,13 @@ public: private: MavlinkOrbSubscription *range_sub; + uint64_t range_time; protected: + explicit MavlinkStreamDistanceSensor() : MavlinkStream(), + range_time(0) + {} + void subscribe(Mavlink *mavlink) { range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); @@ -1501,7 +1589,7 @@ protected: { struct range_finder_report range; - if (range_sub->update(t, &range)) { + if (range_sub->update(&range_time, &range)) { uint8_t type; @@ -1521,7 +1609,6 @@ protected: } }; -unsigned MavlinkStreamServoOutputRaw::_n = 0; StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static), @@ -1534,10 +1621,10 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw::new_instance, &MavlinkStreamServoOutputRaw::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw::new_instance, &MavlinkStreamServoOutputRaw::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw::new_instance, &MavlinkStreamServoOutputRaw::get_name_static), - new StreamListItem(&MavlinkStreamServoOutputRaw::new_instance, &MavlinkStreamServoOutputRaw::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static), + new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static), new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 0a23fb01e5..901fa8f057 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -50,7 +50,6 @@ MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _fd(orb_subscribe(_topic)), _published(false), _topic(topic), - _last_check(0), next(nullptr) { } @@ -67,24 +66,39 @@ MavlinkOrbSubscription::get_topic() const } bool -MavlinkOrbSubscription::update(const hrt_abstime t, void* data) +MavlinkOrbSubscription::update(uint64_t *time, void* data) { - if (_last_check == t) { - /* already checked right now, return result of the check */ - return _updated; + // TODO this is NOT atomic operation, we can get data newer than time + // if topic was published between orb_stat and orb_copy calls. - } else { - _last_check = t; - orb_check(_fd, &_updated); - - if (_updated && data) { - orb_copy(_topic, _fd, data); - _published = true; - return true; - } + uint64_t time_topic; + if (orb_stat(_fd, &time_topic)) { + /* error getting last topic publication time */ + time_topic = 0; } - return false; + if (orb_copy(_topic, _fd, data)) { + /* error copying topic data */ + memset(data, 0, _topic->o_size); + return false; + + } else { + /* data copied successfully */ + _published = true; + if (time_topic != *time) { + *time = time_topic; + return true; + + } else { + return false; + } + } +} + +bool +MavlinkOrbSubscription::update(void* data) +{ + return !orb_copy(_topic, _fd, data); } bool diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index abd4031bdc..71efb43af0 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -53,7 +53,21 @@ public: MavlinkOrbSubscription(const orb_id_t topic); ~MavlinkOrbSubscription(); - bool update(const hrt_abstime t, void* data); + /** + * Check if subscription updated and get data. + * + * @return true only if topic was updated and data copied to buffer successfully. + * If topic was not updated since last check it will return false but still copy the data. + * If no data available data buffer will be filled with zeroes. + */ + bool update(uint64_t *time, void* data); + + /** + * Copy topic data to given buffer. + * + * @return true only if topic data copied successfully. + */ + bool update(void* data); /** * Check if the topic has been published. @@ -68,8 +82,6 @@ private: const orb_id_t _topic; ///< topic metadata int _fd; ///< subscription handle bool _published; ///< topic was ever published - hrt_abstime _last_check; ///< time of last check - bool _updated; ///< updated on last check }; From 44481e3773b7a576b31727c64931216112d953e0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 12 Jun 2014 12:01:54 +0200 Subject: [PATCH 51/67] mavlink: sign of climb rate fixed in VFR_HUD message --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e0a148b53c..aff1aa929c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -554,7 +554,7 @@ protected: heading, throttle, pos.alt, - pos.vel_d); + -pos.vel_d); } } }; From 60e93deaa3971dccba7f1e3184a69f6bafe391d8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 23 Jun 2014 07:44:30 +0200 Subject: [PATCH 52/67] phantom: silence ESC --- ROMFS/px4fmu_common/init.d/3031_phantom | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 24372bddcf..d05c3174f9 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -37,3 +37,7 @@ then fi set MIXER FMU_Q + +# Provide ESC a constant 1000 us pulse +set PWM_OUTPUTS 4 +set PWM_DISARMED 1000 From cfdbf2c5e914c6264d9158470d4f98c84a483f68 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 23 Jun 2014 11:15:27 +0200 Subject: [PATCH 53/67] perfcounter: write time unit for all fields --- src/modules/systemlib/perf_counter.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 22182e39e8..d6d8284d24 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -301,7 +301,7 @@ perf_print_counter_fd(int fd, perf_counter_t handle) case PC_ELAPSED: { struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - dprintf(fd, "%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n", + dprintf(fd, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus\n", handle->name, pce->event_count, pce->time_total, @@ -314,7 +314,7 @@ perf_print_counter_fd(int fd, perf_counter_t handle) case PC_INTERVAL: { struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; - dprintf(fd, "%s: %llu events, %llu avg, min %lluus max %lluus\n", + dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n", handle->name, pci->event_count, (pci->time_last - pci->time_first) / pci->event_count, From d58a992e911114383a44327eb4478193824b580d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Jun 2014 13:44:36 +0200 Subject: [PATCH 54/67] Hotfix: Improve PX4IO monitor command --- src/drivers/px4io/px4io.cpp | 38 +++++++++++++++------------- src/drivers/px4io/px4io_uploader.cpp | 6 ++--- 2 files changed, 24 insertions(+), 20 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 972f451485..992ab9623b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -197,8 +197,10 @@ public: * Print IO status. * * Print all relevant IO status information + * + * @param extended_status Shows more verbose information (in particular RC config) */ - void print_status(); + void print_status(bool extended_status); /** * Fetch and print debug console output. @@ -1850,7 +1852,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) } void -PX4IO::print_status() +PX4IO::print_status(bool extended_status) { /* basic configuration */ printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n", @@ -2013,19 +2015,21 @@ PX4IO::print_status() printf("\n"); } - for (unsigned i = 0; i < _max_rc_input; i++) { - unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; - uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); - printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", - i, - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), - options, - ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), - ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); + if (extended_status) { + for (unsigned i = 0; i < _max_rc_input; i++) { + unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; + uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); + printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", + i, + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), + options, + ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), + ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); + } } printf("failsafe"); @@ -2853,7 +2857,7 @@ monitor(void) if (g_dev != nullptr) { printf("\033[2J\033[H"); /* move cursor home and clear screen */ - (void)g_dev->print_status(); + (void)g_dev->print_status(false); (void)g_dev->print_debug(); printf("\n\n\n[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); @@ -3119,7 +3123,7 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { printf("[px4io] loaded\n"); - g_dev->print_status(); + g_dev->print_status(true); exit(0); } diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 28ec62356b..7b6361a7ca 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -240,9 +240,9 @@ PX4IO_Uploader::upload(const char *filenames[]) close(_io_fd); _io_fd = -1; - // sleep for enough time for the IO chip to boot. This makes - // forceupdate more reliably startup IO again after update - up_udelay(100*1000); + // sleep for enough time for the IO chip to boot. This makes + // forceupdate more reliably startup IO again after update + up_udelay(100*1000); return ret; } From 87857cdd48d43a28c3b8ed1f1fe500ad28a93bbc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Jun 2014 13:45:20 +0200 Subject: [PATCH 55/67] Hotfix: Fix message name typo --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index aff1aa929c..fed2dfb0d9 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1625,7 +1625,7 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static), new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static), new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static), - new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), + new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), From a9653fa10db3884d3d17ee33f80f23aa2e3ef842 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Jun 2014 13:51:05 +0200 Subject: [PATCH 56/67] Hotfix: Only orb_copy items in mavlink app if the timestamp changed --- .../mavlink/mavlink_orb_subscription.cpp | 26 +++++++++++-------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 901fa8f057..3807323c2a 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -44,6 +44,8 @@ #include #include +#include + #include "mavlink_orb_subscription.h" MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : @@ -77,21 +79,23 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data) time_topic = 0; } - if (orb_copy(_topic, _fd, data)) { - /* error copying topic data */ - memset(data, 0, _topic->o_size); - return false; + if (time_topic != *time) { - } else { - /* data copied successfully */ - _published = true; - if (time_topic != *time) { - *time = time_topic; - return true; + if (orb_copy(_topic, _fd, data)) { + /* error copying topic data */ + memset(data, 0, _topic->o_size); + //warnx("err copy, fd: %d, obj: %s, size: %d", _fd, _topic->o_name, _topic->o_size); + return false; } else { - return false; + /* data copied successfully */ + _published = true; + *time = time_topic; + return true; } + + } else { + return false; } } From bf5061aa21872c98576d46aee894e670ce0c52a0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Jun 2014 13:52:09 +0200 Subject: [PATCH 57/67] Fix error reporting in stream config, report if a stream was not found at all in stream list and return error --- src/modules/mavlink/mavlink_main.cpp | 38 +++++++++++++++------------- 1 file changed, 21 insertions(+), 17 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 046f45bd9c..a9b8323f34 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1555,32 +1555,36 @@ Mavlink::configure_stream(const char *stream_name, const float rate) /* delete stream */ LL_DELETE(_streams, stream); delete stream; + warnx("deleted stream %s", stream->get_name()); } return OK; } } - if (interval > 0) { - /* search for stream with specified name in supported streams list */ - for (unsigned int i = 0; streams_list[i] != nullptr; i++) { - - if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { - /* create new instance */ - stream = streams_list[i]->new_instance(); - stream->set_channel(get_channel()); - stream->set_interval(interval); - stream->subscribe(this); - LL_APPEND(_streams, stream); - return OK; - } - } - - } else { - /* stream not found, nothing to disable */ + if (interval == 0) { + /* stream was not active and is requested to be disabled, do nothing */ return OK; } + /* search for stream with specified name in supported streams list */ + for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + + if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { + /* create new instance */ + stream = streams_list[i]->new_instance(); + stream->set_channel(get_channel()); + stream->set_interval(interval); + stream->subscribe(this); + LL_APPEND(_streams, stream); + + return OK; + } + } + + /* if we reach here, the stream list does not contain the stream */ + warnx("stream %s not found", stream_name); + return ERROR; } From 68442e31ac6970be91592282c9b70ebc76fa142d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 23 Jun 2014 14:56:48 +0200 Subject: [PATCH 58/67] Hotfix: Move channel count to right position --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fed2dfb0d9..4433377c6d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1275,6 +1275,7 @@ protected: // New message mavlink_msg_rc_channels_send(_channel, rc.timestamp_publication / 1000, + rc.channel_count, ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX), ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX), ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX), @@ -1293,7 +1294,6 @@ protected: ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX), ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX), ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX), - rc.channel_count, rc.rssi); } } From 52d539d3cdf56eb4145dbf29c407bb0a1a7eebe5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Jun 2014 17:39:48 +0200 Subject: [PATCH 59/67] extend integrator reset flags in uorb attitude setpoint topic to all axes --- src/modules/uORB/topics/vehicle_attitude_setpoint.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index d526a8ff27..8446e9c6e2 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -72,6 +72,8 @@ struct vehicle_attitude_setpoint_s { float thrust; /**< Thrust in Newton the power system should generate */ bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */ + bool pitch_reset_integral; /**< Reset pitch integral part (navigation logic change) */ + bool yaw_reset_integral; /**< Reset yaw integral part (navigation logic change) */ }; From 9a911f7388e1a48630469fd2e875f00a119c829a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Jun 2014 17:40:56 +0200 Subject: [PATCH 60/67] fw att control: reset integrators when requested via attitude setpoint topic --- src/modules/fw_att_control/fw_att_control_main.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 178b590ae5..3cd4ce9285 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -707,14 +707,21 @@ FixedwingAttitudeControl::task_main() float throttle_sp = 0.0f; if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { + /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ - if (_att_sp.roll_reset_integral) + if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); - + } + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + } } else { /* * Scale down roll and pitch as the setpoints are radians From c6cdcfc2638f8d994b3716d8739614c5377c4962 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Jun 2014 17:42:21 +0200 Subject: [PATCH 61/67] fw pos control: set integrator reset flags in attitude setpoint topic, set them to true when launchdetection is running (while waiting for launch) --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) 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 116d3cc63d..518116fa1d 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 @@ -841,6 +841,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* current waypoint (the one currently heading for) */ math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); + /* Initialize attitude controller integrator reset flags to 0 */ + _att_sp.roll_reset_integral = false; + _att_sp.pitch_reset_integral = false; + _att_sp.yaw_reset_integral = false; /* previous waypoint */ math::Vector<2> prev_wp; @@ -1028,6 +1032,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // warnx("Launch detection running"); mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); last_sent = hrt_absolute_time(); + /* Tell the attitude controller to stop integrating while we are + * waiting for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; } launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); if (launchDetector.getLaunchDetected()) { From 819812e11271d7b78f4af2d5bbb7ac6e4be9e494 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Jun 2014 20:52:24 +0200 Subject: [PATCH 62/67] fw pos ctrl: move setting of attitude integral reset flag --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) 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 518116fa1d..fe58476821 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 @@ -1032,12 +1032,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // warnx("Launch detection running"); mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); last_sent = hrt_absolute_time(); - /* Tell the attitude controller to stop integrating while we are - * waiting for the launch */ - _att_sp.roll_reset_integral = true; - _att_sp.pitch_reset_integral = true; - _att_sp.yaw_reset_integral = true; } + + /* Tell the attitude controller to stop integrating while we are waiting + * for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; + + /* Detect launch */ launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); if (launchDetector.getLaunchDetected()) { launch_detected = true; From 4f560f729ec1f40427401119a0f17d9d0e9843f4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 24 Jun 2014 20:53:06 +0200 Subject: [PATCH 63/67] fw pos ctrl: remove comments --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 -- 1 file changed, 2 deletions(-) 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 fe58476821..0e065211e9 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 @@ -1024,12 +1024,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ -// warnx("Launch detection running"); if(!launch_detected) { //do not do further checks once a launch was detected if (launchDetector.launchDetectionEnabled()) { static hrt_abstime last_sent = 0; if(hrt_absolute_time() - last_sent > 4e6) { -// warnx("Launch detection running"); mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); last_sent = hrt_absolute_time(); } From f9d5cf332c9e60661c2e1c0827c84480e49d4fe8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 25 Jun 2014 17:05:20 +0200 Subject: [PATCH 64/67] Revert "Hotfix: Only orb_copy items in mavlink app if the timestamp changed" This reverts commit a9653fa10db3884d3d17ee33f80f23aa2e3ef842. --- .../mavlink/mavlink_orb_subscription.cpp | 30 ++++++++----------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 3807323c2a..901fa8f057 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -44,8 +44,6 @@ #include #include -#include - #include "mavlink_orb_subscription.h" MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : @@ -79,23 +77,21 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data) time_topic = 0; } - if (time_topic != *time) { - - if (orb_copy(_topic, _fd, data)) { - /* error copying topic data */ - memset(data, 0, _topic->o_size); - //warnx("err copy, fd: %d, obj: %s, size: %d", _fd, _topic->o_name, _topic->o_size); - return false; - - } else { - /* data copied successfully */ - _published = true; - *time = time_topic; - return true; - } + if (orb_copy(_topic, _fd, data)) { + /* error copying topic data */ + memset(data, 0, _topic->o_size); + return false; } else { - return false; + /* data copied successfully */ + _published = true; + if (time_topic != *time) { + *time = time_topic; + return true; + + } else { + return false; + } } } From 43c3559763841abacf5e74e15a6172026071088b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:08:25 +0200 Subject: [PATCH 65/67] commander: Make mavlink_fd in arming command non-optional --- src/modules/commander/state_machine_helper.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 0ddd4f05a2..abb9178732 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,7 +57,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0); + arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); From 547f71d791d0a04001580e8371bdf59b808a3d29 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:07:28 +0200 Subject: [PATCH 66/67] Add mavlink_fd to all commander arm transitions to provide user feedback why the arming command failed --- src/modules/commander/commander.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 65922b2a5d..588f48225c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -412,7 +412,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* if HIL got enabled, reset battery status state */ if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { /* reset the arming mode to disarmed */ - arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed, mavlink_fd); if (arming_res != TRANSITION_DENIED) { mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); @@ -945,8 +945,8 @@ int commander_thread_main(int argc, char *argv[]) if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { + mavlink_log_info(mavlink_fd, "#audio DISARMED by safety switch"); } } } @@ -1149,10 +1149,10 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd); } status_changed = true; @@ -1163,7 +1163,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost @@ -1217,7 +1217,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed); + arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd); stick_off_counter = 0; } else { @@ -1239,7 +1239,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { - arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); } stick_on_counter = 0; @@ -1941,7 +1941,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -2004,7 +2004,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); break; } From f76040e55407d3526fb7655ec732304c9ba1dc8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:54:33 +0200 Subject: [PATCH 67/67] mavlink: Always send heartbeat and do not require both topics to update --- src/modules/mavlink/mavlink_messages.cpp | 26 +++++++++++++----------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4433377c6d..b295bf35f8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -226,19 +226,21 @@ protected: struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; - if (status_sub->update(&status) && pos_sp_triplet_sub->update(&pos_sp_triplet)) { - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + /* always send the heartbeat, independent of the update status of the topics */ + (void)status_sub->update(&status); + (void)pos_sp_triplet_sub->update(&pos_sp_triplet); - mavlink_msg_heartbeat_send(_channel, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); - } + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + mavlink_msg_heartbeat_send(_channel, + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_base_mode, + mavlink_custom_mode, + mavlink_state); } };