From 7813566e6680f4940989fb91760ddb0782b05858 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 6 May 2014 19:30:45 +0400 Subject: [PATCH 01/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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 3935540c7df0f67946b637af6dc2d3145453c326 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Jul 2014 16:23:43 +0200 Subject: [PATCH 41/52] 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 42/52] 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 43/52] 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 44/52] 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 45/52] 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 46/52] 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 47/52] 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 48/52] 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 49/52] 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 50/52] 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 51/52] 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 52/52] 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)