From 7813566e6680f4940989fb91760ddb0782b05858 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 6 May 2014 19:30:45 +0400 Subject: [PATCH 01/71] Initial UAVCAN integration. The library compiles successfully, CAN driver appears to be working properly. There is one hardcoded path in the module makefile that needs to be fixed; plus the compilation will likely fail unless arch/math.h contains log2l() --- .gitignore | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + makefiles/setup.mk | 1 + makefiles/toolchain_gnu-arm-eabi.mk | 2 +- src/modules/uavcan/.gitignore | 1 + src/modules/uavcan/module.mk | 74 +++++++++++++++++++ src/modules/uavcan/uavcan_clock.cpp | 65 +++++++++++++++++ src/modules/uavcan/uavcan_main.cpp | 100 ++++++++++++++++++++++++++ src/modules/uavcan/uavcan_main.hpp | 39 ++++++++++ 9 files changed, 283 insertions(+), 1 deletion(-) create mode 100644 src/modules/uavcan/.gitignore create mode 100644 src/modules/uavcan/module.mk create mode 100644 src/modules/uavcan/uavcan_clock.cpp create mode 100644 src/modules/uavcan/uavcan_main.cpp create mode 100644 src/modules/uavcan/uavcan_main.hpp diff --git a/.gitignore b/.gitignore index 5b345b34a8..8c3bb1bb55 100644 --- a/.gitignore +++ b/.gitignore @@ -37,3 +37,4 @@ mavlink/include/mavlink/v0.9/ tags .tags_sorted_by_file .pydevproject +/uavcan diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e13421acc5..09b5bf7c6e 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -73,6 +73,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led +MODULES += modules/uavcan # # Estimation modules (EKF/ SO3 / other filters) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 183b143d6c..a5271c6569 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -48,6 +48,7 @@ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/ export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink)/ +export UAVCAN_DIR = $(abspath $(PX4_BASE)/uavcan)/ export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/ export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/ export BUILD_DIR = $(abspath $(PX4_BASE)/Build)/ diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index b519e0e7a1..3c00e77f11 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -111,7 +111,7 @@ INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) # Language-specific flags # ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics # Generic warnings # diff --git a/src/modules/uavcan/.gitignore b/src/modules/uavcan/.gitignore new file mode 100644 index 0000000000..24fbf171f0 --- /dev/null +++ b/src/modules/uavcan/.gitignore @@ -0,0 +1 @@ +./dsdlc_generated/ diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk new file mode 100644 index 0000000000..5caa8d401a --- /dev/null +++ b/src/modules/uavcan/module.mk @@ -0,0 +1,74 @@ +############################################################################ +# +# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# Author: Pavel Kirienko +# +# 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. +# +############################################################################ + +# +# UAVCAN <--> uORB bridge +# + +MODULE_COMMAND = uavcan + +MAXOPTIMIZATION = -Os + +SRCS += uavcan_main.cpp \ + uavcan_clock.cpp + +# +# libuavcan +# +include $(UAVCAN_DIR)/libuavcan/include.mk +SRCS += $(LIBUAVCAN_SRC) +# TODO fix include path +INCLUDE_DIRS += $(LIBUAVCAN_INC) /media/storage/px4/Firmware/Build/px4fmu-v2_default.build/nuttx-export/include/cxx/ +EXTRADEFINES += -DUAVCAN_MEM_POOL_BLOCK_SIZE=56 \ + -DUAVCAN_TOSTRING=0 \ + -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \ + -DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1 + +# +# libuavcan drivers for STM32 +# +include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk +SRCS += $(LIBUAVCAN_STM32_SRC) +INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) +EXTRADEFINES += -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=1 + +# +# Invoke DSDL compiler +# TODO: Add make target for this, or invoke dsdlc manually. +# The second option assumes that the generated headers shall be saved +# under the version control, which may be undesirable. +# The first option requires python3 and python3-mako for the sources to be built. +# +$(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR))) +INCLUDE_DIRS += dsdlc_generated diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp new file mode 100644 index 0000000000..2a5e889890 --- /dev/null +++ b/src/modules/uavcan/uavcan_clock.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: Pavel Kirienko + * + * 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. + * + ****************************************************************************/ + +#include +#include + +namespace uavcan_stm32 +{ +namespace clock +{ + +uavcan::MonotonicTime getMonotonic() +{ + return uavcan::MonotonicTime::fromUSec(hrt_absolute_time()); +} + +uavcan::UtcTime getUtc() +{ + return uavcan::UtcTime(); +} + +void adjustUtc(uavcan::UtcDuration adjustment) +{ + (void)adjustment; +} + +uavcan::uint64_t getUtcUSecFromCanInterrupt() +{ + return 0; +} + +} +} + diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp new file mode 100644 index 0000000000..fbb2e174b8 --- /dev/null +++ b/src/modules/uavcan/uavcan_main.cpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: Pavel Kirienko + * + * 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. + * + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include "uavcan_main.hpp" + +extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); + +namespace +{ + +uavcan_stm32::CanInitHelper<> can_driver; + +void print_usage() +{ + warnx("usage: uavcan start [can_bitrate]"); +} + +int test_thread(int argc, char *argv[]) +{ + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + stm32_configgpio(GPIO_CAN2_RX); + stm32_configgpio(GPIO_CAN2_TX); + int res = can_driver.init(1000000); + if (res < 0) + { + errx(res, "CAN driver init failed"); + } + while (true) + { + ::sleep(1); + auto iface = static_cast(can_driver.driver.getIface(0)); + res = iface->send(uavcan::CanFrame(), uavcan_stm32::clock::getMonotonic(), 0); + warnx("published %i, pending %u", res, can_driver.driver.getIface(0)->getRxQueueLength()); + } + return 0; +} + +} + +int uavcan_main(int argc, char *argv[]) +{ + if (argc < 2) { + print_usage(); + ::exit(1); + } + + if (!std::strcmp(argv[1], "start")) { + static bool started = false; + if (started) + { + warnx("already started"); + ::exit(1); + } + started = true; + (void)task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 3000, + static_cast(&test_thread), const_cast(argv)); + return 0; + } else { + print_usage(); + ::exit(1); + } + return 0; +} diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp new file mode 100644 index 0000000000..7fcc992d04 --- /dev/null +++ b/src/modules/uavcan/uavcan_main.hpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Author: Pavel Kirienko + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +// ... From 7d7a375dd1a69bd286f127de51b11e5ecd390640 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 6 May 2014 19:42:40 +0400 Subject: [PATCH 02/71] Fixed hardcoded include path --- makefiles/nuttx.mk | 1 + src/modules/uavcan/module.mk | 6 +++--- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/makefiles/nuttx.mk b/makefiles/nuttx.mk index d283096b25..bf07441408 100644 --- a/makefiles/nuttx.mk +++ b/makefiles/nuttx.mk @@ -64,6 +64,7 @@ LDSCRIPT += $(NUTTX_EXPORT_DIR)build/ld.script # Add directories from the NuttX export to the relevant search paths # INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \ + $(NUTTX_EXPORT_DIR)include/cxx \ $(NUTTX_EXPORT_DIR)arch/chip \ $(NUTTX_EXPORT_DIR)arch/common diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 5caa8d401a..6974de8ca0 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -48,8 +48,7 @@ SRCS += uavcan_main.cpp \ # include $(UAVCAN_DIR)/libuavcan/include.mk SRCS += $(LIBUAVCAN_SRC) -# TODO fix include path -INCLUDE_DIRS += $(LIBUAVCAN_INC) /media/storage/px4/Firmware/Build/px4fmu-v2_default.build/nuttx-export/include/cxx/ +INCLUDE_DIRS += $(LIBUAVCAN_INC) EXTRADEFINES += -DUAVCAN_MEM_POOL_BLOCK_SIZE=56 \ -DUAVCAN_TOSTRING=0 \ -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \ @@ -61,7 +60,8 @@ EXTRADEFINES += -DUAVCAN_MEM_POOL_BLOCK_SIZE=56 \ include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk SRCS += $(LIBUAVCAN_STM32_SRC) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) -EXTRADEFINES += -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=1 +EXTRADEFINES += -DUAVCAN_STM32_NUTTX \ + -DUAVCAN_STM32_NUM_IFACES=1 # # Invoke DSDL compiler From 5716dad25db27315fa7cebf8183a71f864860f41 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 6 May 2014 20:14:07 +0400 Subject: [PATCH 03/71] Added workaround for hardware issue on Pixhawk v1 --- src/modules/uavcan/module.mk | 2 +- src/modules/uavcan/uavcan_main.cpp | 8 +++++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 6974de8ca0..3966210b52 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -61,7 +61,7 @@ include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk SRCS += $(LIBUAVCAN_STM32_SRC) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) EXTRADEFINES += -DUAVCAN_STM32_NUTTX \ - -DUAVCAN_STM32_NUM_IFACES=1 + -DUAVCAN_STM32_NUM_IFACES=2 # # Invoke DSDL compiler diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index fbb2e174b8..474063aa65 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include "uavcan_main.hpp" extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -53,9 +54,14 @@ void print_usage() int test_thread(int argc, char *argv[]) { + /* + * Forced pull up on CAN2 is required for Pixhawk v1 where the second interface lacks a transceiver. + * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to + * fail during initialization. + */ stm32_configgpio(GPIO_CAN1_RX); stm32_configgpio(GPIO_CAN1_TX); - stm32_configgpio(GPIO_CAN2_RX); + stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); stm32_configgpio(GPIO_CAN2_TX); int res = can_driver.init(1000000); if (res < 0) From 4b11145797fdc26e5bf29738dd319ab9e8003356 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 7 May 2014 13:42:34 +0400 Subject: [PATCH 04/71] Working UAVCAN node. No application logic is implemented yet; the node just publishes its status once a second (uavcan.protocol.NodeStatus) and responds to basic services (transport stats, node discovery) --- src/modules/uavcan/uavcan_clock.cpp | 9 +- src/modules/uavcan/uavcan_main.cpp | 149 ++++++++++++++++++++++------ src/modules/uavcan/uavcan_main.hpp | 29 +++++- 3 files changed, 152 insertions(+), 35 deletions(-) diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp index 2a5e889890..0e844f3460 100644 --- a/src/modules/uavcan/uavcan_clock.cpp +++ b/src/modules/uavcan/uavcan_clock.cpp @@ -60,6 +60,13 @@ uavcan::uint64_t getUtcUSecFromCanInterrupt() return 0; } -} +} // namespace clock + +SystemClock& SystemClock::instance() +{ + static SystemClock inst; + return inst; +} + } diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 474063aa65..bf6d4fb087 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -40,64 +40,147 @@ #include #include "uavcan_main.hpp" -extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); +/* + * UavcanNode + */ +UavcanNode* UavcanNode::_instance; -namespace +int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { + if (_instance != nullptr) { + warnx("Already started"); + return -1; + } -uavcan_stm32::CanInitHelper<> can_driver; - -void print_usage() -{ - warnx("usage: uavcan start [can_bitrate]"); -} - -int test_thread(int argc, char *argv[]) -{ /* + * GPIO config. * Forced pull up on CAN2 is required for Pixhawk v1 where the second interface lacks a transceiver. * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to * fail during initialization. */ - stm32_configgpio(GPIO_CAN1_RX); - stm32_configgpio(GPIO_CAN1_TX); + stm32_configgpio (GPIO_CAN1_RX); + stm32_configgpio (GPIO_CAN1_TX); stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); - stm32_configgpio(GPIO_CAN2_TX); - int res = can_driver.init(1000000); - if (res < 0) - { - errx(res, "CAN driver init failed"); + stm32_configgpio (GPIO_CAN2_TX); + + /* + * CAN driver init + */ + static CanInitHelper can; + static bool can_initialized = false; + if (!can_initialized) { + const int can_init_res = can.init(bitrate); + if (can_init_res < 0) { + warnx("CAN driver init failed %i", can_init_res); + return can_init_res; + } + can_initialized = true; } - while (true) - { - ::sleep(1); - auto iface = static_cast(can_driver.driver.getIface(0)); - res = iface->send(uavcan::CanFrame(), uavcan_stm32::clock::getMonotonic(), 0); - warnx("published %i, pending %u", res, can_driver.driver.getIface(0)->getRxQueueLength()); + + /* + * Node init + */ + _instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance()); + if (_instance == nullptr) { + warnx("Out of memory"); + return -1; } - return 0; + const int node_init_res = _instance->init(node_id); + if (node_init_res < 0) { + delete _instance; + _instance = nullptr; + warnx("Node init failed %i", node_init_res); + return node_init_res; + } + + /* + * Start the task. Normally it should never exit. + */ + static auto run_trampoline = [](int, char*[]) {return UavcanNode::_instance->run();}; + return task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, + static_cast(run_trampoline), nullptr); } +int UavcanNode::init(uavcan::NodeID node_id) +{ + uavcan::protocol::SoftwareVersion swver; + swver.major = 12; // TODO fill version info + swver.minor = 34; + _node.setSoftwareVersion(swver); + + uavcan::protocol::HardwareVersion hwver; + hwver.major = 42; // TODO fill version info + hwver.minor = 42; + _node.setHardwareVersion(hwver); + + _node.setName("org.pixhawk"); // Huh? + + _node.setNodeID(node_id); + + return _node.start(); } +int UavcanNode::run() +{ + _node.setStatusOk(); + while (true) { + // TODO: ORB multiplexing + const int res = _node.spin(uavcan::MonotonicDuration::getInfinite()); + if (res < 0) { + warnx("Spin error %i", res); + ::sleep(1); + } + } + return -1; +} + +/* + * App entry point + */ +static void print_usage() +{ + warnx("usage: uavcan start [can_bitrate]"); +} + +extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); + int uavcan_main(int argc, char *argv[]) { + constexpr long DEFAULT_CAN_BITRATE = 1000000; + if (argc < 2) { print_usage(); ::exit(1); } if (!std::strcmp(argv[1], "start")) { - static bool started = false; - if (started) - { - warnx("already started"); + if (argc < 3) { + print_usage(); ::exit(1); } - started = true; - (void)task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 3000, - static_cast(&test_thread), const_cast(argv)); - return 0; + /* + * Node ID + */ + const int node_id = atoi(argv[2]); + if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { + warnx("Invalid Node ID %i", node_id); + ::exit(1); + } + /* + * CAN bitrate + */ + long bitrate = 0; + if (argc > 3) { + bitrate = atol(argv[3]); + } + if (bitrate <= 0) { + bitrate = DEFAULT_CAN_BITRATE; + } + /* + * Start + */ + warnx("Node ID %i, bitrate %li", node_id, bitrate); + return UavcanNode::start(node_id, bitrate); } else { print_usage(); ::exit(1); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 7fcc992d04..8bd660cc53 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -36,4 +36,31 @@ #include -// ... +/** + * Implements basic functinality of UAVCAN node. + */ +class UavcanNode +{ + static constexpr unsigned MemPoolSize = 10752; + static constexpr unsigned RxQueueLenPerIface = 64; + static constexpr unsigned StackSize = 3000; + +public: + typedef uavcan::Node Node; + typedef uavcan_stm32::CanInitHelper CanInitHelper; + + UavcanNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& system_clock) + : _node(can_driver, system_clock) + { } + + static int start(uavcan::NodeID node_id, uint32_t bitrate); + + Node& getNode() { return _node; } + +private: + int init(uavcan::NodeID node_id); + int run(); + + static UavcanNode* _instance; + Node _node; +}; From 04df4270f0ac6aec360076185363338366475166 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 7 May 2014 13:56:05 +0400 Subject: [PATCH 05/71] Removed the placement new workaround. It seems like we can pull from the toolchain's standard includes with no harm. --- src/modules/uavcan/module.mk | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 3966210b52..c91208342a 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -51,8 +51,7 @@ SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) EXTRADEFINES += -DUAVCAN_MEM_POOL_BLOCK_SIZE=56 \ -DUAVCAN_TOSTRING=0 \ - -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \ - -DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1 + -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 # # libuavcan drivers for STM32 From be728d189e4b1b04de5a0b45710e08effcf50b8b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 7 May 2014 14:24:40 +0400 Subject: [PATCH 06/71] Catching up with libuavcan - some preprocessor symbols are no longer required to be defined explicitly --- src/modules/uavcan/module.mk | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index c91208342a..0985438796 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -49,9 +49,9 @@ SRCS += uavcan_main.cpp \ include $(UAVCAN_DIR)/libuavcan/include.mk SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) -EXTRADEFINES += -DUAVCAN_MEM_POOL_BLOCK_SIZE=56 \ - -DUAVCAN_TOSTRING=0 \ - -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 +# Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile +# because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. +EXTRADEFINES += -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 # # libuavcan drivers for STM32 From 973b193261fab69320f25fae68345a60d7678dc8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 14:29:30 +0200 Subject: [PATCH 07/71] Fixed comments and code style of UAVCAN node --- src/modules/uavcan/uavcan_clock.cpp | 13 +++++++--- src/modules/uavcan/uavcan_main.cpp | 40 +++++++++++++++++++++++------ src/modules/uavcan/uavcan_main.hpp | 19 +++++++++----- 3 files changed, 55 insertions(+), 17 deletions(-) diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp index 0e844f3460..e41d5f953b 100644 --- a/src/modules/uavcan/uavcan_clock.cpp +++ b/src/modules/uavcan/uavcan_clock.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: Pavel Kirienko + * 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 @@ -35,6 +34,14 @@ #include #include +/** + * @file uavcan_clock.cpp + * + * Implements a clock for the CAN node. + * + * @author Pavel Kirienko + */ + namespace uavcan_stm32 { namespace clock @@ -62,7 +69,7 @@ uavcan::uint64_t getUtcUSecFromCanInterrupt() } // namespace clock -SystemClock& SystemClock::instance() +SystemClock &SystemClock::instance() { static SystemClock inst; return inst; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index bf6d4fb087..8d715e3b1d 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: Pavel Kirienko + * 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 @@ -40,10 +39,18 @@ #include #include "uavcan_main.hpp" +/** + * @file uavcan_main.cpp + * + * Implements basic functinality of UAVCAN node. + * + * @author Pavel Kirienko + */ + /* * UavcanNode */ -UavcanNode* UavcanNode::_instance; +UavcanNode *UavcanNode::_instance; int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { @@ -58,22 +65,25 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to * fail during initialization. */ - stm32_configgpio (GPIO_CAN1_RX); - stm32_configgpio (GPIO_CAN1_TX); + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP); - stm32_configgpio (GPIO_CAN2_TX); + stm32_configgpio(GPIO_CAN2_TX); /* * CAN driver init */ static CanInitHelper can; static bool can_initialized = false; + if (!can_initialized) { const int can_init_res = can.init(bitrate); + if (can_init_res < 0) { warnx("CAN driver init failed %i", can_init_res); return can_init_res; } + can_initialized = true; } @@ -81,11 +91,14 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * Node init */ _instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance()); + if (_instance == nullptr) { warnx("Out of memory"); return -1; } + const int node_init_res = _instance->init(node_id); + if (node_init_res < 0) { delete _instance; _instance = nullptr; @@ -96,9 +109,9 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) /* * Start the task. Normally it should never exit. */ - static auto run_trampoline = [](int, char*[]) {return UavcanNode::_instance->run();}; + static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; return task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, - static_cast(run_trampoline), nullptr); + static_cast(run_trampoline), nullptr); } int UavcanNode::init(uavcan::NodeID node_id) @@ -123,14 +136,17 @@ int UavcanNode::init(uavcan::NodeID node_id) int UavcanNode::run() { _node.setStatusOk(); + while (true) { // TODO: ORB multiplexing const int res = _node.spin(uavcan::MonotonicDuration::getInfinite()); + if (res < 0) { warnx("Spin error %i", res); ::sleep(1); } } + return -1; } @@ -158,32 +174,40 @@ int uavcan_main(int argc, char *argv[]) print_usage(); ::exit(1); } + /* * Node ID */ const int node_id = atoi(argv[2]); + if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { warnx("Invalid Node ID %i", node_id); ::exit(1); } + /* * CAN bitrate */ long bitrate = 0; + if (argc > 3) { bitrate = atol(argv[3]); } + if (bitrate <= 0) { bitrate = DEFAULT_CAN_BITRATE; } + /* * Start */ warnx("Node ID %i, bitrate %li", node_id, bitrate); return UavcanNode::start(node_id, bitrate); + } else { print_usage(); ::exit(1); } + return 0; } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 8bd660cc53..b27449f1f4 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. - * Author: Pavel Kirienko + * 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 @@ -37,7 +36,15 @@ #include /** - * Implements basic functinality of UAVCAN node. + * @file uavcan_main.hpp + * + * Defines basic functinality of UAVCAN node. + * + * @author Pavel Kirienko + */ + +/** + * A UAVCAN node. */ class UavcanNode { @@ -49,18 +56,18 @@ public: typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; - UavcanNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& system_clock) + UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : _node(can_driver, system_clock) { } static int start(uavcan::NodeID node_id, uint32_t bitrate); - Node& getNode() { return _node; } + Node &getNode() { return _node; } private: int init(uavcan::NodeID node_id); int run(); - static UavcanNode* _instance; + static UavcanNode *_instance; ///< pointer to the library instance Node _node; }; From ab5e76e3d9da5a76d6520e166d5370c6bdfc4a53 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 15:03:08 +0200 Subject: [PATCH 08/71] Fixed printing of bit rate --- src/modules/uavcan/uavcan_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8d715e3b1d..ca4da1c2dc 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -162,7 +162,7 @@ extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); int uavcan_main(int argc, char *argv[]) { - constexpr long DEFAULT_CAN_BITRATE = 1000000; + constexpr unsigned DEFAULT_CAN_BITRATE = 1000000; if (argc < 2) { print_usage(); @@ -188,7 +188,7 @@ int uavcan_main(int argc, char *argv[]) /* * CAN bitrate */ - long bitrate = 0; + unsigned bitrate = 0; if (argc > 3) { bitrate = atol(argv[3]); @@ -201,7 +201,7 @@ int uavcan_main(int argc, char *argv[]) /* * Start */ - warnx("Node ID %i, bitrate %li", node_id, bitrate); + warnx("Node ID %u, bitrate %u", node_id, bitrate); return UavcanNode::start(node_id, bitrate); } else { From d62f3b8aa7a1ba932b932b26068b79bd14e205dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 May 2014 09:14:23 +0200 Subject: [PATCH 09/71] Added mixing code, not complete, not compiliing --- src/modules/uavcan/uavcan_main.cpp | 254 ++++++++++++++++++++++++++++- src/modules/uavcan/uavcan_main.hpp | 27 ++- 2 files changed, 273 insertions(+), 8 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ca4da1c2dc..94fb15544f 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -37,6 +37,9 @@ #include #include #include + +#include + #include "uavcan_main.hpp" /** @@ -52,6 +55,20 @@ */ UavcanNode *UavcanNode::_instance; +UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : + _node(can_driver, system_clock), + _controls({}), + _poll_fds({}) +{ + _control_topics[0] = ORB_ID(actuator_controls_0); + _control_topics[1] = ORB_ID(actuator_controls_1); + _control_topics[2] = ORB_ID(actuator_controls_2); + _control_topics[3] = ORB_ID(actuator_controls_3); + + memset(_controls, 0, sizeof(_controls)); + memset(_poll_fds, 0, sizeof(_poll_fds)); +} + int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { if (_instance != nullptr) { @@ -106,6 +123,14 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return node_init_res; } + int ret; + + /* do regular cdev init */ + ret = CDev::init(); + + if (ret != OK) + return ret; + /* * Start the task. Normally it should never exit. */ @@ -137,8 +162,94 @@ int UavcanNode::run() { _node.setStatusOk(); + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + while (true) { - // TODO: ORB multiplexing + + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + /* force setting update rate */ + _current_update_rate = 0; + } + + int ret = ::poll(_poll_fds, _poll_fds_num, 5/* 5 ms wait time */); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + usleep(1000000); + continue; + + } else if (ret == 0) { + /* timeout: no control data, switch to failsafe values */ +// warnx("no PWM: failsafe"); + + } else { + + /* get controls for required topics */ + unsigned poll_id = 0; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); + } + poll_id++; + } + } + + /* can we mix? */ + if (_mixers != nullptr) { + + // XXX one output group has 8 outputs max, + // but this driver could well serve multiple groups. + unsigned num_outputs_max = 8; + + // Do mixing + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max); + outputs.timestamp = hrt_absolute_time(); + + /* iterate actuators */ + for (unsigned i = 0; i < outputs.noutputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (!isfinite(outputs.output[i]) || + outputs.output[i] < -1.0f || + outputs.output[i] > 1.0f) { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = -1.0f; + } + } + + printf("CAN out: ") + /* output to the bus */ + for (unsigned i = 0; i < outputs.noutputs; i++) { + printf("%u: %8.4f ", i, outputs.output[i]) + // can_send_xxx + } + printf("\n"); + + } + } + + /* check arming state */ + bool updated = false; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + + /* update the armed status and check that we're not locked down */ + bool set_armed = _armed.armed && !_armed.lockdown; + + arm_actuators(set_armed); + } + + // Output commands and fetch data + const int res = _node.spin(uavcan::MonotonicDuration::getInfinite()); if (res < 0) { @@ -147,7 +258,146 @@ int UavcanNode::run() } } - return -1; + teardown(); + + exit(0); +} + +int +UavcanNode::control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + input = _controls[control_group].control[control_index]; + return 0; +} + +int +UavcanNode::teardown() +{ + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs > 0) { + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + } + ::close(_armed_sub); +} + +void +UavcanNode::arm_actuators(bool arm) +{ + bool changed = (_armed != arm); + + _armed = arm; + + if (changed) { + // Propagate immediately to CAN bus + } +} + +void +UavcanNode::subscribe() +{ + /* subscribe/unsubscribe to required actuator control groups */ + uint32_t sub_groups = _groups_required & ~_groups_subscribed; + uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + _poll_fds_num = 0; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (sub_groups & (1 << i)) { + warnx("subscribe to actuator_controls_%d", i); + _control_subs[i] = orb_subscribe(_control_topics[i]); + } + if (unsub_groups & (1 << i)) { + warnx("unsubscribe from actuator_controls_%d", i); + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + + if (_control_subs[i] > 0) { + _poll_fds[_poll_fds_num].fd = _control_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + } +} + +int +PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + lock(); + + switch (cmd) { + case PWM_SERVO_ARM: + arm_actuators(true); + break; + + case PWM_SERVO_SET_ARM_OK: + case PWM_SERVO_CLEAR_ARM_OK: + case PWM_SERVO_SET_FORCE_SAFETY_OFF: + // these are no-ops, as no safety switch + break; + + case PWM_SERVO_DISARM: + arm_actuators(false); + break; + + case MIXERIOCGETOUTPUTCOUNT: + *(unsigned *)arg = _output_count; + break; + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + } + + break; + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); + + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); + + if (_mixers == nullptr) { + _groups_required = 0; + ret = -ENOMEM; + + } else { + + ret = _mixers->load_from_buf(buf, buflen); + + if (ret != 0) { + debug("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + ret = -EINVAL; + } else { + + _mixers->groups_required(_groups_required); + } + } + + break; + } + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + return ret; } /* diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index b27449f1f4..213cb42061 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -34,6 +34,7 @@ #pragma once #include +#include /** * @file uavcan_main.hpp @@ -46,7 +47,7 @@ /** * A UAVCAN node. */ -class UavcanNode +class UavcanNode : public device::CDev { static constexpr unsigned MemPoolSize = 10752; static constexpr unsigned RxQueueLenPerIface = 64; @@ -56,18 +57,32 @@ public: typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; - UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) - : _node(can_driver, system_clock) - { } + UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); static int start(uavcan::NodeID node_id, uint32_t bitrate); Node &getNode() { return _node; } + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); + void subscribe(); + private: int init(uavcan::NodeID node_id); int run(); - static UavcanNode *_instance; ///< pointer to the library instance - Node _node; + static UavcanNode *_instance; ///< pointer to the library instance + Node _node; + + MixerGroup *_mixers; + + uint32_t _groups_required; + uint32_t _groups_subscribed; + int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; + actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; + unsigned _poll_fds_num; }; From 517f2df0d1de008c795061badc57fbfdbb68d0d5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 May 2014 13:28:41 +0200 Subject: [PATCH 10/71] UAVCAN: Fixed all compile errors --- src/modules/uavcan/uavcan_main.cpp | 126 ++++++++++++++++++++--------- src/modules/uavcan/uavcan_main.hpp | 46 ++++++++--- 2 files changed, 124 insertions(+), 48 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 94fb15544f..332c3a5e87 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -31,14 +31,18 @@ * ****************************************************************************/ +#include + #include #include #include #include +#include #include #include -#include +#include +#include #include "uavcan_main.hpp" @@ -55,7 +59,13 @@ */ UavcanNode *UavcanNode::_instance; -UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : +UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : + CDev("uavcan", UAVCAN_DEVICE_PATH), + _task(0), + _task_should_exit(false), + _armed_sub(-1), + _is_armed(false), + _output_count(0), _node(can_driver, system_clock), _controls({}), _poll_fds({}) @@ -65,8 +75,37 @@ UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : _control_topics[2] = ORB_ID(actuator_controls_2); _control_topics[3] = ORB_ID(actuator_controls_3); - memset(_controls, 0, sizeof(_controls)); - memset(_poll_fds, 0, sizeof(_poll_fds)); + // memset(_controls, 0, sizeof(_controls)); + // memset(_poll_fds, 0, sizeof(_poll_fds)); +} + +UavcanNode::~UavcanNode() +{ + if (_task != -1) { + /* tell the task we want it to go away */ + _task_should_exit = true; + + unsigned i = 10; + + do { + /* wait 50ms - it should wake every 100ms or so worst-case */ + usleep(50000); + + /* if we have given up, kill it */ + if (--i == 0) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + /* clean up the alternate device node */ + // unregister_driver(PWM_OUTPUT_DEVICE_PATH); + + ::close(_armed_sub); + + _instance = nullptr; } int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) @@ -123,6 +162,24 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return node_init_res; } + /* + * Start the task. Normally it should never exit. + */ + static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; + _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, + static_cast(run_trampoline), nullptr); + + if (_instance->_task < 0) { + warnx("start failed: %d", errno); + return -errno; + } + + return OK; +} + +int UavcanNode::init(uavcan::NodeID node_id) +{ + int ret; /* do regular cdev init */ @@ -131,16 +188,6 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) if (ret != OK) return ret; - /* - * Start the task. Normally it should never exit. - */ - static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; - return task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, - static_cast(run_trampoline), nullptr); -} - -int UavcanNode::init(uavcan::NodeID node_id) -{ uavcan::protocol::SoftwareVersion swver; swver.major = 12; // TODO fill version info swver.minor = 34; @@ -162,32 +209,35 @@ int UavcanNode::run() { _node.setStatusOk(); + // XXX figure out the output count + _output_count = 2; + + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - while (true) { - + actuator_outputs_s outputs; + memset(&outputs, 0, sizeof(outputs)); + + while (!_task_should_exit) { + if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; - /* force setting update rate */ - _current_update_rate = 0; } int ret = ::poll(_poll_fds, _poll_fds_num, 5/* 5 ms wait time */); - /* this would be bad... */ + // this would be bad... if (ret < 0) { log("poll error %d", errno); - usleep(1000000); continue; } else if (ret == 0) { - /* timeout: no control data, switch to failsafe values */ -// warnx("no PWM: failsafe"); + // timeout: no control data, switch to failsafe values } else { - /* get controls for required topics */ + // get controls for required topics unsigned poll_id = 0; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { @@ -198,7 +248,7 @@ int UavcanNode::run() } } - /* can we mix? */ + //can we mix? if (_mixers != nullptr) { // XXX one output group has 8 outputs max, @@ -209,9 +259,9 @@ int UavcanNode::run() outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max); outputs.timestamp = hrt_absolute_time(); - /* iterate actuators */ + // iterate actuators for (unsigned i = 0; i < outputs.noutputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ + // last resort: catch NaN, INF and out-of-band errors if (!isfinite(outputs.output[i]) || outputs.output[i] < -1.0f || outputs.output[i] > 1.0f) { @@ -224,25 +274,25 @@ int UavcanNode::run() } } - printf("CAN out: ") + printf("CAN out: "); /* output to the bus */ for (unsigned i = 0; i < outputs.noutputs; i++) { - printf("%u: %8.4f ", i, outputs.output[i]) + printf("%u: %8.4f ", i, outputs.output[i]); // can_send_xxx } - printf("\n"); + printf("%s\n", (_is_armed) ? "ARMED" : "DISARMED"); } } - /* check arming state */ + // Check arming state bool updated = false; orb_check(_armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - /* update the armed status and check that we're not locked down */ + // Update the armed status and check that we're not locked down bool set_armed = _armed.armed && !_armed.lockdown; arm_actuators(set_armed); @@ -271,7 +321,7 @@ UavcanNode::control_callback(uintptr_t handle, { const actuator_controls_s *controls = (actuator_controls_s *)handle; - input = _controls[control_group].control[control_index]; + input = controls[control_group].control[control_index]; return 0; } @@ -287,22 +337,24 @@ UavcanNode::teardown() ::close(_armed_sub); } -void +int UavcanNode::arm_actuators(bool arm) { - bool changed = (_armed != arm); + bool changed = (_is_armed != arm); - _armed = arm; + _is_armed = arm; if (changed) { // Propagate immediately to CAN bus } + + return OK; } void UavcanNode::subscribe() { - /* subscribe/unsubscribe to required actuator control groups */ + // Subscribe/unsubscribe to required actuator control groups uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; _poll_fds_num = 0; @@ -326,7 +378,7 @@ UavcanNode::subscribe() } int -PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) +UavcanNode::pwm_ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 213cb42061..857c1dc662 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -33,8 +33,14 @@ #pragma once +#include + #include -#include +#include + +#include +#include +#include /** * @file uavcan_main.hpp @@ -44,6 +50,9 @@ * @author Pavel Kirienko */ +#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 +#define UAVCAN_DEVICE_PATH "/dev/uavcan" + /** * A UAVCAN node. */ @@ -57,21 +66,36 @@ public: typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; - UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); + UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); - static int start(uavcan::NodeID node_id, uint32_t bitrate); + virtual ~UavcanNode(); - Node &getNode() { return _node; } + static int start(uavcan::NodeID node_id, uint32_t bitrate); + + Node& getNode() { return _node; } static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); - void subscribe(); + + void subscribe(); + + int teardown(); + int arm_actuators(bool arm); private: - int init(uavcan::NodeID node_id); - int run(); + int init(uavcan::NodeID node_id); + int run(); + int pwm_ioctl(file *filp, int cmd, unsigned long arg); + + int _task; ///< handle to the OS task + bool _task_should_exit; ///< flag to indicate to tear down the CAN driver + int _armed_sub; ///< uORB subscription of the arming status + actuator_armed_s _armed; ///< the arming request of the system + bool _is_armed; ///< the arming status of the actuators on the bus + + unsigned _output_count; ///< number of actuators currently available static UavcanNode *_instance; ///< pointer to the library instance Node _node; @@ -80,9 +104,9 @@ private: uint32_t _groups_required; uint32_t _groups_subscribed; - int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; - actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; - orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; + int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; + actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; + orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; unsigned _poll_fds_num; }; From 185c95fda6acac869c1821846d44359faeef22d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 May 2014 13:57:23 +0200 Subject: [PATCH 11/71] UAVCAN: improve printing, ready for full closed loop test --- src/modules/uavcan/uavcan_main.cpp | 46 ++++++++++++++++++++++++++---- src/modules/uavcan/uavcan_main.hpp | 9 ++++-- 2 files changed, 47 insertions(+), 8 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 332c3a5e87..a86314852e 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -68,7 +68,11 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys _output_count(0), _node(can_driver, system_clock), _controls({}), - _poll_fds({}) + _poll_fds({}), + _mixers(nullptr), + _groups_required(0), + _groups_subscribed(0), + _poll_fds_num(0) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); @@ -225,7 +229,7 @@ int UavcanNode::run() _groups_subscribed = _groups_required; } - int ret = ::poll(_poll_fds, _poll_fds_num, 5/* 5 ms wait time */); + int ret = ::poll(_poll_fds, _poll_fds_num, 50/* 50 ms wait time */); // this would be bad... if (ret < 0) { @@ -234,6 +238,7 @@ int UavcanNode::run() } else if (ret == 0) { // timeout: no control data, switch to failsafe values + // XXX trigger failsafe } else { @@ -278,11 +283,12 @@ int UavcanNode::run() /* output to the bus */ for (unsigned i = 0; i < outputs.noutputs; i++) { printf("%u: %8.4f ", i, outputs.output[i]); - // can_send_xxx + // XXX send out via CAN here } printf("%s\n", (_is_armed) ? "ARMED" : "DISARMED"); } + } // Check arming state @@ -300,7 +306,7 @@ int UavcanNode::run() // Output commands and fetch data - const int res = _node.spin(uavcan::MonotonicDuration::getInfinite()); + const int res = _node.spin(uavcan::MonotonicDuration::fromUSec(5000)); if (res < 0) { warnx("Spin error %i", res); @@ -309,6 +315,7 @@ int UavcanNode::run() } teardown(); + warnx("exiting."); exit(0); } @@ -378,7 +385,7 @@ UavcanNode::subscribe() } int -UavcanNode::pwm_ioctl(file *filp, int cmd, unsigned long arg) +UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; @@ -428,7 +435,7 @@ UavcanNode::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = _mixers->load_from_buf(buf, buflen); if (ret != 0) { - debug("mixer load failed with %d", ret); + warnx("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; _groups_required = 0; @@ -449,9 +456,24 @@ UavcanNode::pwm_ioctl(file *filp, int cmd, unsigned long arg) unlock(); + if (ret == -ENOTTY) { + ret = CDev::ioctl(filp, cmd, arg); + } + return ret; } +void +UavcanNode::print_info() +{ + if (!_instance) { + warnx("not running, start first"); + } + + warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK"); +} + /* * App entry point */ @@ -511,5 +533,17 @@ int uavcan_main(int argc, char *argv[]) ::exit(1); } + /* commands below require the app to be started */ + UavcanNode *inst = UavcanNode::instance(); + + if (!inst) { + errx(1, "application not running"); + } + + if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { + + inst->print_info(); + } + return 0; } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 857c1dc662..97598ddf3c 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -66,10 +66,12 @@ public: typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; - UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); + UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); virtual ~UavcanNode(); + virtual int ioctl(file *filp, int cmd, unsigned long arg); + static int start(uavcan::NodeID node_id, uint32_t bitrate); Node& getNode() { return _node; } @@ -84,10 +86,13 @@ public: int teardown(); int arm_actuators(bool arm); + void print_info(); + + static UavcanNode* instance() { return _instance; } + private: int init(uavcan::NodeID node_id); int run(); - int pwm_ioctl(file *filp, int cmd, unsigned long arg); int _task; ///< handle to the OS task bool _task_should_exit; ///< flag to indicate to tear down the CAN driver From f70db56e90972cf0492ac9d295c3d0f5df87aa66 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 May 2014 14:14:52 +0200 Subject: [PATCH 12/71] UAVCAN: Fix start / stop commands --- src/modules/uavcan/uavcan_main.cpp | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index a86314852e..9cd486bf5d 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -92,8 +92,8 @@ UavcanNode::~UavcanNode() unsigned i = 10; do { - /* wait 50ms - it should wake every 100ms or so worst-case */ - usleep(50000); + /* wait 5ms - it should wake every 10ms or so worst-case */ + usleep(5000); /* if we have given up, kill it */ if (--i == 0) { @@ -522,15 +522,16 @@ int uavcan_main(int argc, char *argv[]) bitrate = DEFAULT_CAN_BITRATE; } + if (UavcanNode::instance()) { + errx(1, "already started"); + } + /* * Start */ warnx("Node ID %u, bitrate %u", node_id, bitrate); return UavcanNode::start(node_id, bitrate); - } else { - print_usage(); - ::exit(1); } /* commands below require the app to be started */ @@ -543,7 +544,16 @@ int uavcan_main(int argc, char *argv[]) if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { inst->print_info(); + return OK; } - return 0; + if (!std::strcmp(argv[1], "stop")) { + + delete inst; + inst = nullptr; + return OK; + } + + print_usage(); + ::exit(1); } From ec5602e45d51e500327bd3aa08d4a3d678573936 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 May 2014 14:23:33 +0200 Subject: [PATCH 13/71] UAVCAN quad X autostart setup --- ROMFS/px4fmu_common/init.d/4012_quad_x_can | 27 ++++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 5 ++++ ROMFS/px4fmu_common/init.d/rc.interface | 5 ++++ ROMFS/px4fmu_common/init.d/rcS | 12 +++++++++- 4 files changed, 48 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/init.d/4012_quad_x_can diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can new file mode 100644 index 0000000000..471ac54b4d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -0,0 +1,27 @@ +#!nsh +# +# F450-sized quadrotor with CAN +# +# Lorenz Meier +# + +sh /etc/init.d/4001_quad_x + +if [ $DO_AUTOCONFIG == yes ] +then + # TODO REVIEW + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.003 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.003 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 +fi + +set OUTPUT_MODE can diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 5ec735d393..3d0de950d2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -136,6 +136,11 @@ then sh /etc/init.d/4011_dji_f450 fi +if param compare SYS_AUTOSTART 4012 +then + sh /etc/init.d/4012_quad_x_can +fi + # # Quad + # diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 7f793b2198..d6f1b54bc1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -24,6 +24,11 @@ then else set OUTPUT_DEV /dev/pwm_output fi + + if [ $OUTPUT_MODE == can ] + then + set OUTPUT_DEV /dev/uavcan + fi if mixer load $OUTPUT_DEV $MIXER_FILE then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 756ee8ef88..5d76e4283e 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -284,7 +284,17 @@ then # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output if [ $OUTPUT_MODE != none ] then - if [ $OUTPUT_MODE == io ] + if [ $OUTPUT_MODE == can ] + then + if uavcan start 1 + then + echo "CAN UP" + else + echo "CAN ERR" + fi + fi + + if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == can ] then echo "[init] Use PX4IO PWM as primary output" if px4io start From 4055833c9e6b6ecadab44d231047c44796ff17bc Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 8 May 2014 17:03:40 +0400 Subject: [PATCH 14/71] UAVCAN mixer renamed to /dev/uavcan/esc --- ROMFS/px4fmu_common/init.d/rc.interface | 2 +- src/modules/uavcan/uavcan_main.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index d6f1b54bc1..da4c734704 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -27,7 +27,7 @@ then if [ $OUTPUT_MODE == can ] then - set OUTPUT_DEV /dev/uavcan + set OUTPUT_DEV /dev/uavcan/esc fi if mixer load $OUTPUT_DEV $MIXER_FILE diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 97598ddf3c..beaa5e1d42 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -51,7 +51,7 @@ */ #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 -#define UAVCAN_DEVICE_PATH "/dev/uavcan" +#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" /** * A UAVCAN node. From f4c28473f9038875a56eace4b9b7364694bb03df Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 8 May 2014 17:12:05 +0400 Subject: [PATCH 15/71] Warning fixes --- src/modules/uavcan/uavcan_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 9cd486bf5d..859db93c78 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -336,12 +336,12 @@ int UavcanNode::teardown() { for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs > 0) { + if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; } } - ::close(_armed_sub); + return ::close(_armed_sub); } int From 4a98dae227f3e60f1a220164bce0b995ce303f3d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 8 May 2014 19:42:20 +0400 Subject: [PATCH 16/71] UAVCAN ESC controller - proof of concept state --- src/modules/uavcan/esc_controller.cpp | 124 ++++++++++++++++++++++++++ src/modules/uavcan/esc_controller.hpp | 99 ++++++++++++++++++++ src/modules/uavcan/module.mk | 5 +- src/modules/uavcan/uavcan_main.cpp | 47 +++++----- src/modules/uavcan/uavcan_main.hpp | 35 ++++---- 5 files changed, 265 insertions(+), 45 deletions(-) create mode 100644 src/modules/uavcan/esc_controller.cpp create mode 100644 src/modules/uavcan/esc_controller.hpp diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp new file mode 100644 index 0000000000..bde4d2a7fa --- /dev/null +++ b/src/modules/uavcan/esc_controller.cpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * 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 esc_controller.cpp + * + * @author Pavel Kirienko + */ + +#include "esc_controller.hpp" +#include + +UavcanEscController::UavcanEscController(uavcan::INode &node) : + _node(node), + _uavcan_pub_raw_cmd(node), + _uavcan_sub_status(node), + _orb_timer(node) +{ +} + +int UavcanEscController::init() +{ + int res = -1; + + // ESC status subscription + res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); + if (res < 0) + { + warnx("ESC status sub failed %i", res); + return res; + } + + // ESC status will be relayed from UAVCAN bus into ORB at this rate + _orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb)); + _orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ)); + + return res; +} + +void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) +{ + assert(outputs != nullptr); + assert(num_outputs <= MAX_ESCS); + + /* + * Rate limiting - we don't want to congest the bus + */ + const auto timestamp = _node.getMonotonicTime(); + if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { + return; + } + _prev_cmd_pub = timestamp; + + /* + * Fill the command message + * If unarmed, we publish an empty message anyway + */ + uavcan::equipment::esc::RawCommand msg; + + if (_armed) { + for (unsigned i = 0; i < num_outputs; i++) { + + float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; + if (scaled < 1.0F) + scaled = 1.0F; // Since we're armed, we don't want to stop it completely + + assert(scaled >= uavcan::equipment::esc::RawCommand::CMD_MIN); + assert(scaled <= uavcan::equipment::esc::RawCommand::CMD_MAX); + + msg.cmd.push_back(scaled); + } + } + + /* + * Publish the command message to the bus + * Note that for a quadrotor it takes one CAN frame + */ + (void)_uavcan_pub_raw_cmd.broadcast(msg); +} + +void UavcanEscController::arm_esc(bool arm) +{ + _armed = arm; +} + +void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + // TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb() +} + +void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) +{ + // TODO publish to ORB +} diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/esc_controller.hpp new file mode 100644 index 0000000000..0ed0c59b56 --- /dev/null +++ b/src/modules/uavcan/esc_controller.hpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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 esc_controller.hpp + * + * UAVCAN <--> ORB bridge for ESC messages: + * uavcan.equipment.esc.RawCommand + * uavcan.equipment.esc.RPMCommand + * uavcan.equipment.esc.Status + * + * @author Pavel Kirienko + */ + +#pragma once + +#include +#include +#include + +class UavcanEscController +{ +public: + UavcanEscController(uavcan::INode& node); + + int init(); + + void update_outputs(float *outputs, unsigned num_outputs); + + void arm_esc(bool arm); + +private: + /** + * ESC status message reception will be reported via this callback. + */ + void esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg); + + /** + * ESC status will be published to ORB from this callback (fixed rate). + */ + void orb_pub_timer_cb(const uavcan::TimerEvent &event); + + + static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable + static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5; + static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; + + typedef uavcan::MethodBinder&)> + StatusCbBinder; + + typedef uavcan::MethodBinder + TimerCbBinder; + + /* + * libuavcan related things + */ + uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting + uavcan::INode &_node; + uavcan::Publisher _uavcan_pub_raw_cmd; + uavcan::Subscriber _uavcan_sub_status; + uavcan::TimerEventForwarder _orb_timer; + + /* + * ESC states + */ + bool _armed = false; + uavcan::equipment::esc::Status _states[MAX_ESCS]; +}; diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 0985438796..ce9f981a88 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,8 +40,9 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -Os -SRCS += uavcan_main.cpp \ - uavcan_clock.cpp +SRCS += uavcan_main.cpp \ + uavcan_clock.cpp \ + esc_controller.cpp # # libuavcan diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 859db93c78..e7829cbba9 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -61,18 +61,8 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), - _task(0), - _task_should_exit(false), - _armed_sub(-1), - _is_armed(false), - _output_count(0), _node(can_driver, system_clock), - _controls({}), - _poll_fds({}), - _mixers(nullptr), - _groups_required(0), - _groups_subscribed(0), - _poll_fds_num(0) + _esc_controller(_node) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); @@ -183,8 +173,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) int UavcanNode::init(uavcan::NodeID node_id) { - - int ret; + int ret = -1; /* do regular cdev init */ ret = CDev::init(); @@ -192,6 +181,10 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret != OK) return ret; + ret = _esc_controller.init(); + if (ret < 0) + return ret; + uavcan::protocol::SoftwareVersion swver; swver.major = 12; // TODO fill version info swver.minor = 34; @@ -222,6 +215,11 @@ int UavcanNode::run() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); + /* + * XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update(); + * IO multiplexing shall be done here. + */ + while (!_task_should_exit) { if (_groups_subscribed != _groups_required) { @@ -279,14 +277,16 @@ int UavcanNode::run() } } + /* + * Output to the bus + */ printf("CAN out: "); - /* output to the bus */ for (unsigned i = 0; i < outputs.noutputs; i++) { printf("%u: %8.4f ", i, outputs.output[i]); - // XXX send out via CAN here } printf("%s\n", (_is_armed) ? "ARMED" : "DISARMED"); + _esc_controller.update_outputs(outputs.output, outputs.noutputs); } } @@ -304,13 +304,12 @@ int UavcanNode::run() arm_actuators(set_armed); } - // Output commands and fetch data + // Output commands and fetch data TODO ORB multiplexing - const int res = _node.spin(uavcan::MonotonicDuration::fromUSec(5000)); + const int spin_res = _node.spin(uavcan::MonotonicDuration::fromMSec(1)); - if (res < 0) { - warnx("Spin error %i", res); - ::sleep(1); + if (spin_res < 0) { + warnx("node spin error %i", spin_res); } } @@ -347,14 +346,8 @@ UavcanNode::teardown() int UavcanNode::arm_actuators(bool arm) { - bool changed = (_is_armed != arm); - _is_armed = arm; - - if (changed) { - // Propagate immediately to CAN bus - } - + _esc_controller.arm_esc(arm); return OK; } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index beaa5e1d42..f4a709c793 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -42,6 +42,8 @@ #include #include +#include "esc_controller.hpp" + /** * @file uavcan_main.hpp * @@ -94,24 +96,25 @@ private: int init(uavcan::NodeID node_id); int run(); - int _task; ///< handle to the OS task - bool _task_should_exit; ///< flag to indicate to tear down the CAN driver - int _armed_sub; ///< uORB subscription of the arming status - actuator_armed_s _armed; ///< the arming request of the system - bool _is_armed; ///< the arming status of the actuators on the bus + int _task = -1; ///< handle to the OS task + bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver + int _armed_sub = -1; ///< uORB subscription of the arming status + actuator_armed_s _armed; ///< the arming request of the system + bool _is_armed = false; ///< the arming status of the actuators on the bus - unsigned _output_count; ///< number of actuators currently available + unsigned _output_count = 0; ///< number of actuators currently available - static UavcanNode *_instance; ///< pointer to the library instance - Node _node; + static UavcanNode *_instance; ///< singleton pointer + Node _node; ///< library instance + UavcanEscController _esc_controller; - MixerGroup *_mixers; + MixerGroup *_mixers = nullptr; - uint32_t _groups_required; - uint32_t _groups_subscribed; - int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; - actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; - orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; - unsigned _poll_fds_num; + uint32_t _groups_required = 0; + uint32_t _groups_subscribed = 0; + int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + unsigned _poll_fds_num = 0; }; From c697aae17a32f25b2f163282b9cb18efedb14d77 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 8 May 2014 23:34:23 +0400 Subject: [PATCH 17/71] Proper IO miltiplexing libuavcan + ORB --- src/modules/uavcan/uavcan_main.cpp | 68 ++++++++++++++++++------------ src/modules/uavcan/uavcan_main.hpp | 3 +- 2 files changed, 44 insertions(+), 27 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index e7829cbba9..6ba596ad05 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -202,9 +203,17 @@ int UavcanNode::init(uavcan::NodeID node_id) return _node.start(); } +void UavcanNode::node_spin_once() +{ + const int spin_res = _node.spin(uavcan::MonotonicTime()); + if (spin_res < 0) { + warnx("node spin error %i", spin_res); + } +} + int UavcanNode::run() { - _node.setStatusOk(); + const unsigned PollTimeoutMs = 50; // XXX figure out the output count _output_count = 2; @@ -215,42 +224,65 @@ int UavcanNode::run() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); + const int busevent_fd = ::open(uavcan_stm32::Event::DevName, 0); + if (busevent_fd < 0) + { + warnx("Failed to open %s", uavcan_stm32::Event::DevName); + _task_should_exit = true; + } + /* * XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update(); * IO multiplexing shall be done here. */ + _node.setStatusOk(); + while (!_task_should_exit) { if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; + /* + * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). + * Actual event type (POLLIN/POLLOUT/...) doesn't matter here. + * Please note that with such multiplexing it is no longer possible to rely only on + * the value returned from poll() to detect whether actuator control has timed out or not. + * Instead, all ORB events need to be checked individually (see below). + */ + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = busevent_fd; + _poll_fds[_poll_fds_num].events = POLLIN | POLLOUT; + _poll_fds_num += 1; } - int ret = ::poll(_poll_fds, _poll_fds_num, 50/* 50 ms wait time */); + const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); + + node_spin_once(); // Non-blocking // this would be bad... - if (ret < 0) { + if (poll_ret < 0) { log("poll error %d", errno); continue; - - } else if (ret == 0) { - // timeout: no control data, switch to failsafe values - // XXX trigger failsafe - } else { - // get controls for required topics + bool controls_updated = false; unsigned poll_id = 0; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { + controls_updated = true; orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } poll_id++; } } + if (!controls_updated) { + // timeout: no control data, switch to failsafe values + // XXX trigger failsafe + } + //can we mix? if (_mixers != nullptr) { @@ -277,15 +309,7 @@ int UavcanNode::run() } } - /* - * Output to the bus - */ - printf("CAN out: "); - for (unsigned i = 0; i < outputs.noutputs; i++) { - printf("%u: %8.4f ", i, outputs.output[i]); - } - printf("%s\n", (_is_armed) ? "ARMED" : "DISARMED"); - + // Output to the bus _esc_controller.update_outputs(outputs.output, outputs.noutputs); } @@ -303,14 +327,6 @@ int UavcanNode::run() arm_actuators(set_armed); } - - // Output commands and fetch data TODO ORB multiplexing - - const int spin_res = _node.spin(uavcan::MonotonicDuration::fromMSec(1)); - - if (spin_res < 0) { - warnx("node spin error %i", spin_res); - } } teardown(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index f4a709c793..751a94a8a7 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -94,6 +94,7 @@ public: private: int init(uavcan::NodeID node_id); + void node_spin_once(); int run(); int _task = -1; ///< handle to the OS task @@ -115,6 +116,6 @@ private: int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent unsigned _poll_fds_num = 0; }; From 5a905825675a33b210469eff96f0b82a8bd70eb9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 9 May 2014 02:18:45 +0400 Subject: [PATCH 18/71] Catching up with STM32 driver optimizations in libuavcan --- src/modules/uavcan/uavcan_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 6ba596ad05..d4a0d894a2 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -224,10 +224,10 @@ int UavcanNode::run() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); - const int busevent_fd = ::open(uavcan_stm32::Event::DevName, 0); + const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); if (busevent_fd < 0) { - warnx("Failed to open %s", uavcan_stm32::Event::DevName); + warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName); _task_should_exit = true; } @@ -252,7 +252,7 @@ int UavcanNode::run() */ _poll_fds[_poll_fds_num] = ::pollfd(); _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN | POLLOUT; + _poll_fds[_poll_fds_num].events = POLLIN; _poll_fds_num += 1; } From 8501158427d7cf96b125eafe48193f654c7fb2f0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 9 May 2014 02:23:52 +0400 Subject: [PATCH 19/71] Micro optimization in UAVCAN polling loop --- src/modules/uavcan/uavcan_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index d4a0d894a2..73f519fa1f 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -284,7 +284,7 @@ int UavcanNode::run() } //can we mix? - if (_mixers != nullptr) { + if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, // but this driver could well serve multiple groups. From 4edc432f399297ed6e74685408e80c0640873099 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 9 May 2014 02:24:46 +0400 Subject: [PATCH 20/71] Removed misleading comment --- src/modules/uavcan/uavcan_main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 73f519fa1f..ab687a6b9d 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -245,7 +245,6 @@ int UavcanNode::run() _groups_subscribed = _groups_required; /* * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). - * Actual event type (POLLIN/POLLOUT/...) doesn't matter here. * Please note that with such multiplexing it is no longer possible to rely only on * the value returned from poll() to detect whether actuator control has timed out or not. * Instead, all ORB events need to be checked individually (see below). From be33b4b6a56402135e5cfb8b38e0cc0ae8b7d673 Mon Sep 17 00:00:00 2001 From: holger Date: Tue, 24 Jun 2014 19:28:39 +0200 Subject: [PATCH 21/71] UAVCAN: append to EXTRADEFINES to those given by make cmd line --- src/modules/uavcan/module.mk | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index ce9f981a88..5ac7019e3b 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -52,7 +52,7 @@ SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. -EXTRADEFINES += -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 +override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 # # libuavcan drivers for STM32 @@ -60,8 +60,7 @@ EXTRADEFINES += -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk SRCS += $(LIBUAVCAN_STM32_SRC) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) -EXTRADEFINES += -DUAVCAN_STM32_NUTTX \ - -DUAVCAN_STM32_NUM_IFACES=2 +override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 # # Invoke DSDL compiler From 8acbe6d5b6770e92fdcb86ba268492217d3e26bd Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Tue, 1 Jul 2014 14:08:59 -0700 Subject: [PATCH 22/71] Added class to convert gnss message from uavcan to uorb --- src/modules/uavcan/gnss_receiver.cpp | 122 +++++++++++++++++++++++++++ src/modules/uavcan/gnss_receiver.hpp | 86 +++++++++++++++++++ src/modules/uavcan/uavcan_main.cpp | 7 +- src/modules/uavcan/uavcan_main.hpp | 2 + 4 files changed, 216 insertions(+), 1 deletion(-) create mode 100644 src/modules/uavcan/gnss_receiver.cpp create mode 100644 src/modules/uavcan/gnss_receiver.hpp diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp new file mode 100644 index 0000000000..e924683330 --- /dev/null +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * 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 gnss_receiver.cpp + * + * @author Pavel Kirienko + * @author Andrew Chambers + * + */ + +#include "gnss_receiver.hpp" +#include + +#define MM_PER_CM 10 // Millimeters per centimeter + +UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : + _node(node), + _uavcan_sub_status(node), + _report_pub(-1) +{ +} + +int UavcanGnssReceiver::init() +{ + int res = -1; + + // GNSS fix subscription + res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb)); + if (res < 0) + { + warnx("GNSS fix sub failed %i", res); + return res; + } + + // Clear the uORB GPS report + memset(&_report, 0, sizeof(_report)); + + return res; +} + +void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + _report.timestamp_position = hrt_absolute_time(); + _report.lat = msg.lat_1e7; + _report.lon = msg.lon_1e7; + _report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3) + + _report.timestamp_variance = _report.timestamp_position; + _report.s_variance_m_s = msg.velocity_covariance[0] + msg.velocity_covariance[4] + msg.velocity_covariance[8]; + _report.p_variance_m = msg.position_covariance[0] + msg.position_covariance[4]; + + /* Use Jacobian to transform velocity covariance to heading covariance + * heading = atan2(vel_e_m_s, vel_n_m_s) + * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative + * + * To calculate the variance of heading from the variance of velocity, + * var(heading) = J(velocity)*var(velocity)*J(velocity)^T + */ + _report.c_variance_rad = + msg.ned_velocity[1] * msg.ned_velocity[1] * msg.velocity_covariance[0] + + -2*msg.ned_velocity[1] * msg.ned_velocity[0] * msg.velocity_covariance[1] + + msg.ned_velocity[0] * msg.ned_velocity[0] * msg.velocity_covariance[4]; + + _report.fix_type = msg.status; + + _report.eph_m = sqrtf(_report.p_variance_m); + _report.epv_m = sqrtf(msg.position_covariance[8]); + + _report.timestamp_velocity = _report.timestamp_position; + _report.vel_n_m_s = msg.ned_velocity[0]; + _report.vel_e_m_s = msg.ned_velocity[1]; + _report.vel_d_m_s = msg.ned_velocity[2]; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s); + _report.vel_ned_valid = true; + + _report.timestamp_time = _report.timestamp_position; + _report.time_gps_usec = msg.timestamp.husec * msg.timestamp.USEC_PER_LSB; // Convert to microseconds + + _report.timestamp_satellites = _report.timestamp_position; + _report.satellites_visible = msg.sats_used; + _report.satellite_info_available = 0; // Set to 0 for no info available + + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + +} diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/gnss_receiver.hpp new file mode 100644 index 0000000000..abb8a821af --- /dev/null +++ b/src/modules/uavcan/gnss_receiver.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * 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 gnss_receiver.hpp + * + * UAVCAN <--> ORB bridge for ESC messages: + * uavcan.equipment.esc.RawCommand + * uavcan.equipment.esc.RPMCommand + * uavcan.equipment.esc.Status + * + * @author Pavel Kirienko + * @author Andrew Chambers + */ + +#pragma once + +#include + +#include +#include + +#include +#include + +class UavcanGnssReceiver +{ +public: + UavcanGnssReceiver(uavcan::INode& node); + + int init(); + +private: + /** + * GNSS fix message will be reported via this callback. + */ + void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); + + + typedef uavcan::MethodBinder&)> + FixCbBinder; + + /* + * libuavcan related things + */ + uavcan::INode &_node; + uavcan::Subscriber _uavcan_sub_status; + + /* + * uORB + */ + struct vehicle_gps_position_s _report; ///< uORB topic for gnss position + orb_advert_t _report_pub; ///< uORB pub for gnss position + +}; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ab687a6b9d..5bc4376709 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -63,7 +63,8 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), - _esc_controller(_node) + _esc_controller(_node), + _gnss_receiver(_node) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); @@ -186,6 +187,10 @@ int UavcanNode::init(uavcan::NodeID node_id) if (ret < 0) return ret; + ret = _gnss_receiver.init(); + if (ret < 0) + return ret; + uavcan::protocol::SoftwareVersion swver; swver.major = 12; // TODO fill version info swver.minor = 34; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 751a94a8a7..126d441377 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -43,6 +43,7 @@ #include #include "esc_controller.hpp" +#include "gnss_receiver.hpp" /** * @file uavcan_main.hpp @@ -108,6 +109,7 @@ private: static UavcanNode *_instance; ///< singleton pointer Node _node; ///< library instance UavcanEscController _esc_controller; + UavcanGnssReceiver _gnss_receiver; MixerGroup *_mixers = nullptr; From 6c6de9395818717916bbc1077fecd51c4db87936 Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Wed, 2 Jul 2014 10:04:07 -0700 Subject: [PATCH 23/71] Fixed heading covariance calculation and build errors. --- src/modules/uavcan/gnss_receiver.cpp | 17 ++++++++++++----- src/modules/uavcan/module.mk | 3 ++- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index e924683330..490e35bd17 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -80,17 +80,24 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure Date: Wed, 2 Jul 2014 11:18:30 -0700 Subject: [PATCH 24/71] Fixed bug with zero-sized covariance arrays --- src/modules/uavcan/gnss_receiver.cpp | 75 ++++++++++++++++++---------- src/modules/uavcan/uavcan_main.cpp | 23 +++++---- 2 files changed, 60 insertions(+), 38 deletions(-) diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 490e35bd17..23c2215652 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -42,12 +42,13 @@ #include "gnss_receiver.hpp" #include -#define MM_PER_CM 10 // Millimeters per centimeter +#define MM_PER_CM 10 // Millimeters per centimeter +#define EXPECTED_COV_SIZE 9 // Expect a 3x3 matrix for position and velocity covariance UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : - _node(node), - _uavcan_sub_status(node), - _report_pub(-1) +_node(node), +_uavcan_sub_status(node), +_report_pub(-1) { } @@ -77,33 +78,53 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0); + bool valid_velocity_covariance = (msg.velocity_covariance.size() == EXPECTED_COV_SIZE && + msg.velocity_covariance[0] > 0); + + if (valid_position_covariance) { + _report.p_variance_m = msg.position_covariance[0] + msg.position_covariance[4]; + _report.eph_m = sqrtf(_report.p_variance_m); + } else { + _report.p_variance_m = -1.0; + _report.eph_m = -1.0; + } + + if (valid_velocity_covariance) { + _report.s_variance_m_s = msg.velocity_covariance[0] + msg.velocity_covariance[4] + msg.velocity_covariance[8]; + + /* There is a nonlinear relationship between the velocity vector and the heading. + * Use Jacobian to transform velocity covariance to heading covariance + * + * Nonlinear equation: + * heading = atan2(vel_e_m_s, vel_n_m_s) + * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative + * + * To calculate the variance of heading from the variance of velocity, + * cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T + */ + float vel_n = msg.ned_velocity[0]; + float vel_e = msg.ned_velocity[1]; + float vel_n_sq = vel_n * vel_n; + float vel_e_sq = vel_e * vel_e; + _report.c_variance_rad = + (vel_e_sq * msg.velocity_covariance[0] + + -2 * vel_n * vel_e * msg.velocity_covariance[1] + // Covariance matrix is symmetric + vel_n_sq* msg.velocity_covariance[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq)); + + _report.epv_m = sqrtf(msg.position_covariance[8]); + + } else { + _report.s_variance_m_s = -1.0; + _report.c_variance_rad = -1.0; + _report.epv_m = -1.0; + } _report.fix_type = msg.status; - _report.eph_m = sqrtf(_report.p_variance_m); - _report.epv_m = sqrtf(msg.position_covariance[8]); - _report.timestamp_velocity = _report.timestamp_position; _report.vel_n_m_s = msg.ned_velocity[0]; _report.vel_e_m_s = msg.ned_velocity[1]; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 5bc4376709..c0c07be536 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -243,21 +243,22 @@ int UavcanNode::run() _node.setStatusOk(); + /* + * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). + * Please note that with such multiplexing it is no longer possible to rely only on + * the value returned from poll() to detect whether actuator control has timed out or not. + * Instead, all ORB events need to be checked individually (see below). + */ + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = busevent_fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; + while (!_task_should_exit) { if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; - /* - * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). - * Please note that with such multiplexing it is no longer possible to rely only on - * the value returned from poll() to detect whether actuator control has timed out or not. - * Instead, all ORB events need to be checked individually (see below). - */ - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; } const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); @@ -271,7 +272,7 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - unsigned poll_id = 0; + unsigned poll_id = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { From 607b6511a413b5bc2b2b0ae350a9451e83da9803 Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Wed, 2 Jul 2014 11:27:49 -0700 Subject: [PATCH 25/71] Fixed comments --- src/modules/uavcan/gnss_receiver.hpp | 6 ++---- src/modules/uavcan/uavcan_main.hpp | 2 +- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/gnss_receiver.hpp index abb8a821af..18df8da2f5 100644 --- a/src/modules/uavcan/gnss_receiver.hpp +++ b/src/modules/uavcan/gnss_receiver.hpp @@ -34,10 +34,8 @@ /** * @file gnss_receiver.hpp * - * UAVCAN <--> ORB bridge for ESC messages: - * uavcan.equipment.esc.RawCommand - * uavcan.equipment.esc.RPMCommand - * uavcan.equipment.esc.Status + * UAVCAN --> ORB bridge for GNSS messages: + * uavcan.equipment.gnss.Fix * * @author Pavel Kirienko * @author Andrew Chambers diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 126d441377..443525379e 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -109,7 +109,7 @@ private: static UavcanNode *_instance; ///< singleton pointer Node _node; ///< library instance UavcanEscController _esc_controller; - UavcanGnssReceiver _gnss_receiver; + UavcanGnssReceiver _gnss_receiver; MixerGroup *_mixers = nullptr; From 6c5e3d53412fa1cdad687818328b3bfc1a83e9ca Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Wed, 2 Jul 2014 19:06:30 -0700 Subject: [PATCH 26/71] Address Paval's comments regarding extracting matrix from uavcan msg, position covariance calculation, and _poll_fds_num --- src/modules/uavcan/gnss_receiver.cpp | 28 +++++++++++++++------------- src/modules/uavcan/uavcan_main.cpp | 23 +++++++++++------------ 2 files changed, 26 insertions(+), 25 deletions(-) diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 23c2215652..65a7b4a2a3 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -43,7 +43,6 @@ #include #define MM_PER_CM 10 // Millimeters per centimeter -#define EXPECTED_COV_SIZE 9 // Expect a 3x3 matrix for position and velocity covariance UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : _node(node), @@ -75,18 +74,19 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0); - bool valid_velocity_covariance = (msg.velocity_covariance.size() == EXPECTED_COV_SIZE && - msg.velocity_covariance[0] > 0); + + // Check if the msg contains valid covariance information + const bool valid_position_covariance = !msg.position_covariance.empty(); + const bool valid_velocity_covariance = !msg.velocity_covariance.empty(); if (valid_position_covariance) { - _report.p_variance_m = msg.position_covariance[0] + msg.position_covariance[4]; + float pos_cov[9]; + msg.position_covariance.unpackSquareMatrix(pos_cov); + _report.p_variance_m = std::max(pos_cov[0], pos_cov[4]); _report.eph_m = sqrtf(_report.p_variance_m); } else { _report.p_variance_m = -1.0; @@ -94,7 +94,9 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index c0c07be536..5bc4376709 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -243,22 +243,21 @@ int UavcanNode::run() _node.setStatusOk(); - /* - * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). - * Please note that with such multiplexing it is no longer possible to rely only on - * the value returned from poll() to detect whether actuator control has timed out or not. - * Instead, all ORB events need to be checked individually (see below). - */ - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; - while (!_task_should_exit) { if (_groups_subscribed != _groups_required) { subscribe(); _groups_subscribed = _groups_required; + /* + * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error). + * Please note that with such multiplexing it is no longer possible to rely only on + * the value returned from poll() to detect whether actuator control has timed out or not. + * Instead, all ORB events need to be checked individually (see below). + */ + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = busevent_fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; } const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); @@ -272,7 +271,7 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - unsigned poll_id = 1; + unsigned poll_id = 0; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { From c6c33142ceb6bf59b8c9b8e32e94ae5ea7959dbd Mon Sep 17 00:00:00 2001 From: Andrew Chambers Date: Thu, 3 Jul 2014 11:32:27 -0700 Subject: [PATCH 27/71] Using proper math library. Corrected speed variance calculation --- src/modules/uavcan/gnss_receiver.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 65a7b4a2a3..3e98bdf144 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -41,6 +41,7 @@ #include "gnss_receiver.hpp" #include +#include #define MM_PER_CM 10 // Millimeters per centimeter @@ -86,7 +87,7 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure Date: Sun, 6 Jul 2014 15:33:54 +0200 Subject: [PATCH 28/71] Code style improvement, fix linter warning --- src/modules/uavcan/esc_controller.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp index bde4d2a7fa..f603d94482 100644 --- a/src/modules/uavcan/esc_controller.cpp +++ b/src/modules/uavcan/esc_controller.cpp @@ -50,10 +50,8 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : int UavcanEscController::init() { - int res = -1; - // ESC status subscription - res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); + int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb)); if (res < 0) { warnx("ESC status sub failed %i", res); From 2669f7f3af65921d4abbf3850cd62e48f2eeeec7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Jul 2014 15:34:50 +0200 Subject: [PATCH 29/71] Fix mixer limiter to never output min for an input of max + 1 quantum --- src/modules/uavcan/uavcan_main.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 5bc4376709..27e77e9c5a 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -301,9 +301,7 @@ int UavcanNode::run() // iterate actuators for (unsigned i = 0; i < outputs.noutputs; i++) { // last resort: catch NaN, INF and out-of-band errors - if (!isfinite(outputs.output[i]) || - outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { + if (!isfinite(outputs.output[i])) { /* * Value is NaN, INF or out of band - set to the minimum value. * This will be clearly visible on the servo status and will limit the risk of accidentally @@ -311,6 +309,18 @@ int UavcanNode::run() */ outputs.output[i] = -1.0f; } + + // limit outputs to valid range + + // never go below min + if (outputs.output[i] < -1.0f) { + outputs.output[i] = -1.0f; + } + + // never go below max + if (outputs.output[i] > 1.0f) { + outputs.output[i] = 1.0f; + } } // Output to the bus From 324322cb29720dd78b6eb534bb679532d5ed83f2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 7 Jul 2014 02:10:09 +0400 Subject: [PATCH 30/71] UAVCAN ESC perf counters --- src/modules/uavcan/esc_controller.cpp | 28 +++++++++++++++++++++------ src/modules/uavcan/esc_controller.hpp | 8 ++++++++ 2 files changed, 30 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp index f603d94482..406eba88c4 100644 --- a/src/modules/uavcan/esc_controller.cpp +++ b/src/modules/uavcan/esc_controller.cpp @@ -48,6 +48,12 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : { } +UavcanEscController::~UavcanEscController() +{ + perf_free(_perfcnt_invalid_input); + perf_free(_perfcnt_scaling_error); +} + int UavcanEscController::init() { // ESC status subscription @@ -67,8 +73,10 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - assert(outputs != nullptr); - assert(num_outputs <= MAX_ESCS); + if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) { + perf_count(_perfcnt_invalid_input); + return; + } /* * Rate limiting - we don't want to congest the bus @@ -89,13 +97,21 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) for (unsigned i = 0; i < num_outputs; i++) { float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; - if (scaled < 1.0F) + if (scaled < 1.0F) { scaled = 1.0F; // Since we're armed, we don't want to stop it completely + } - assert(scaled >= uavcan::equipment::esc::RawCommand::CMD_MIN); - assert(scaled <= uavcan::equipment::esc::RawCommand::CMD_MAX); + if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) { + scaled = uavcan::equipment::esc::RawCommand::CMD_MIN; + perf_count(_perfcnt_scaling_error); + } else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) { + scaled = uavcan::equipment::esc::RawCommand::CMD_MAX; + perf_count(_perfcnt_scaling_error); + } else { + ; // Correct value + } - msg.cmd.push_back(scaled); + msg.cmd.push_back(static_cast(scaled)); } } diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/esc_controller.hpp index 0ed0c59b56..559ede561e 100644 --- a/src/modules/uavcan/esc_controller.hpp +++ b/src/modules/uavcan/esc_controller.hpp @@ -47,11 +47,13 @@ #include #include #include +#include class UavcanEscController { public: UavcanEscController(uavcan::INode& node); + ~UavcanEscController(); int init(); @@ -96,4 +98,10 @@ private: */ bool _armed = false; uavcan::equipment::esc::Status _states[MAX_ESCS]; + + /* + * Perf counters + */ + perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); + perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error"); }; From dae9b48462bc851ee61d6d18ff2b5697dddf620b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 7 Jul 2014 02:31:05 +0400 Subject: [PATCH 31/71] Renamed OUTPUT_MODE: can --> uavcan_esc --- ROMFS/px4fmu_common/init.d/4012_quad_x_can | 2 +- ROMFS/px4fmu_common/init.d/rc.interface | 2 +- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can index 471ac54b4d..8c5a4fbf2d 100644 --- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -24,4 +24,4 @@ then param set MC_YAWRATE_D 0.0 fi -set OUTPUT_MODE can +set OUTPUT_MODE uavcan_esc diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index da4c734704..1de0abb58d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -25,7 +25,7 @@ then set OUTPUT_DEV /dev/pwm_output fi - if [ $OUTPUT_MODE == can ] + if [ $OUTPUT_MODE == uavcan_esc ] then set OUTPUT_DEV /dev/uavcan/esc fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c95016acee..975cb6d1d4 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -292,7 +292,7 @@ then # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output if [ $OUTPUT_MODE != none ] then - if [ $OUTPUT_MODE == can ] + if [ $OUTPUT_MODE == uavcan_esc ] then if uavcan start 1 then @@ -302,7 +302,7 @@ then fi fi - if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == can ] + if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ] then echo "[init] Use PX4IO PWM as primary output" if px4io start From 72a531b018f0089c89ed40261969555fd282e459 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 7 Jul 2014 15:07:37 +0400 Subject: [PATCH 32/71] Fixed UAVCAN GNSS bridge - EPV computation, catching up with the new GPS ORB topic --- src/modules/uavcan/gnss_receiver.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index 3e98bdf144..debba9fee3 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -85,13 +85,18 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) ? sqrtf(horizontal_pos_variance) : -1.0F; + + // Vertical position uncertainty + _report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; } else { - _report.p_variance_m = -1.0; - _report.eph_m = -1.0; + _report.eph = -1.0F; + _report.epv = -1.0F; } if (valid_velocity_covariance) { @@ -118,12 +123,9 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); From 664795c9db9a0d938cbe7221aed87755ca8de7bf Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 7 Jul 2014 15:47:40 +0400 Subject: [PATCH 33/71] UAVCAN GNSS - using GNSS time to initialize the field time_gps_usec --- src/modules/uavcan/gnss_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index debba9fee3..ba1fe5e499 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -139,7 +139,7 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure Date: Tue, 8 Jul 2014 20:19:17 +0400 Subject: [PATCH 34/71] UAVCAN as a submodule --- .gitignore | 1 - .gitmodules | 3 +++ Makefile | 3 +++ Tools/check_submodules.sh | 16 ++++++++++++++++ uavcan | 1 + 5 files changed, 23 insertions(+), 1 deletion(-) create mode 160000 uavcan diff --git a/.gitignore b/.gitignore index d0c624543f..8b09e4783f 100644 --- a/.gitignore +++ b/.gitignore @@ -36,6 +36,5 @@ mavlink/include/mavlink/v0.9/ tags .tags_sorted_by_file .pydevproject -/uavcan .ropeproject *.orig diff --git a/.gitmodules b/.gitmodules index 8436b398e0..4b84afac2a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "NuttX"] path = NuttX url = git://github.com/PX4/NuttX.git +[submodule "uavcan"] + path = uavcan + url = git://github.com/pavel-kirienko/uavcan.git diff --git a/Makefile b/Makefile index 8bf96ca230..e2d50e8fc8 100644 --- a/Makefile +++ b/Makefile @@ -212,6 +212,9 @@ endif $(NUTTX_SRC): $(Q) (./Tools/check_submodules.sh) +$(UAVCAN_DIR): + $(Q) (./Tools/check_submodules.sh) + .PHONY: checksubmodules checksubmodules: $(Q) (./Tools/check_submodules.sh) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index fb180ef47e..a178b4a387 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -31,4 +31,20 @@ else git submodule update; fi + +if [ -d uavcan/libuavcan_drivers ]; +then + STATUSRETVAL=$(git status --porcelain | grep -i uavcan) + if [ "$STATUSRETVAL" == "" ]; then + echo "Checked uavcan submodule, correct version found" + else + echo "uavcan sub repo not at correct version. Try 'make updatesubmodules'" + echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + exit 1 + fi +else + git submodule init + git submodule update +fi + exit 0 diff --git a/uavcan b/uavcan new file mode 160000 index 0000000000..f66c1a7de3 --- /dev/null +++ b/uavcan @@ -0,0 +1 @@ +Subproject commit f66c1a7de3076ff956bdf159dc3a166cbffe6089 From e64a28e736224da5d1db8e3477eeeffc0b3b1f6c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 12 Jul 2014 17:34:36 +0400 Subject: [PATCH 35/71] Building UAVCAN without run-time checks. This saves 9.5KB of flash and reduces CPU usage. --- src/modules/uavcan/module.mk | 10 +++++----- uavcan | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 2c75944d4e..1ef6f0cfa8 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,9 +40,9 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -Os -SRCS += uavcan_main.cpp \ - uavcan_clock.cpp \ - esc_controller.cpp \ +SRCS += uavcan_main.cpp \ + uavcan_clock.cpp \ + esc_controller.cpp \ gnss_receiver.cpp # @@ -53,7 +53,7 @@ SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. -override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 +override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS # # libuavcan drivers for STM32 @@ -68,7 +68,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM # TODO: Add make target for this, or invoke dsdlc manually. # The second option assumes that the generated headers shall be saved # under the version control, which may be undesirable. -# The first option requires python3 and python3-mako for the sources to be built. +# The first option requires any Python and the Python Mako library for the sources to be built. # $(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR))) INCLUDE_DIRS += dsdlc_generated diff --git a/uavcan b/uavcan index f66c1a7de3..af065e9ca9 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit f66c1a7de3076ff956bdf159dc3a166cbffe6089 +Subproject commit af065e9ca9e3ba9100c125d3ab739313d0500ca8 From 024c8213a10d83743caea21206d21f3de497b18a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 12 Jul 2014 17:45:05 +0400 Subject: [PATCH 36/71] Fixed check_submodules.sh for UAVCAN --- Tools/check_submodules.sh | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index abe3088c7f..8fd9a8f003 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -46,14 +46,22 @@ else fi -if [ -d uavcan/libuavcan_drivers ]; +if [ -d uavcan ] then - STATUSRETVAL=$(git status --porcelain | grep -i uavcan) - if [ "$STATUSRETVAL" == "" ]; then + STATUSRETVAL=$(git submodule summary | grep -A20 -i uavcan | grep "<") + if [ -z "$STATUSRETVAL" ] + then echo "Checked uavcan submodule, correct version found" else - echo "uavcan sub repo not at correct version. Try 'make updatesubmodules'" + echo "" + echo "" + echo "uavcan sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" exit 1 fi else From 528253e33a49702a9c394a5b25d823e4db0a4e7d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 18 Jul 2014 23:24:07 +0400 Subject: [PATCH 37/71] UAVCAN update --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index af065e9ca9..767462c992 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit af065e9ca9e3ba9100c125d3ab739313d0500ca8 +Subproject commit 767462c9924d7afbb789dc3e4c76d31261c557d3 From 3a0fc36c67ef89faa0b20e847eb16de6362ef3e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 14:12:33 +0200 Subject: [PATCH 38/71] Consider the throttle load for battery voltage calculation --- src/modules/commander/commander.cpp | 11 ++++++++++- src/modules/commander/commander_helper.cpp | 10 +++++++--- src/modules/commander/commander_helper.h | 3 ++- src/modules/commander/commander_params.c | 4 ++-- 4 files changed, 21 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 953feec2a7..7332c6eaa7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -918,6 +918,11 @@ int commander_thread_main(int argc, char *argv[]) struct system_power_s system_power; memset(&system_power, 0, sizeof(system_power)); + /* Subscribe to actuator controls (outputs) */ + int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + struct actuator_controls_s actuator_controls; + memset(&actuator_controls, 0, sizeof(actuator_controls)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -1199,13 +1204,17 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); /* only consider battery voltage if system has been running 2s and battery voltage is valid */ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; status.battery_current = battery.current_a; status.condition_battery_voltage_valid = true; - status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah); + + /* get throttle (if armed), as we only care about energy negative throttle also counts */ + float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f; + status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle); } } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index d5fe122cb6..a4aafa1f63 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -281,15 +281,17 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern) } } -float battery_remaining_estimate_voltage(float voltage, float discharged) +float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized) { float ret = 0; static param_t bat_v_empty_h; static param_t bat_v_full_h; static param_t bat_n_cells_h; static param_t bat_capacity_h; + static param_t bat_v_load_drop_h; static float bat_v_empty = 3.2f; static float bat_v_full = 4.0f; + static float bat_v_load_drop = 0.1f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; static bool initialized = false; @@ -300,20 +302,22 @@ float battery_remaining_estimate_voltage(float voltage, float discharged) bat_v_full_h = param_find("BAT_V_FULL"); bat_n_cells_h = param_find("BAT_N_CELLS"); bat_capacity_h = param_find("BAT_CAPACITY"); + bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP"); initialized = true; } if (counter % 100 == 0) { param_get(bat_v_empty_h, &bat_v_empty); param_get(bat_v_full_h, &bat_v_full); + param_get(bat_v_load_drop_h, &bat_v_load_drop); param_get(bat_n_cells_h, &bat_n_cells); param_get(bat_capacity_h, &bat_capacity); } counter++; - /* remaining charge estimate based on voltage */ - float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty)); + /* remaining charge estimate based on voltage and internal resistance (drop under load) */ + float remaining_voltage = (voltage - bat_n_cells * (bat_v_empty - (bat_v_load_drop * throttle_normalized)) / (bat_n_cells * (bat_v_full - bat_v_empty)); if (bat_capacity > 0.0f) { /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index a49c9e263d..4a77fe4871 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -80,8 +80,9 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern); * * @param voltage the current battery voltage * @param discharged the discharged capacity + * @param throttle_normalized the normalized throttle magnitude from 0 to 1. Negative throttle should be converted to this range as well, as it consumes energy. * @return the estimated remaining capacity in 0..1 */ -float battery_remaining_estimate_voltage(float voltage, float discharged); +float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized); #endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 4750f9d5cb..448bc53cd4 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.3f); /** * Full cell voltage. @@ -65,7 +65,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.15f); /** * Number of cells. From 3db78a4ab31b79f092940f4186549132a40ed855 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 14:34:19 +0200 Subject: [PATCH 39/71] commander: Warn at 18 and 9% battery remaining instead of 25 and 10% to not trigger the warning too early (at 50%) as it was before --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7332c6eaa7..cb447f8ce0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1276,13 +1276,13 @@ int commander_thread_main(int argc, char *argv[]) } /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { + if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.9f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); From 5d2489880c2a42bc67554efa7715ba3a761c5d17 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 21:14:14 +0200 Subject: [PATCH 40/71] Fix low voltage warning threshold to 9%, not 90% --- src/modules/commander/commander.cpp | 2 +- src/modules/commander/commander_params.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cb447f8ce0..562e9a1b6d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1282,7 +1282,7 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.9f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 448bc53cd4..a09e2d6bf4 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.3f); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); /** * Full cell voltage. From e7d8d9c91fe72169d292a5e0ac44133a218f27a0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 19 Jul 2014 23:19:49 +0200 Subject: [PATCH 41/71] position_estimator_inav: parameters descriptions added --- .../position_estimator_inav_params.c | 174 ++++++++++++++++++ 1 file changed, 174 insertions(+) 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 0581f8236b..98b31d17b3 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,22 +40,196 @@ #include "position_estimator_inav_params.h" +/** + * Z axis weight for barometer + * + * Weight (cutoff frequency) for barometer altitude measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); + +/** + * Z axis weight for GPS + * + * Weight (cutoff frequency) for GPS altitude measurements. GPS altitude data is very noisy and should be used only as slow correction for baro offset. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); + +/** + * Z axis weight for sonar + * + * Weight (cutoff frequency) for sonar measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); + +/** + * XY axis weight for GPS position + * + * Weight (cutoff frequency) for GPS position measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); + +/** + * XY axis weight for GPS velocity + * + * Weight (cutoff frequency) for GPS velocity measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); + +/** + * XY axis weight for optical flow + * + * Weight (cutoff frequency) for optical flow (velocity) measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); + +/** + * XY axis weight for resetting velocity + * + * When velocity sources lost slowly decrease estimated horizontal velocity with this weight. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); + +/** + * XY axis weight factor for GPS when optical flow available + * + * When optical flow data available, multiply GPS weights (for position and velocity) by this factor. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); + +/** + * Accelerometer bias estimation weight + * + * Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. + * + * @min 0.0 + * @max 0.1 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); + +/** + * Optical flow scale factor + * + * Factor to convert raw optical flow (in pixels) to radians [rad/px]. + * + * @min 0.0 + * @max 1.0 + * @unit rad/px + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); + +/** + * Minimal acceptable optical flow quality + * + * 0 - lowest quality, 1 - best quality. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); + +/** + * Weight for sonar filter + * + * Sonar filter detects spikes on sonar measurements and used to detect new surface level. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); + +/** + * Sonar maximal error for new surface + * + * If sonar measurement error is larger than this value it skiped (spike) or accepted as new surface level (if offset is stable). + * + * @min 0.0 + * @max 1.0 + * @unit m + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); + +/** + * Land detector time + * + * Vehicle assumed landed if no altitude changes happened during this time on low throttle. + * + * @min 0.0 + * @max 10.0 + * @unit s + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); + +/** + * Land detector altitude dispersion threshold + * + * Dispersion threshold for triggering land detector. + * + * @min 0.0 + * @max 10.0 + * @unit m + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); + +/** + * Land detector throttle threshold + * + * Value should be lower than minimal hovering thrust. Half of it is good choice. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); + +/** + * GPS delay + * + * GPS delay compensation + * + * @min 0.0 + * @max 1.0 + * @unit s + * @group Position Estimator INAV + */ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); int parameters_init(struct position_estimator_inav_param_handles *h) From 5fb2a92e7704c2e298128addf890722b6b03289a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 14:28:05 +0200 Subject: [PATCH 42/71] commander: Do not confuse developers with wrong comments, do not confuse users with not at all helpful "general error" messages. --- src/modules/commander/commander.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 04450a44fb..48cbc254fd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1407,8 +1407,12 @@ int commander_thread_main(int argc, char *argv[]) arming_state_changed = true; } else if (arming_ret == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "arming state transition denied"); + /* + * the arming transition can be denied to a number of reasons: + * - pre-flight check failed (sensors not ok or not calibrated) + * - safety not disabled + * - system not in manual mode + */ tune_negative(true); } From b3a80025b3bf514e987d19b7292ba0f9d16d7d1d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 14:28:24 +0200 Subject: [PATCH 43/71] Do not confuse operators / users with technical error messages --- src/modules/commander/state_machine_helper.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 372ba9d7dc..4ca8471fc9 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -216,11 +216,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current } if (ret == TRANSITION_DENIED) { - static const char *errMsg = "INVAL: %s - %s"; - - mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); - - warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); + /* only print to console here as this is too technical to be useful during operation */ + warnx("INVAL: %s - %s", state_names[status->arming_state], state_names[new_arming_state]); } return ret; From f3b8890601e40e5131c55e6f96ad1452a53410eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 14:31:23 +0200 Subject: [PATCH 44/71] commander: Explain sensor arming fail case to users --- src/modules/commander/state_machine_helper.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4ca8471fc9..16ed8dd52b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -200,6 +200,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current /* Sensors need to be initialized for STANDBY state */ if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); valid_transition = false; } From a5f538dc5c9570b59c7135570a662651cb857698 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 16:06:31 +0200 Subject: [PATCH 45/71] Commander: Print technical feedback as last resort if no feedback was provided before --- .../commander/state_machine_helper.cpp | 24 +++++++++++++------ 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 16ed8dd52b..7b26e3e8cb 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -110,8 +110,8 @@ arming_state_transition(struct vehicle_status_s *status, /// current ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); transition_result_t ret = TRANSITION_DENIED; - arming_state_t current_arming_state = status->arming_state; + bool feedback_provided = false; /* only check transition if the new state is actually different from the current one */ if (new_arming_state == current_arming_state) { @@ -156,13 +156,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if pre-arm check fails if (prearm_ret) { + /* the prearm check already prints the reject reason */ + feedback_provided = true; valid_transition = false; // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!"); - + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); + feedback_provided = true; valid_transition = false; } @@ -173,6 +175,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current if (!status->condition_power_input_valid) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); + feedback_provided = true; valid_transition = false; } @@ -182,6 +185,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current (status->avionics_power_rail_voltage < 4.9f))) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); + feedback_provided = true; valid_transition = false; } } @@ -201,6 +205,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current /* Sensors need to be initialized for STANDBY state */ if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); + feedback_provided = true; valid_transition = false; } @@ -217,8 +222,14 @@ arming_state_transition(struct vehicle_status_s *status, /// current } if (ret == TRANSITION_DENIED) { - /* only print to console here as this is too technical to be useful during operation */ - warnx("INVAL: %s - %s", state_names[status->arming_state], state_names[new_arming_state]); + const char * str = "INVAL: %s - %s"; + /* only print to console here by default as this is too technical to be useful during operation */ + warnx(str, state_names[status->arming_state], state_names[new_arming_state]); + + /* print to MAVLink if we didn't provide any feedback yet */ + if (!feedback_provided) { + mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]); + } } return ret; @@ -646,8 +657,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE"); - mavlink_log_critical(mavlink_fd, "hold still while arming"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still"); /* this is frickin' fatal */ failed = true; goto system_eval; From 3935540c7df0f67946b637af6dc2d3145453c326 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 16:23:43 +0200 Subject: [PATCH 46/71] Set full voltage correctly to ensure percentage range fits. Force all users to new value by param renaming. Since this will tend to show batteries as more drained than before, this is a change in a safe direction and will not trigger unnoticed discharges. --- src/modules/commander/commander_helper.cpp | 6 +++--- src/modules/commander/commander_params.c | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index a4aafa1f63..239b77df04 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -289,8 +289,8 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float static param_t bat_n_cells_h; static param_t bat_capacity_h; static param_t bat_v_load_drop_h; - static float bat_v_empty = 3.2f; - static float bat_v_full = 4.0f; + static float bat_v_empty = 3.4f; + static float bat_v_full = 4.2f; static float bat_v_load_drop = 0.1f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; @@ -299,7 +299,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float if (!initialized) { bat_v_empty_h = param_find("BAT_V_EMPTY"); - bat_v_full_h = param_find("BAT_V_FULL"); + bat_v_full_h = param_find("BAT_V_CHARGED"); bat_n_cells_h = param_find("BAT_N_CELLS"); bat_capacity_h = param_find("BAT_CAPACITY"); bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP"); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index a09e2d6bf4..dea12db409 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -65,7 +65,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.15f); +PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); /** * Number of cells. From 264fe884a2825ae430a19ba61e8c89d6a214f5cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 16:28:44 +0200 Subject: [PATCH 47/71] Fixed load voltage calculation --- src/modules/commander/commander_helper.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 239b77df04..d87af2d7d4 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -317,7 +317,8 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float counter++; /* remaining charge estimate based on voltage and internal resistance (drop under load) */ - float remaining_voltage = (voltage - bat_n_cells * (bat_v_empty - (bat_v_load_drop * throttle_normalized)) / (bat_n_cells * (bat_v_full - bat_v_empty)); + float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized); + float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - bat_v_empty)); if (bat_capacity > 0.0f) { /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ From c95de36d3a01bda1d3664fbc46df3a5fc35fe4da Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 19:42:31 +0200 Subject: [PATCH 48/71] commander: Add missing parameter definition --- src/modules/commander/commander_helper.cpp | 2 +- src/modules/commander/commander_params.c | 10 ++++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index d87af2d7d4..2022e99fb3 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -291,7 +291,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float static param_t bat_v_load_drop_h; static float bat_v_empty = 3.4f; static float bat_v_full = 4.2f; - static float bat_v_load_drop = 0.1f; + static float bat_v_load_drop = 0.06f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; static bool initialized = false; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index dea12db409..0e4973b5fa 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -67,6 +67,16 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); */ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); +/** + * Voltage drop per cell on 100% load + * + * This implicitely defines the internal resistance + * to maximum current ratio and assumes linearity. + * + * @group Battery Calibration + */ +PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.06f); + /** * Number of cells. * From 25d1cc3995f3ecafe2408582296d6dedfe49ce53 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 07:35:23 +0200 Subject: [PATCH 49/71] Final value for battery load param default --- src/modules/commander/commander_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 0e4973b5fa..dba68700b4 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -75,7 +75,7 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.06f); +PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); /** * Number of cells. From afff12374240240ec2bbf2ba043b37cca8b3298a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 13:30:56 +0200 Subject: [PATCH 50/71] Add PX4IO voltage dataset and linear fit --- Tools/tests-host/data/fit_linear_voltage.m | 14 +++++ Tools/tests-host/data/px4io_v1.3.csv | 70 ++++++++++++++++++++++ 2 files changed, 84 insertions(+) create mode 100644 Tools/tests-host/data/fit_linear_voltage.m create mode 100644 Tools/tests-host/data/px4io_v1.3.csv diff --git a/Tools/tests-host/data/fit_linear_voltage.m b/Tools/tests-host/data/fit_linear_voltage.m new file mode 100644 index 0000000000..a0384b4865 --- /dev/null +++ b/Tools/tests-host/data/fit_linear_voltage.m @@ -0,0 +1,14 @@ +close all; +clear all; +M = importdata('px4io_v1.3.csv'); +voltage = M.data(:, 1); +counts = M.data(:, 2); +plot(counts, voltage, 'b*-', 'LineWidth', 2, 'MarkerSize', 15); +coeffs = polyfit(counts, voltage, 1); +fittedC = linspace(min(counts), max(counts), 500); +fittedV = polyval(coeffs, fittedC); +hold on +plot(fittedC, fittedV, 'r-', 'LineWidth', 3); + +slope = coeffs(1) +y_intersection = coeffs(2) \ No newline at end of file diff --git a/Tools/tests-host/data/px4io_v1.3.csv b/Tools/tests-host/data/px4io_v1.3.csv new file mode 100644 index 0000000000..2100d840ac --- /dev/null +++ b/Tools/tests-host/data/px4io_v1.3.csv @@ -0,0 +1,70 @@ +voltage, counts +4.3, 950 +4.4, 964 +4.5, 986 +4.6, 1009 +4.7, 1032 +4.8, 1055 +4.9, 1078 +5.0, 1101 +5.2, 1124 +5.3, 1148 +5.4, 1171 +5.5, 1195 +6.0, 1304 +6.1, 1329 +6.2, 1352 +7.0, 1517 +7.1, 1540 +7.2, 1564 +7.3, 1585 +7.4, 1610 +7.5, 1636 +8.0, 1728 +8.1, 1752 +8.2, 1755 +8.3, 1798 +8.4, 1821 +9.0, 1963 +9.1, 1987 +9.3, 2010 +9.4, 2033 +10.0, 2174 +10.1, 2198 +10.2, 2221 +10.3, 2245 +10.4, 2268 +11.0, 2385 +11.1, 2409 +11.2, 2432 +11.3, 2456 +11.4, 2480 +11.5, 2502 +11.6, 2526 +11.7, 2550 +11.8, 2573 +11.9, 2597 +12.0, 2621 +12.1, 2644 +12.3, 2668 +12.4, 2692 +12.5, 2716 +12.6, 2737 +12.7, 2761 +13.0, 2832 +13.5, 2950 +13.6, 2973 +14.1, 3068 +14.2, 3091 +14.7, 3209 +15.0, 3280 +15.1, 3304 +15.5, 3374 +15.6, 3397 +15.7, 3420 +16.0, 3492 +16.1, 3514 +16.2, 3538 +16.9, 3680 +17.0, 3704 +17.1, 3728 \ No newline at end of file From 956c084f31af9ec2aa80d61eee17e7506b4c7172 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 13:31:35 +0200 Subject: [PATCH 51/71] Fit IO voltage estimation using a new dataset, cross-validated with a second unit. Pending testing --- src/modules/px4iofirmware/registers.c | 23 +++++------------------ 1 file changed, 5 insertions(+), 18 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index fcd53f1f1b..7fbb0ecf19 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -739,30 +739,17 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val { /* * Coefficients here derived by measurement of the 5-16V - * range on one unit: + * range on one unit, validated on sample points of another unit * - * V counts - * 5 1001 - * 6 1219 - * 7 1436 - * 8 1653 - * 9 1870 - * 10 2086 - * 11 2303 - * 12 2522 - * 13 2738 - * 14 2956 - * 15 3172 - * 16 3389 + * Data in Tools/tests-host/data folder. * - * slope = 0.0046067 - * intercept = 0.3863 + * slope = 0.004585267878277 (int: 4585) + * intercept = 0.016646394188076 (int: 16646 * - * Intercept corrected for best results @ 12V. */ unsigned counts = adc_measure(ADC_VBATT); if (counts != 0xffff) { - unsigned mV = (4150 + (counts * 46)) / 10 - 200; + unsigned mV = (16646 + (counts * 4585)) / 1000; unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; r_page_status[PX4IO_P_STATUS_VBATT] = corrected; From 0fc73a1484ac9e9cbc2f52ec7e81b28a109777eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 13:46:38 +0200 Subject: [PATCH 52/71] Fix comment, missing brace in comment --- src/modules/px4iofirmware/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 7fbb0ecf19..59b3043aa3 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -744,7 +744,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val * Data in Tools/tests-host/data folder. * * slope = 0.004585267878277 (int: 4585) - * intercept = 0.016646394188076 (int: 16646 + * intercept = 0.016646394188076 (int: 16646) * */ unsigned counts = adc_measure(ADC_VBATT); From 331623bbd40978adf14c2034e75f31c937c34fba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 13:48:59 +0200 Subject: [PATCH 53/71] Fix missing line endings --- Tools/tests-host/data/fit_linear_voltage.m | 2 +- Tools/tests-host/data/px4io_v1.3.csv | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/tests-host/data/fit_linear_voltage.m b/Tools/tests-host/data/fit_linear_voltage.m index a0384b4865..7d0c2c27fc 100644 --- a/Tools/tests-host/data/fit_linear_voltage.m +++ b/Tools/tests-host/data/fit_linear_voltage.m @@ -11,4 +11,4 @@ hold on plot(fittedC, fittedV, 'r-', 'LineWidth', 3); slope = coeffs(1) -y_intersection = coeffs(2) \ No newline at end of file +y_intersection = coeffs(2) diff --git a/Tools/tests-host/data/px4io_v1.3.csv b/Tools/tests-host/data/px4io_v1.3.csv index 2100d840ac..b41ee8f1f9 100644 --- a/Tools/tests-host/data/px4io_v1.3.csv +++ b/Tools/tests-host/data/px4io_v1.3.csv @@ -67,4 +67,4 @@ voltage, counts 16.2, 3538 16.9, 3680 17.0, 3704 -17.1, 3728 \ No newline at end of file +17.1, 3728 From 178b0f7399ab881e44f2d2ecff809aea53a4397d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 19:25:23 +0200 Subject: [PATCH 54/71] Cross-check with nominal values from divider --- src/modules/px4iofirmware/registers.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 59b3043aa3..8c15c66c1f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -743,13 +743,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val * * Data in Tools/tests-host/data folder. * - * slope = 0.004585267878277 (int: 4585) + * measured slope = 0.004585267878277 (int: 4585) + * nominal theoretic slope: 0.00459340659 (int: 4593) * intercept = 0.016646394188076 (int: 16646) + * nominal theoretic intercept: 0.00 (int: 0) * */ unsigned counts = adc_measure(ADC_VBATT); if (counts != 0xffff) { - unsigned mV = (16646 + (counts * 4585)) / 1000; + unsigned mV = (0 + (counts * 4593)) / 1000; unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; r_page_status[PX4IO_P_STATUS_VBATT] = corrected; From 07d11583bb0926ac9fe1cf2f1c7e11c683606a4b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jul 2014 20:21:20 +0200 Subject: [PATCH 55/71] Rely on theoretical value, as the closed-loop test with multimeter suggests this is the most accurate measurement --- src/modules/px4iofirmware/registers.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 8c15c66c1f..43161aa70e 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -751,7 +751,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val */ unsigned counts = adc_measure(ADC_VBATT); if (counts != 0xffff) { - unsigned mV = (0 + (counts * 4593)) / 1000; + unsigned mV = (166460 + (counts * 45934)) / 10000; unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; r_page_status[PX4IO_P_STATUS_VBATT] = corrected; From 940becd0c1e39168e4836db5927e989f2d780ad3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 22 Jul 2014 02:55:14 +0400 Subject: [PATCH 56/71] UAVCAN update: Mako dependency removed --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index 767462c992..dca2611c31 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 767462c9924d7afbb789dc3e4c76d31261c557d3 +Subproject commit dca2611c3186eaa1cac42557f07b013e2dc633d3 From 1b5d4e5bd4fbee8f994c95d69be7660fd211d2f4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Jul 2014 17:42:34 +0200 Subject: [PATCH 57/71] Comment out uavcan due to build breakage, will go back in ASAP --- makefiles/config_px4fmu-v2_default.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a498a1b402..fea1bade33 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -74,7 +74,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -MODULES += modules/uavcan +#MODULES += modules/uavcan # # Estimation modules (EKF/ SO3 / other filters) From e6594e519295ec081c6f962fca6b4fbdd190846f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 22 Jul 2014 10:20:10 +0900 Subject: [PATCH 58/71] tone_alarm: add EKF_WARNING tune Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/drv_tone_alarm.h | 1 + src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index b7981e9c4b..19f792d19c 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -149,6 +149,7 @@ enum { TONE_GPS_WARNING_TUNE, TONE_ARMING_FAILURE_TUNE, TONE_PARACHUTE_RELEASE_TUNE, + TONE_EKF_WARNING_TUNE, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 810f4aacc4..03c7bd399f 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -336,6 +336,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<< Date: Sat, 19 Jul 2014 14:38:39 +0200 Subject: [PATCH 59/71] code style only: Fix indendation in mavlink.h --- src/modules/mavlink/mavlink_main.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index da989d6c51..adca0e88f2 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -250,13 +250,13 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; - MavlinkMissionManager *_mission_manager; + MavlinkMissionManager *_mission_manager; - orb_advert_t _mission_pub; + orb_advert_t _mission_pub; int _mission_result_sub; - MAVLINK_MODE _mode; + MAVLINK_MODE _mode; - mavlink_channel_t _channel; + mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; unsigned int _total_counter; From 0c0b1a4c666671e964d295fe6aea64d7b6a98fc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 14:59:13 +0200 Subject: [PATCH 60/71] Print mavlink radio module rates --- src/modules/mavlink/mavlink_main.cpp | 51 ++++++++++++++++++++++-- src/modules/mavlink/mavlink_main.h | 10 +++++ src/modules/mavlink/mavlink_receiver.cpp | 3 +- 3 files changed, 59 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 76b5459a3e..e25851c7d5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -247,6 +247,7 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _rstatus{}, _message_buffer{}, _message_buffer_mutex{}, _param_initialized(false), @@ -424,6 +425,29 @@ Mavlink::destroy_all_instances() return OK; } +int +Mavlink::get_status_all_instances() +{ + Mavlink *inst = ::_mavlink_instances; + + unsigned iterations = 0; + + warnx("waiting for instances to stop"); + + while (inst != nullptr) { + + printf("instance #%u:\n", iterations); + inst->display_status(); + + /* move on */ + inst = inst->next; + iterations++; + } + + /* return an error if there are no instances */ + return (iterations == 0); +} + bool Mavlink::instance_exists(const char *device_name, Mavlink *self) { @@ -1395,7 +1419,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); configure_stream("ATTITUDE", 10.0f * rate_mult); - configure_stream("VFR_HUD", 10.0f * rate_mult); + configure_stream("VFR_HUD", 8.0f * rate_mult); configure_stream("GPS_RAW_INT", 1.0f * rate_mult); configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); @@ -1665,6 +1689,27 @@ void Mavlink::display_status() { warnx("running"); + + if (_rstatus.timestamp > 0) { + printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time); + + switch (_rstatus.type) { + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + printf("\t3DR RADIO\n"); + break; + default: + printf("\tUNKNOWN RADIO\n"); + break; + } + + printf("\trssi:\t%d\t\n", _rstatus.rssi); + printf("\tremote rssi:\t%u\tus\n", _rstatus.remote_rssi); + printf("\ttxbuf:\t%u\tus\n", _rstatus.txbuf); + printf("\tnoise:\t%d\tus\n", _rstatus.noise); + printf("\tremote noise:\t%u\tus\n", _rstatus.remote_noise); + printf("\trx errors:\t%u\tus\n", _rstatus.rxerrors); + printf("\tfixed:\t%u\tus\n", _rstatus.fixed); + } } int @@ -1752,8 +1797,8 @@ int mavlink_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop-all")) { return Mavlink::destroy_all_instances(); - // } else if (!strcmp(argv[1], "status")) { - // mavlink::g_mavlink->status(); + } else if (!strcmp(argv[1], "status")) { + return Mavlink::get_status_all_instances(); } else if (!strcmp(argv[1], "stream")) { return Mavlink::stream_command(argc, argv); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index adca0e88f2..d51120462a 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -51,6 +51,7 @@ #include #include #include +#include #include "mavlink_bridge_header.h" #include "mavlink_orb_subscription.h" @@ -97,6 +98,8 @@ public: static int destroy_all_instances(); + static int get_status_all_instances(); + static bool instance_exists(const char *device_name, Mavlink *self); static void forward_message(const mavlink_message_t *msg, Mavlink *self); @@ -229,6 +232,11 @@ public: */ void count_txerr(); + /** + * Get the receive status of this MAVLink link + */ + struct telemetry_status_s& get_rx_status() { return _rstatus; } + protected: Mavlink *next; @@ -285,6 +293,8 @@ private: bool _flow_control_enabled; + struct telemetry_status_s _rstatus; ///< receive status + struct mavlink_message_buffer { int write_ptr; int read_ptr; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3ee7ec34da..528c8b72a3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -400,8 +400,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) mavlink_radio_status_t rstatus; mavlink_msg_radio_status_decode(msg, &rstatus); - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); + struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); tstatus.timestamp = hrt_absolute_time(); tstatus.heartbeat_time = _telemetry_heartbeat_time; From 91becef344833dbb19230ec3f3c69750bce216e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:56:37 +0200 Subject: [PATCH 61/71] Cleanup of heartbeat handling and status printing. Ready to go mainline --- src/modules/mavlink/mavlink_main.cpp | 32 ++++++++++++---------- src/modules/mavlink/mavlink_receiver.cpp | 20 ++++++++------ src/modules/mavlink/mavlink_receiver.h | 1 - src/modules/uORB/topics/telemetry_status.h | 3 +- 4 files changed, 32 insertions(+), 24 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e25851c7d5..0e8f17c77e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -432,11 +432,9 @@ Mavlink::get_status_all_instances() unsigned iterations = 0; - warnx("waiting for instances to stop"); - while (inst != nullptr) { - printf("instance #%u:\n", iterations); + printf("\ninstance #%u:\n", iterations); inst->display_status(); /* move on */ @@ -1688,27 +1686,33 @@ Mavlink::start(int argc, char *argv[]) void Mavlink::display_status() { - warnx("running"); + + if (_rstatus.heartbeat_time > 0) { + printf("\theartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + } if (_rstatus.timestamp > 0) { - printf("\ttime:\t%llu\tus\n", _rstatus.heartbeat_time); + + printf("\ttype:\t\t"); switch (_rstatus.type) { case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: - printf("\t3DR RADIO\n"); + printf("3DR RADIO\n"); break; default: - printf("\tUNKNOWN RADIO\n"); + printf("UNKNOWN RADIO\n"); break; } - printf("\trssi:\t%d\t\n", _rstatus.rssi); - printf("\tremote rssi:\t%u\tus\n", _rstatus.remote_rssi); - printf("\ttxbuf:\t%u\tus\n", _rstatus.txbuf); - printf("\tnoise:\t%d\tus\n", _rstatus.noise); - printf("\tremote noise:\t%u\tus\n", _rstatus.remote_noise); - printf("\trx errors:\t%u\tus\n", _rstatus.rxerrors); - printf("\tfixed:\t%u\tus\n", _rstatus.fixed); + printf("\trssi:\t\t%d\n", _rstatus.rssi); + printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi); + printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf); + printf("\tnoise:\t\t%d\n", _rstatus.noise); + printf("\tremote noise:\t%u\n", _rstatus.remote_noise); + printf("\trx errors:\t%u\n", _rstatus.rxerrors); + printf("\tfixed:\t\t%u\n", _rstatus.fixed); + } else { + printf("\tno telem status.\n"); } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 528c8b72a3..454d730d83 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -111,7 +111,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - _telemetry_heartbeat_time(0), _radio_status_available(false), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), @@ -403,7 +402,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); tstatus.timestamp = hrt_absolute_time(); - tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.telem_time = tstatus.timestamp; + /* tstatus.heartbeat_time is set by system heartbeats */ tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; tstatus.rssi = rstatus.rssi; tstatus.remote_rssi = rstatus.remrssi; @@ -460,16 +460,20 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) /* ignore own heartbeats, accept only heartbeats from GCS */ if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { - _telemetry_heartbeat_time = hrt_absolute_time(); + + struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); + + hrt_abstime tnow = hrt_absolute_time(); + + /* always set heartbeat, publish only if telemetry link not up */ + tstatus.heartbeat_time = tnow; /* if no radio status messages arrive, lets at least publish that heartbeats were received */ if (!_radio_status_available) { - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); - - tstatus.timestamp = _telemetry_heartbeat_time; - tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.timestamp = tnow; + /* telem_time indicates the timestamp of a telemetry status packet and we got none */ + tstatus.telem_time = 0; tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; if (_telemetry_status_pub < 0) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index b64a060e3a..f4d0c52d88 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -148,7 +148,6 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - hrt_abstime _telemetry_heartbeat_time; bool _radio_status_available; int _control_mode_sub; int _hil_frames; diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 3347724a53..1297c1a9d3 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -57,7 +57,8 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; - uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ + uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ + uint64_t telem_time; /**< Time of last received telemetry status packet, 0 for none */ enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ uint8_t rssi; /**< local signal strength */ uint8_t remote_rssi; /**< remote signal strength */ From 63f1ee184a63ecabd93ce0cfa91afe97fe7e65d8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 15:57:42 +0200 Subject: [PATCH 62/71] Add hint that heartbeats are only considered if coming from a GCS --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0e8f17c77e..7bd2b9754b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1688,7 +1688,7 @@ Mavlink::display_status() { if (_rstatus.heartbeat_time > 0) { - printf("\theartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); + printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); } if (_rstatus.timestamp > 0) { From b344b94d3039273a1659a4b5640e7dc197922767 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jul 2014 16:00:37 +0200 Subject: [PATCH 63/71] Fix code style, no actual code edits --- src/modules/mavlink/mavlink_main.cpp | 166 +++++++++++++++------------ 1 file changed, 92 insertions(+), 74 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7bd2b9754b..cb8c298c04 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -81,7 +81,7 @@ #include "mavlink_commands.h" #ifndef MAVLINK_CRC_EXTRA - #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems +#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems #endif /* oddly, ERROR is not defined for c++ */ @@ -154,7 +154,8 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length instance = Mavlink::get_instance(6); break; #endif - default: + + default: return; } @@ -169,17 +170,16 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length int buf_free = 0; if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { /* 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]) - { + 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); } @@ -202,8 +202,10 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } ssize_t ret = write(uart, ch, desired); + if (ret != desired) { instance->count_txerr(); + } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } @@ -232,33 +234,33 @@ Mavlink::Mavlink() : _mission_result_sub(-1), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), - _logbuffer{}, - _total_counter(0), - _receive_thread{}, - _verbose(false), - _forwarding_on(false), - _passing_on(false), - _ftp_on(false), - _uart_fd(-1), - _baudrate(57600), - _datarate(10000), - _mavlink_param_queue_index(0), - mavlink_link_termination_allowed(false), - _subscribe_to_stream(nullptr), - _subscribe_to_stream_rate(0.0f), - _flow_control_enabled(true), - _rstatus{}, - _message_buffer{}, - _message_buffer_mutex{}, - _param_initialized(false), - _param_system_id(0), - _param_component_id(0), - _param_system_type(0), - _param_use_hil_gps(0), + _logbuffer {}, + _total_counter(0), + _receive_thread {}, + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _ftp_on(false), + _uart_fd(-1), + _baudrate(57600), + _datarate(10000), + _mavlink_param_queue_index(0), + mavlink_link_termination_allowed(false), + _subscribe_to_stream(nullptr), + _subscribe_to_stream_rate(0.0f), + _flow_control_enabled(true), + _rstatus {}, + _message_buffer {}, + _message_buffer_mutex {}, + _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_el")), - _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; @@ -637,7 +639,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * case 921600: speed = B921600; break; default: - warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud); + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", + baud); return -EINVAL; } @@ -867,8 +870,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav case MAVLINK_MSG_ID_PARAM_REQUEST_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)) { + (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { /* Start sending parameters */ mavlink_pm_start_queued_send(); send_statustext_info("[pm] sending list"); @@ -883,7 +887,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav mavlink_param_set_t mavlink_param_set; mavlink_msg_param_set_decode(msg, &mavlink_param_set); - if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { + if (mavlink_param_set.target_system == mavlink_system.sysid + && ((mavlink_param_set.target_component == mavlink_system.compid) + || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { /* local name buffer to enforce null-terminated string */ char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); @@ -910,7 +916,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav mavlink_param_request_read_t mavlink_param_request_read; mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); - if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { + if (mavlink_param_request_read.target_system == mavlink_system.sysid + && ((mavlink_param_request_read.target_component == mavlink_system.compid) + || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { /* when no index is given, loop through string ids and compare them */ if (mavlink_param_request_read.param_index == -1) { /* local name buffer to enforce null-terminated string */ @@ -973,15 +981,17 @@ Mavlink::send_statustext(unsigned severity, const char *string) /* Map severity */ switch (severity) { - case MAVLINK_IOC_SEND_TEXT_INFO: - statustext.severity = MAV_SEVERITY_INFO; - break; - case MAVLINK_IOC_SEND_TEXT_CRITICAL: - statustext.severity = MAV_SEVERITY_CRITICAL; - break; - case MAVLINK_IOC_SEND_TEXT_EMERGENCY: - statustext.severity = MAV_SEVERITY_EMERGENCY; - break; + case MAVLINK_IOC_SEND_TEXT_INFO: + statustext.severity = MAV_SEVERITY_INFO; + break; + + case MAVLINK_IOC_SEND_TEXT_CRITICAL: + statustext.severity = MAV_SEVERITY_CRITICAL; + break; + + case MAVLINK_IOC_SEND_TEXT_EMERGENCY: + statustext.severity = MAV_SEVERITY_EMERGENCY; + break; } mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); @@ -1098,12 +1108,14 @@ Mavlink::message_buffer_init(int size) _message_buffer.size = size; _message_buffer.write_ptr = 0; _message_buffer.read_ptr = 0; - _message_buffer.data = (char*)malloc(_message_buffer.size); + _message_buffer.data = (char *)malloc(_message_buffer.size); int ret; + if (_message_buffer.data == 0) { ret = ERROR; _message_buffer.size = 0; + } else { ret = OK; } @@ -1475,7 +1487,8 @@ Mavlink::task_main(int argc, char *argv[]) if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { if (_subscribe_to_stream_rate > 0.0f) { - warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate); + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, + (double)_subscribe_to_stream_rate); } else { warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); @@ -1514,45 +1527,46 @@ Mavlink::task_main(int argc, char *argv[]) bool is_part; uint8_t *read_ptr; - uint8_t *write_ptr; + uint8_t *write_ptr; pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr((void**)&read_ptr, &is_part); + int available = message_buffer_get_ptr((void **)&read_ptr, &is_part); pthread_mutex_unlock(&_message_buffer_mutex); if (available > 0) { - // Reconstruct message from buffer + // Reconstruct message from buffer mavlink_message_t msg; - write_ptr = (uint8_t*)&msg; + write_ptr = (uint8_t *)&msg; + + // Pull a single message from the buffer + size_t read_count = available; + + if (read_count > sizeof(mavlink_message_t)) { + read_count = sizeof(mavlink_message_t); + } + + memcpy(write_ptr, read_ptr, read_count); + + // We hold the mutex until after we complete the second part of the buffer. If we don't + // we may end up breaking the empty slot overflow detection semantics when we mark the + // possibly partial read below. + pthread_mutex_lock(&_message_buffer_mutex); - // Pull a single message from the buffer - size_t read_count = available; - if (read_count > sizeof(mavlink_message_t)) { - read_count = sizeof(mavlink_message_t); - } - - memcpy(write_ptr, read_ptr, read_count); - - // We hold the mutex until after we complete the second part of the buffer. If we don't - // we may end up breaking the empty slot overflow detection semantics when we mark the - // possibly partial read below. - pthread_mutex_lock(&_message_buffer_mutex); - message_buffer_mark_read(read_count); /* write second part of buffer if there is some */ if (is_part && read_count < sizeof(mavlink_message_t)) { - write_ptr += read_count; - available = message_buffer_get_ptr((void**)&read_ptr, &is_part); - read_count = sizeof(mavlink_message_t) - read_count; - memcpy(write_ptr, read_ptr, read_count); + write_ptr += read_count; + available = message_buffer_get_ptr((void **)&read_ptr, &is_part); + read_count = sizeof(mavlink_message_t) - read_count; + memcpy(write_ptr, read_ptr, read_count); message_buffer_mark_read(available); } - - pthread_mutex_unlock(&_message_buffer_mutex); - _mavlink_resend_uart(_channel, &msg); + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, &msg); } } @@ -1606,6 +1620,7 @@ Mavlink::task_main(int argc, char *argv[]) message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } + /* destroy log buffer */ mavlink_logbuffer_destroy(&_logbuffer); @@ -1627,6 +1642,7 @@ int Mavlink::start_helper(int argc, char *argv[]) /* out of memory */ res = -ENOMEM; warnx("OUT OF MEM"); + } else { /* this will actually only return once MAVLink exits */ res = instance->task_main(argc, argv); @@ -1696,10 +1712,11 @@ Mavlink::display_status() printf("\ttype:\t\t"); switch (_rstatus.type) { - case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: + case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: printf("3DR RADIO\n"); break; - default: + + default: printf("UNKNOWN RADIO\n"); break; } @@ -1711,6 +1728,7 @@ Mavlink::display_status() printf("\tremote noise:\t%u\n", _rstatus.remote_noise); printf("\trx errors:\t%u\n", _rstatus.rxerrors); printf("\tfixed:\t\t%u\n", _rstatus.fixed); + } else { printf("\tno telem status.\n"); } From ef363edfcd810e3b153609f0a723f53db0cd3885 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 19 Jul 2014 17:37:13 +0200 Subject: [PATCH 64/71] mavlink: TX/RX rate counters added --- src/modules/mavlink/mavlink_main.cpp | 28 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_main.h | 23 +++++++++++++++++++ src/modules/mavlink/mavlink_receiver.cpp | 3 +++ 3 files changed, 54 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cb8c298c04..0c6f8c42f8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -197,6 +197,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (buf_free < desired) { /* we don't want to send anything just in half, so return */ instance->count_txerr(); + instance->count_txerrbytes(desired); return; } } @@ -205,9 +206,11 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (ret != desired) { instance->count_txerr(); + instance->count_txerrbytes(desired); } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; + instance->count_txbytes(desired); } } } @@ -249,6 +252,13 @@ Mavlink::Mavlink() : _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _bytes_tx(0), + _bytes_txerr(0), + _bytes_rx(0), + _bytes_timestamp(0), + _rate_tx(0.0f), + _rate_txerr(0.0f), + _rate_rx(0.0f), _rstatus {}, _message_buffer {}, _message_buffer_mutex {}, @@ -1570,6 +1580,20 @@ Mavlink::task_main(int argc, char *argv[]) } } + /* update TX/RX rates*/ + if (t > _bytes_timestamp + 1000000) { + if (_bytes_timestamp != 0) { + float dt = (t - _bytes_timestamp) / 1000.0f; + _rate_tx = _bytes_tx / dt; + _rate_txerr = _bytes_txerr / dt; + _rate_rx = _bytes_rx / dt; + _bytes_tx = 0; + _bytes_txerr = 0; + _bytes_rx = 0; + } + _bytes_timestamp = t; + } + perf_end(_loop_perf); } @@ -1732,6 +1756,10 @@ Mavlink::display_status() } else { printf("\tno telem status.\n"); } + printf("\trates:\n"); + printf("\ttx: %.3f kB/s\n", (double)_rate_tx); + printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr); + printf("\trx: %.3f kB/s\n", (double)_rate_rx); } int diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index d51120462a..70d13acb09 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -232,6 +232,21 @@ public: */ void count_txerr(); + /** + * Count transmitted bytes + */ + void count_txbytes(unsigned n) { _bytes_tx += n; }; + + /** + * Count bytes not transmitted because of errors + */ + void count_txerrbytes(unsigned n) { _bytes_txerr += n; }; + + /** + * Count received bytes + */ + void count_rxbytes(unsigned n) { _bytes_rx += n; }; + /** * Get the receive status of this MAVLink link */ @@ -293,6 +308,14 @@ private: bool _flow_control_enabled; + unsigned _bytes_tx; + unsigned _bytes_txerr; + unsigned _bytes_rx; + uint64_t _bytes_timestamp; + float _rate_tx; + float _rate_txerr; + float _rate_rx; + struct telemetry_status_s _rstatus; ///< receive status struct mavlink_message_buffer { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 454d730d83..54c412ce7c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -958,6 +958,9 @@ MavlinkReceiver::receive_thread(void *arg) _mavlink->handle_message(&msg); } } + + /* count received bytes */ + _mavlink->count_rxbytes(nread); } } From 7f0e67522878d1e850c22553fc53e93283ed92f8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Jul 2014 08:24:56 +0200 Subject: [PATCH 65/71] Pass over drivers to reduce build size --- src/drivers/airspeed/module.mk | 2 ++ src/drivers/bma180/module.mk | 2 ++ src/drivers/ets_airspeed/module.mk | 2 ++ src/drivers/hil/module.mk | 2 ++ src/drivers/led/module.mk | 2 ++ src/drivers/md25/module.mk | 2 ++ src/drivers/meas_airspeed/module.mk | 2 ++ src/drivers/ms5611/module.mk | 2 ++ src/drivers/px4flow/module.mk | 2 ++ src/drivers/roboclaw/module.mk | 2 ++ 10 files changed, 20 insertions(+) diff --git a/src/drivers/airspeed/module.mk b/src/drivers/airspeed/module.mk index 4eef061610..5fbc75309c 100644 --- a/src/drivers/airspeed/module.mk +++ b/src/drivers/airspeed/module.mk @@ -36,3 +36,5 @@ # SRCS = airspeed.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/bma180/module.mk b/src/drivers/bma180/module.mk index 4c60ee082c..33433307a6 100644 --- a/src/drivers/bma180/module.mk +++ b/src/drivers/bma180/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = bma180 SRCS = bma180.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/ets_airspeed/module.mk b/src/drivers/ets_airspeed/module.mk index 966a5b8190..8aaaf0ebb0 100644 --- a/src/drivers/ets_airspeed/module.mk +++ b/src/drivers/ets_airspeed/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = ets_airspeed SRCS = ets_airspeed.cpp MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/hil/module.mk b/src/drivers/hil/module.mk index f8895f5d5a..f1fc49fb3d 100644 --- a/src/drivers/hil/module.mk +++ b/src/drivers/hil/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = hil SRCS = hil.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/led/module.mk b/src/drivers/led/module.mk index 777f3e4425..5b7b4491b7 100644 --- a/src/drivers/led/module.mk +++ b/src/drivers/led/module.mk @@ -36,3 +36,5 @@ # SRCS = led.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/md25/module.mk b/src/drivers/md25/module.mk index 13821a6b58..3f9cf2d891 100644 --- a/src/drivers/md25/module.mk +++ b/src/drivers/md25/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = md25 SRCS = md25.cpp \ md25_main.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/meas_airspeed/module.mk b/src/drivers/meas_airspeed/module.mk index 2a15b669f4..6f5909978b 100644 --- a/src/drivers/meas_airspeed/module.mk +++ b/src/drivers/meas_airspeed/module.mk @@ -42,3 +42,5 @@ SRCS = meas_airspeed.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk index 20f8aa1737..ee74058fc1 100644 --- a/src/drivers/ms5611/module.mk +++ b/src/drivers/ms5611/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = ms5611 SRCS = ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/px4flow/module.mk b/src/drivers/px4flow/module.mk index d3062e4579..460bec7b96 100644 --- a/src/drivers/px4flow/module.mk +++ b/src/drivers/px4flow/module.mk @@ -38,3 +38,5 @@ MODULE_COMMAND = px4flow SRCS = px4flow.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/roboclaw/module.mk b/src/drivers/roboclaw/module.mk index 1abecf198b..c5e55bdc30 100644 --- a/src/drivers/roboclaw/module.mk +++ b/src/drivers/roboclaw/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = roboclaw SRCS = roboclaw_main.cpp \ RoboClaw.cpp + +MAXOPTIMIZATION = -Os From a7d2963e2bca060493f787cf637b0e1b0d9d829e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Jul 2014 08:27:45 +0200 Subject: [PATCH 66/71] Enable UAVCAN --- makefiles/config_px4fmu-v2_default.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index fea1bade33..a498a1b402 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -74,7 +74,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -#MODULES += modules/uavcan +MODULES += modules/uavcan # # Estimation modules (EKF/ SO3 / other filters) From 1fdc666bb0be393f048c85b1827494beedff0426 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Jul 2014 16:53:19 +0200 Subject: [PATCH 67/71] Update NuttX to deal with parity --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 7e1b97bcf1..088146b90e 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 7e1b97bcf10d8495169eec355988ca5890bfd5df +Subproject commit 088146b90eee5b614ea6386a64dae343a49a5172 From 53f1b3190215f373aa5d8d38264b4ba5771b1676 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Jul 2014 21:41:09 +0200 Subject: [PATCH 68/71] Do not abort on submodule init feedback --- Tools/check_submodules.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 0c03396e7b..3904a27753 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -73,8 +73,8 @@ then exit 1 fi else - git submodule init - git submodule update + git submodule init; + git submodule update; fi exit 0 From b72e89697c564c878c4e17c46b435e8fe94736c5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 25 Jul 2014 00:52:42 +0400 Subject: [PATCH 69/71] UAVCAN update for #1229 --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index dca2611c31..f51ee9cdec 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit dca2611c3186eaa1cac42557f07b013e2dc633d3 +Subproject commit f51ee9cdecd7841f6208bfb0b16228888daad663 From fd52e5561eaf14f9c6ca1abc00cb82b0e3d38ec8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 23:46:39 +0200 Subject: [PATCH 70/71] update comment about condition_global_position_valid --- src/modules/uORB/topics/vehicle_status.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b46c00b758..cbb148edaf 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -186,7 +186,7 @@ struct vehicle_status_s { bool condition_system_sensors_initialized; bool condition_system_returned_to_home; bool condition_auto_mission_available; - bool condition_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool condition_global_position_valid; /**< set to true by the commander app if the quality of the position estimate is good enough to use it for navigation */ bool condition_launch_position_valid; /**< indicates a valid launch position */ bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_local_position_valid; From e92a0cd10dc20ced072c77a0183a6112179f4639 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 25 Jul 2014 02:14:35 +0400 Subject: [PATCH 71/71] UAVCAN update for #1229 (windoze) --- uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uavcan b/uavcan index f51ee9cdec..d84fc8a846 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit f51ee9cdecd7841f6208bfb0b16228888daad663 +Subproject commit d84fc8a84678d93f97f93b240c81472797ca5889