From 8d67483d182990d54fd348a8b4fb6c5c7de85795 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 23 Aug 2015 21:28:47 +0300 Subject: [PATCH 01/11] UAVCAN: using only primary interface for servers --- src/modules/uavcan/uavcan_main.cpp | 2 +- src/modules/uavcan/uavcan_servers.cpp | 11 ++++++----- src/modules/uavcan/uavcan_servers.hpp | 13 +++++++------ 3 files changed, 14 insertions(+), 12 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ed3ba5e445..5e4c8b228f 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -194,7 +194,7 @@ int UavcanNode::start_fw_server() if (_servers == nullptr) { - rv = UavcanServers::start(2, _node); + rv = UavcanServers::start(_node); if (rv >= 0) { /* diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 1713b282e7..52f0ce9697 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -76,7 +76,7 @@ UavcanServers *UavcanServers::_instance; UavcanServers::UavcanServers(uavcan::INode &main_node) : _subnode_thread(-1), - _vdriver(UAVCAN_STM32_NUM_IFACES, uavcan_stm32::SystemClock::instance()), + _vdriver(NumIfaces, uavcan_stm32::SystemClock::instance()), _subnode(_vdriver, uavcan_stm32::SystemClock::instance()), _main_node(main_node), _tracer(), @@ -101,7 +101,7 @@ UavcanServers::~UavcanServers() _main_node.getDispatcher().removeRxFrameListener(); } -int UavcanServers::stop(void) +int UavcanServers::stop() { UavcanServers *server = instance(); @@ -123,7 +123,7 @@ int UavcanServers::stop(void) return 0; } -int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) +int UavcanServers::start(uavcan::INode &main_node) { if (_instance != nullptr) { warnx("Already started"); @@ -140,7 +140,7 @@ int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) return -2; } - int rv = _instance->init(num_ifaces); + int rv = _instance->init(); if (rv < 0) { warnx("Node init failed: %d", rv); @@ -174,7 +174,7 @@ int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) return rv; } -int UavcanServers::init(unsigned num_ifaces) +int UavcanServers::init() { errno = 0; @@ -268,6 +268,7 @@ int UavcanServers::init(unsigned num_ifaces) return OK; } + __attribute__((optimize("-O0"))) pthread_addr_t UavcanServers::run(pthread_addr_t) { diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 9a68a8cc25..9d21b8f93f 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -72,9 +72,11 @@ */ class UavcanServers { + static constexpr unsigned NumIfaces = 1; // UAVCAN_STM32_NUM_IFACES + static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; - static constexpr unsigned MaxCanFramsPerTransfer = 63; + static constexpr unsigned MaxCanFramesPerTransfer = 63; /** * This number is based on the worst case max number of frames per interface. With @@ -84,7 +86,7 @@ class UavcanServers * 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver. */ static constexpr unsigned QueuePoolSize = - (UAVCAN_STM32_NUM_IFACES * uavcan::MemPoolBlockSize *MaxCanFramsPerTransfer); + (NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer); static constexpr unsigned StackSize = 3500; static constexpr unsigned Priority = 120; @@ -96,8 +98,8 @@ public: virtual ~UavcanServers(); - static int start(unsigned num_ifaces, uavcan::INode &main_node); - static int stop(void); + static int start(uavcan::INode &main_node); + static int stop(); SubNode &get_node() { return _subnode; } @@ -116,8 +118,7 @@ private: pthread_t _subnode_thread; pthread_mutex_t _subnode_mutex; - int init(unsigned num_ifaces); - void deinit(void); + int init(); pthread_addr_t run(pthread_addr_t); From ae83543b63e3e9410dd2a9d6aab93367189fc19e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Aug 2015 18:12:51 +0200 Subject: [PATCH 02/11] Save RAM on UART buffer size --- nuttx-configs/px4fmu-v2/nsh/defconfig | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index e828e42be0..59abc22fb6 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -564,7 +564,7 @@ CONFIG_USART1_2STOP=0 # USART2 Configuration # CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_TXBUFSIZE=1860 +CONFIG_USART2_TXBUFSIZE=1100 CONFIG_USART2_BAUD=57600 CONFIG_USART2_BITS=8 CONFIG_USART2_PARITY=0 @@ -575,8 +575,8 @@ CONFIG_USART2_OFLOWCONTROL=y # # USART3 Configuration # -CONFIG_USART3_RXBUFSIZE=400 -CONFIG_USART3_TXBUFSIZE=400 +CONFIG_USART3_RXBUFSIZE=300 +CONFIG_USART3_TXBUFSIZE=300 CONFIG_USART3_BAUD=57600 CONFIG_USART3_BITS=8 CONFIG_USART3_PARITY=0 @@ -587,8 +587,8 @@ CONFIG_USART3_OFLOWCONTROL=y # # UART4 Configuration # -CONFIG_UART4_RXBUFSIZE=400 -CONFIG_UART4_TXBUFSIZE=400 +CONFIG_UART4_RXBUFSIZE=300 +CONFIG_UART4_TXBUFSIZE=300 CONFIG_UART4_BAUD=57600 CONFIG_UART4_BITS=8 CONFIG_UART4_PARITY=0 @@ -599,8 +599,8 @@ CONFIG_UART4_2STOP=0 # # USART6 Configuration # -CONFIG_USART6_RXBUFSIZE=400 -CONFIG_USART6_TXBUFSIZE=400 +CONFIG_USART6_RXBUFSIZE=300 +CONFIG_USART6_TXBUFSIZE=300 CONFIG_USART6_BAUD=57600 CONFIG_USART6_BITS=8 CONFIG_USART6_PARITY=0 @@ -611,8 +611,8 @@ CONFIG_USART6_2STOP=0 # # UART7 Configuration # -CONFIG_UART7_RXBUFSIZE=400 -CONFIG_UART7_TXBUFSIZE=400 +CONFIG_UART7_RXBUFSIZE=300 +CONFIG_UART7_TXBUFSIZE=300 CONFIG_UART7_BAUD=57600 CONFIG_UART7_BITS=8 CONFIG_UART7_PARITY=0 @@ -623,8 +623,8 @@ CONFIG_UART7_2STOP=0 # # UART8 Configuration # -CONFIG_UART8_RXBUFSIZE=400 -CONFIG_UART8_TXBUFSIZE=400 +CONFIG_UART8_RXBUFSIZE=300 +CONFIG_UART8_TXBUFSIZE=300 CONFIG_UART8_BAUD=57600 CONFIG_UART8_BITS=8 CONFIG_UART8_PARITY=0 @@ -667,7 +667,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=600 -CONFIG_CDCACM_TXBUFSIZE=5000 +CONFIG_CDCACM_TXBUFSIZE=4000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" From 5ed223221c85c8f9d86f114803b30f97fae8c713 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Sep 2015 10:54:09 +0200 Subject: [PATCH 03/11] EKF: Fix param naming --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index c75a9e547d..d23dd1b9e9 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -119,7 +119,7 @@ AttitudePositionEstimatorEKF *g_estimator = nullptr; } AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : - SuperBlock(NULL, "EKF"), + SuperBlock(NULL, "PE"), _task_should_exit(false), _task_running(false), _estimator_task(-1), @@ -208,9 +208,9 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _newRangeData(false), _mavlink_fd(-1), - _mag_offset_x(this, "PE_MAGB_X"), - _mag_offset_y(this, "PE_MAGB_Y"), - _mag_offset_z(this, "PE_MAGB_Z"), + _mag_offset_x(this, "MAGB_X"), + _mag_offset_y(this, "MAGB_Y"), + _mag_offset_z(this, "MAGB_Z"), _parameters{}, _parameter_handles{}, _ekf(nullptr), From 27bf924a408c354a12d9b124a6af7035d8d0d319 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 23 Sep 2015 11:41:42 +0200 Subject: [PATCH 04/11] fix handling of mavlink mode argument --- src/modules/mavlink/mavlink_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ea810c8233..90729fb138 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1484,9 +1484,11 @@ Mavlink::task_main(int argc, char *argv[]) } else if (strcmp(myoptarg, "onboard") == 0) { _mode = MAVLINK_MODE_ONBOARD; - } else if (strcmp(optarg, "osd") == 0) { + + } else if (strcmp(myoptarg, "osd") == 0) { _mode = MAVLINK_MODE_OSD; - } else if (strcmp(optarg, "config") == 0) { + + } else if (strcmp(myoptarg, "config") == 0) { _mode = MAVLINK_MODE_CONFIG; } From f424cc6b18f0d0382eedbb99bb80a4ccdc086ef3 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 7 Aug 2015 11:18:56 -1000 Subject: [PATCH 05/11] Using the uavcan stm32 clock driver to support UAVCAN time syncing Conflicts: src/lib/uavcan src/modules/systemlib/print_load_posix.c src/modules/uavcannode/module.mk --- src/lib/uavcan | 2 +- src/modules/uavcan/module.mk | 6 +-- src/modules/uavcan/uavcan_clock.cpp | 81 ----------------------------- 3 files changed, 4 insertions(+), 85 deletions(-) delete mode 100644 src/modules/uavcan/uavcan_clock.cpp diff --git a/src/lib/uavcan b/src/lib/uavcan index 3ae5400aa5..50dc08663a 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 3ae5400aa5ead18139106d30f730114d5e9b65dd +Subproject commit 50dc08663af2d9b55eae0ccf5f07c63db0ee8907 diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index da62e55634..f846fd27bc 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -44,8 +44,7 @@ WFRAME_LARGER_THAN = 1400 # Main SRCS += uavcan_main.cpp \ - uavcan_servers.cpp \ - uavcan_clock.cpp \ + uavcan_servers.cpp \ uavcan_params.c # Actuators @@ -70,7 +69,8 @@ override EXTRADEFINES := $(EXTRADEFINES) \ -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \ -DUAVCAN_NO_ASSERTIONS \ -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 \ --DUAVCAN_MAX_NETWORK_SIZE_HINT=16 +-DUAVCAN_MAX_NETWORK_SIZE_HINT=16 \ +-DUAVCAN_STM32_TIMER_NUMBER=2 # # libuavcan drivers for STM32 diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp deleted file mode 100644 index fe8ba406a5..0000000000 --- a/src/modules/uavcan/uavcan_clock.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -#include -#include - -/** - * @file uavcan_clock.cpp - * - * Implements a clock for the CAN node. - * - * @author Pavel Kirienko - */ - -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(); - -uavcan::uint64_t getUtcUSecFromCanInterrupt() -{ - return 0; -} - -} // namespace clock - -SystemClock &SystemClock::instance() -{ - static SystemClock inst; - return inst; -} - -} - From d69be4b554dbaead3fba5d42c2e5de1e056acf37 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Sat, 8 Aug 2015 08:00:00 -1000 Subject: [PATCH 06/11] Added UAVCAN Time Synchronization Master capabilities to FMU --- src/modules/uavcan/module.mk | 2 +- src/modules/uavcan/uavcan_main.cpp | 68 +++++++++++++++++++++++++++++- src/modules/uavcan/uavcan_main.hpp | 14 +++++- 3 files changed, 81 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index f846fd27bc..77ff77c6d7 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -70,7 +70,7 @@ override EXTRADEFINES := $(EXTRADEFINES) \ -DUAVCAN_NO_ASSERTIONS \ -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 \ -DUAVCAN_MAX_NETWORK_SIZE_HINT=16 \ --DUAVCAN_STM32_TIMER_NUMBER=2 +-DUAVCAN_STM32_TIMER_NUMBER=5 # # libuavcan drivers for STM32 diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 5e4c8b228f..9daff4b075 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -77,7 +77,10 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), _node_mutex(), - _esc_controller(_node) + _esc_controller(_node), + _time_sync_master(_node), + _time_sync_slave(_node), + _master_timer(_node) { _task_should_exit = false; _fw_server_action = None; @@ -459,6 +462,48 @@ int UavcanNode::add_poll_fd(int fd) } +void UavcanNode::handle_time_sync(const uavcan::TimerEvent &) +{ + + /* + * Check whether there are higher priority masters in the network. + * If there are, we need to activate the local slave in order to sync with them. + */ + if (_time_sync_slave.isActive()) { // "Active" means that the slave tracks at least one remote master in the network + if (_node.getNodeID() < _time_sync_slave.getMasterNodeID()) { + /* + * We're the highest priority master in the network. + * We need to suppress the slave now to prevent it from picking up unwanted sync messages from + * lower priority masters. + */ + _time_sync_slave.suppress(true); // SUPPRESS + + } else { + /* + * There is at least one higher priority master in the network. + * We need to allow the slave to adjust our local clock in order to be in sync. + */ + _time_sync_slave.suppress(false); // UNSUPPRESS + } + + } else { + /* + * There are no other time sync masters in the network, so we're the only time source. + * The slave must be suppressed anyway to prevent it from disrupting the local clock if a new + * lower priority master suddenly appears in the network. + */ + _time_sync_slave.suppress(true); + } + + /* + * Publish the sync message now, even if we're not a higher priority master. + * Other nodes will be able to pick the right master anyway. + */ + _time_sync_master.publish(); +} + + + int UavcanNode::run() { (void)pthread_mutex_lock(&_node_mutex); @@ -472,6 +517,27 @@ int UavcanNode::run() memset(&_outputs, 0, sizeof(_outputs)); + /* + * Set up the time synchronization + */ + + const int slave_init_res = _time_sync_slave.start(); + if (slave_init_res < 0) + { + warnx("Failed to start time_sync_slave"); + _task_should_exit = true; + } + + /* When we have a system wide notion of time update (i.e the transition from the initial + * System RTC setting to the GPS) we would call uavcan_stm32::clock::setUtc() when that + * happens, but for now we use adjustUtc with a correction of 0 + */ + uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0)); + _master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync)); + _master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000)); + + + const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); if (busevent_fd < 0) { diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index acc1c32843..098d8ea55d 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -36,6 +36,9 @@ #include #include +#include +#include + #include #include @@ -48,7 +51,7 @@ #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" -# include "uavcan_servers.hpp" +#include "uavcan_servers.hpp" /** * @file uavcan_main.hpp @@ -121,6 +124,7 @@ public: void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;} private: + void fill_node_info(); int init(uavcan::NodeID node_id); void node_spin_once(); @@ -150,6 +154,8 @@ private: pthread_mutex_t _node_mutex; sem_t _server_command_sem; UavcanEscController _esc_controller; + uavcan::GlobalTimeSyncMaster _time_sync_master; + uavcan::GlobalTimeSyncSlave _time_sync_slave; List _sensor_bridges; ///< List of active sensor bridges @@ -175,4 +181,10 @@ private: perf_counter_t _perfcnt_node_spin_elapsed = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed"); perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed"); perf_counter_t _perfcnt_esc_mixer_total_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed"); + + void handle_time_sync(const uavcan::TimerEvent &); + + typedef uavcan::MethodBinder TimerCallback; + uavcan::TimerEventForwarder _master_timer; + }; From 05ec165ed587f386898d9f16b0d45a95bc91b8e4 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 23 Sep 2015 04:00:01 -1000 Subject: [PATCH 07/11] Updated Submudule NuttX with cstdlib fixes ==master_cstdlib --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 8891d035df..aa15c32058 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 8891d035df45c8be570cfbd9419b438679faf7ee +Subproject commit aa15c32058acbda4cc2ee51e97d31e9e49225193 From 1996d2b55a71e1a713c7388cbca9cb6b67e97506 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 21 Sep 2015 18:18:24 -1000 Subject: [PATCH 08/11] Uavcan parameter and reset command line operations --- src/modules/uavcan/module.mk | 2 +- src/modules/uavcan/uavcan_main.cpp | 326 +++++++++++++++++++++++++++-- src/modules/uavcan/uavcan_main.hpp | 31 ++- 3 files changed, 339 insertions(+), 20 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 77ff77c6d7..9873d87fbe 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,7 +40,7 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -O3 MODULE_STACKSIZE = 3200 -WFRAME_LARGER_THAN = 1400 +WFRAME_LARGER_THAN = 1416 # Main SRCS += uavcan_main.cpp \ diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 9daff4b075..96fca21ff1 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -55,6 +55,8 @@ #include "uavcan_main.hpp" #include +#include + //todo:The Inclusion of file_server_backend is killing // #include and leaving OK undefined # define OK 0 @@ -80,7 +82,8 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys _esc_controller(_node), _time_sync_master(_node), _time_sync_slave(_node), - _master_timer(_node) + _master_timer(_node), + _setget_response(0) { _task_should_exit = false; _fw_server_action = None; @@ -189,6 +192,239 @@ int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver) return rv; } +int UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp) +{ + if (resp.value.is(uavcan::protocol::param::Value::Tag::integer_value)) { + return std::printf("name: %s %lld\n", resp.name.c_str(), + resp.value.to()); + + } else if (resp.value.is(uavcan::protocol::param::Value::Tag::real_value)) { + return std::printf("name: %s %.4f\n", resp.name.c_str(), + static_cast(resp.value.to())); + + } else if (resp.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) { + return std::printf("name: %s %d\n", resp.name.c_str(), + resp.value.to()); + + } else if (resp.value.is(uavcan::protocol::param::Value::Tag::string_value)) { + return std::printf("name: %s '%s'\n", resp.name.c_str(), + resp.value.to().c_str()); + } + + return -1; +} + +void UavcanNode::cb_opcode(const uavcan::ServiceCallResult &result) +{ + uavcan::protocol::param::ExecuteOpcode::Response resp; + _callback_success = result.isSuccessful(); + resp = result.getResponse(); + _callback_success &= resp.ok; +} + +int UavcanNode::save_params(int remote_node_id) +{ + uavcan::protocol::param::ExecuteOpcode::Request opcode_req; + opcode_req.opcode = opcode_req.OPCODE_SAVE; + uavcan::ServiceClient client(_node); + client.setCallback(ExecuteOpcodeCallback(this, &UavcanNode::cb_opcode)); + _callback_success = false; + int call_res = client.call(remote_node_id, opcode_req); + + if (call_res >= 0) { + while (client.hasPendingCalls()) { + usleep(10000); + } + } + + if (!_callback_success) { + std::printf("Failed to save parameters: %d\n", call_res); + return -1; + } + + return 0; +} + +void UavcanNode::cb_restart(const uavcan::ServiceCallResult &result) +{ + uavcan::protocol::RestartNode::Response resp; + _callback_success = result.isSuccessful(); + resp = result.getResponse(); + _callback_success &= resp.ok; +} + +int UavcanNode::reset_node(int remote_node_id) +{ + uavcan::protocol::RestartNode::Request restart_req; + restart_req.magic_number = restart_req.MAGIC_NUMBER; + uavcan::ServiceClient client(_node); + client.setCallback(RestartNodeCallback(this, &UavcanNode::cb_restart)); + _callback_success = false; + int call_res = client.call(remote_node_id, restart_req); + + if (call_res >= 0) { + while (client.hasPendingCalls()) { + usleep(10000); + } + } + + if (!call_res) { + std::printf("Failed to reset node: %d\n", remote_node_id); + return -1; + } + + return 0; +} + +int UavcanNode::list_params(int remote_node_id) +{ + int rv = 0; + int index = 0; + uavcan::protocol::param::GetSet::Response resp; + set_setget_response(&resp); + + while (true) { + uavcan::protocol::param::GetSet::Request req; + req.index = index++; + _callback_success = false; + int call_res = get_set_param(remote_node_id, nullptr, req); + + if (call_res < 0 || !_callback_success) { + std::printf("Failed to get param: %d\n", call_res); + rv = -1; + break; + } + + if (resp.name.empty()) { // Empty name means no such param, which means we're finished + break; + } + + print_params(resp); + } + + free_setget_response(); + return rv; +} + + +void UavcanNode::cb_setget(const uavcan::ServiceCallResult &result) +{ + _callback_success = result.isSuccessful(); + *_setget_response = result.getResponse(); +} + +int UavcanNode::get_set_param(int remote_node_id, const char *name, uavcan::protocol::param::GetSet::Request &req) +{ + if (name != nullptr) { + req.name = name; + } + + uavcan::ServiceClient client(_node); + client.setCallback(GetSetCallback(this, &UavcanNode::cb_setget)); + _callback_success = false; + int call_res = client.call(remote_node_id, req); + + if (call_res >= 0) { + + while (client.hasPendingCalls()) { + usleep(10000); + } + + if (!_callback_success) { + call_res = -1; + } + } + + return call_res; +} + +int UavcanNode::set_param(int remote_node_id, const char *name, char *value) +{ + uavcan::protocol::param::GetSet::Request req; + uavcan::protocol::param::GetSet::Response resp; + set_setget_response(&resp); + int rv = get_set_param(remote_node_id, name, req); + + if (rv < 0 || resp.name.empty()) { + std::printf("Failed to retrieve param: %s\n", name); + rv = -1; + + } else { + + rv = 0; + req = {}; + + if (resp.value.is(uavcan::protocol::param::Value::Tag::integer_value)) { + int64_t i = std::strtoull(value, NULL, 10); + int64_t min = resp.min_value.to(); + int64_t max = resp.max_value.to(); + + if (i >= min && i <= max) { + req.value.to() = i; + + } else { + std::printf("Invalid value for: %s must be between %lld and %lld\n", name, min, max); + rv = -1; + } + + } else if (resp.value.is(uavcan::protocol::param::Value::Tag::real_value)) { + float f = static_cast(std::atof(value)); + float min = resp.min_value.to(); + float max = resp.max_value.to(); + + if (f >= min && f <= max) { + req.value.to() = f; + + } else { + std::printf("Invalid value for: %s must be between %.4f and %.4f\n", name, static_cast(min), + static_cast(max)); + rv = -1; + } + + } else if (resp.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) { + int8_t i = (value[0] == '1' || value[0] == 't') ? 1 : 0; + req.value.to() = i; + + } else if (resp.value.is(uavcan::protocol::param::Value::Tag::string_value)) { + req.value.to() = value; + } + + if (rv == 0) { + rv = get_set_param(remote_node_id, name, req); + + if (rv < 0 || resp.name.empty()) { + std::printf("Failed to set param: %s\n", name); + return -1; + } + + return 0; + } + } + + free_setget_response(); + return rv; +} + +int UavcanNode::get_param(int remote_node_id, const char *name) +{ + uavcan::protocol::param::GetSet::Request req; + uavcan::protocol::param::GetSet::Response resp; + set_setget_response(&resp); + int rv = get_set_param(remote_node_id, name, req); + + if (rv < 0 || resp.name.empty()) { + std::printf("Failed to get param: %s\n", name); + rv = -1; + + } else { + print_params(resp); + rv = 0; + } + + free_setget_response(); + return rv; +} + int UavcanNode::start_fw_server() { int rv = -1; @@ -517,24 +753,24 @@ int UavcanNode::run() memset(&_outputs, 0, sizeof(_outputs)); - /* - * Set up the time synchronization - */ + /* + * Set up the time synchronization + */ - const int slave_init_res = _time_sync_slave.start(); - if (slave_init_res < 0) - { - warnx("Failed to start time_sync_slave"); - _task_should_exit = true; - } + const int slave_init_res = _time_sync_slave.start(); - /* When we have a system wide notion of time update (i.e the transition from the initial - * System RTC setting to the GPS) we would call uavcan_stm32::clock::setUtc() when that - * happens, but for now we use adjustUtc with a correction of 0 - */ - uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0)); - _master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync)); - _master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000)); + if (slave_init_res < 0) { + warnx("Failed to start time_sync_slave"); + _task_should_exit = true; + } + + /* When we have a system wide notion of time update (i.e the transition from the initial + * System RTC setting to the GPS) we would call uavcan_stm32::clock::setUtc() when that + * happens, but for now we use adjustUtc with a correction of 0 + */ + uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0)); + _master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync)); + _master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000)); @@ -936,7 +1172,7 @@ UavcanNode::print_info() static void print_usage() { warnx("usage: \n" - "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw}"); + "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -1022,6 +1258,60 @@ int uavcan_main(int argc, char *argv[]) ::exit(0); } + /* + * Parameter setting commands + * + * uavcan param list + * uavcan param save + * uavcan param get + * uavcan param set + * + */ + int node_arg = !std::strcmp(argv[1], "reset") ? 2 : 3; + + if (!std::strcmp(argv[1], "param") || node_arg == 2) { + if (argc < node_arg + 1) { + errx(1, "Node id required"); + } + + int nodeid = atoi(argv[node_arg]); + + if (nodeid == 0 || nodeid > 127 || nodeid == inst->get_node().getNodeID().get()) { + errx(1, "Invalid Node id"); + } + + if (node_arg == 2) { + + return inst->reset_node(nodeid); + + } else if (!std::strcmp(argv[2], "list")) { + + return inst->list_params(nodeid); + + } else if (!std::strcmp(argv[2], "save")) { + + return inst->save_params(nodeid); + + } else if (!std::strcmp(argv[2], "get")) { + if (argc < 5) { + errx(1, "Name required"); + } + + return inst->get_param(nodeid, argv[4]); + + } else if (!std::strcmp(argv[2], "set")) { + if (argc < 5) { + errx(1, "Name required"); + } + + if (argc < 6) { + errx(1, "Value required"); + } + + return inst->set_param(nodeid, argv[4], argv[5]); + } + } + if (!std::strcmp(argv[1], "stop")) { if (fw) { diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 098d8ea55d..23e9c89417 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -38,6 +38,9 @@ #include #include #include +#include +#include +#include #include #include @@ -122,7 +125,11 @@ public: static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver); int fw_server(eServerAction action); void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;} - + int list_params(int remote_node_id); + int save_params(int remote_node_id); + int set_param(int remote_node_id, const char *name, char *value); + int get_param(int remote_node_id, const char *name); + int reset_node(int remote_node_id); private: void fill_node_info(); @@ -133,6 +140,16 @@ private: int start_fw_server(); int stop_fw_server(); int request_fw_check(); + int print_params(uavcan::protocol::param::GetSet::Response &resp); + int get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req); + void set_setget_response(uavcan::protocol::param::GetSet::Response *resp) + { + _setget_response = resp; + } + void free_setget_response(void) + { + _setget_response = nullptr; + } int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver @@ -187,4 +204,16 @@ private: typedef uavcan::MethodBinder TimerCallback; uavcan::TimerEventForwarder _master_timer; + bool _callback_success; + uavcan::protocol::param::GetSet::Response *_setget_response; + typedef uavcan::MethodBinder &)> GetSetCallback; + typedef uavcan::MethodBinder &)> ExecuteOpcodeCallback; + typedef uavcan::MethodBinder &)> RestartNodeCallback; + void cb_setget(const uavcan::ServiceCallResult &result); + void cb_opcode(const uavcan::ServiceCallResult &result); + void cb_restart(const uavcan::ServiceCallResult &result); + }; From 2a36067cc7bf90d96ca78fec0dfeffd62eb9be66 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 23 Sep 2015 04:18:40 -1000 Subject: [PATCH 09/11] Added reset to usage --- 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 96fca21ff1..b12f9e0211 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -1172,7 +1172,7 @@ UavcanNode::print_info() static void print_usage() { warnx("usage: \n" - "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]}"); + "\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); From 8eaf213eddd3025133d8ba6471324c14a2f956a5 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 23 Sep 2015 05:32:14 -1000 Subject: [PATCH 10/11] Update uavcan_main.cpp --- src/modules/uavcan/uavcan_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index b12f9e0211..4b782eb404 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -766,9 +766,10 @@ int UavcanNode::run() /* When we have a system wide notion of time update (i.e the transition from the initial * System RTC setting to the GPS) we would call uavcan_stm32::clock::setUtc() when that - * happens, but for now we use adjustUtc with a correction of 0 + * happens, but for now we use adjustUtc with a correction of the hrt so that the + * time bases are the same */ - uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0)); + uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(hrt_absolute_time())); _master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync)); _master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000)); From 27412ef2a28c54d343e10ec845fc4f9a2663fe26 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Sep 2015 19:24:48 +0200 Subject: [PATCH 11/11] Update README to include Snapdragon Flight --- README.md | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 1abab0df13..7cb07907ff 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,7 @@ This repository contains the PX4 Flight Core, with the main applications located * [Downloads](http://px4.io/firmware/downloads) * Releases * [Downloads](https://github.com/PX4/Firmware/releases) -* Mailing list: [Google Groups](http://groups.google.com/group/px4users) +* Forum / Mailing list: [Google Groups](http://groups.google.com/group/px4users) ### Users ### @@ -29,7 +29,7 @@ Contributing guide: * [PX4 Contribution Guide](http://px4.io/dev/contributing) Software in the Loop guide: -Use software in the loop [to get started with the codebase](https://github.com/PX4/Firmware/tree/master/posix-configs/SITL) +Use software in the loop [to get started with the codebase](https://pixhawk.org/dev/simulation/native_sitl). Developer guide: http://px4.io/dev/ @@ -38,8 +38,10 @@ Testing guide: http://px4.io/dev/unit_tests This repository contains code supporting these boards: + * [Snapdragon Flight](https://www.intrinsyc.com/qualcomm-snapdragon-flight/) * FMUv1.x - * FMUv2.x + * FMUv2.x (Pixhawk) + * FMUv3.x (Pixhawk 2) * AeroCore (v1 and v2) * STM32F4Discovery (basic support) [Tutorial](https://pixhawk.org/modules/stm32f4discovery)