From e227f139a5b362f308a20197e87aa79faec62819 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Oct 2015 17:24:14 +0300 Subject: [PATCH 001/136] Libuavcan switched to footprint_reduction --- src/modules/uavcan/libuavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/libuavcan b/src/modules/uavcan/libuavcan index 0643879922..4e4d9b7854 160000 --- a/src/modules/uavcan/libuavcan +++ b/src/modules/uavcan/libuavcan @@ -1 +1 @@ -Subproject commit 0643879922239930cf7482777356f06891c26616 +Subproject commit 4e4d9b7854b7821f42fd83d89885cc489cc18c33 From 7373d99d72db00d76a6cb2a776d5e3f31d7d90fb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Oct 2015 18:56:07 +0300 Subject: [PATCH 002/136] UAVCAN CMakeLists - removed useless definitions --- src/modules/uavcan/CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt index b8c899840a..ce348ec910 100644 --- a/src/modules/uavcan/CMakeLists.txt +++ b/src/modules/uavcan/CMakeLists.txt @@ -37,15 +37,12 @@ set(UAVCAN_PLATFORM stm32 CACHE STRING "uavcan platform") string(TOUPPER "${OS}" OS_UPPER) add_definitions( -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 - -DUAVCAN_MAX_NETWORK_SIZE_HINT=16 -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 -DUAVCAN_NO_ASSERTIONS -DUAVCAN_PLATFORM=stm32 -DUAVCAN_STM32_${OS_UPPER}=1 -DUAVCAN_STM32_NUM_IFACES=2 -DUAVCAN_STM32_TIMER_NUMBER=5 - -DUAVCAN_USE_CPP03=ON - -DUAVCAN_USE_EXTERNAL_SNPRINT ) add_subdirectory(libuavcan EXCLUDE_FROM_ALL) From 407191d4ab1a90bb413c3db1241cbba8adb95172 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Oct 2015 19:37:18 +0300 Subject: [PATCH 003/136] UAVCAN driver transformed to use global memory pool --- src/modules/uavcan/uavcan_main.cpp | 3 ++- src/modules/uavcan/uavcan_main.hpp | 20 ++++++++++---- src/modules/uavcan/uavcan_servers.cpp | 4 +-- src/modules/uavcan/uavcan_servers.hpp | 26 ++++--------------- .../uavcan/uavcan_virtual_can_driver.hpp | 18 +++++-------- 5 files changed, 31 insertions(+), 40 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 1ba9d345fb..871551dd2c 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -77,7 +77,8 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), - _node(can_driver, system_clock), + _pool_allocator(MemoryPoolBlockCapacitySoftLimit, MemoryPoolBlockCapacityHardLimit), + _node(can_driver, system_clock, _pool_allocator), _node_mutex(), _esc_controller(_node), _time_sync_master(_node), diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 6a1d2f391d..05ceca83f6 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -80,7 +81,6 @@ class UavcanNode : public device::CDev static constexpr unsigned PollTimeoutMs = 10; - static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; /* * This memory is reserved for uavcan to use for queuing CAN frames. * At 1Mbit there is approximately one CAN frame every 145 uS. @@ -96,8 +96,10 @@ class UavcanNode : public device::CDev static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At static constexpr unsigned StackSize = 1800; + static constexpr unsigned MemoryPoolBlockCapacitySoftLimit = 250; + static constexpr unsigned MemoryPoolBlockCapacityHardLimit = 500; + public: - typedef uavcan::Node Node; typedef uavcan_stm32::CanInitHelper CanInitHelper; enum eServerAction {None, Start, Stop, CheckFW , Busy}; @@ -109,7 +111,7 @@ public: static int start(uavcan::NodeID node_id, uint32_t bitrate); - Node &get_node() { return _node; } + uavcan::Node<> &get_node() { return _node; } // TODO: move the actuator mixing stuff into the ESC controller class static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); @@ -130,8 +132,8 @@ public: 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: +private: void fill_node_info(); int init(uavcan::NodeID node_id); void node_spin_once(); @@ -167,7 +169,15 @@ private: static UavcanNode *_instance; ///< singleton pointer - Node _node; ///< library instance + struct MemoryPoolSynchronizer + { + const ::irqstate_t state = ::irqsave(); + ~MemoryPoolSynchronizer() { ::irqrestore(state); } + }; + + uavcan::HeapBasedPoolAllocator _pool_allocator; + + uavcan::Node<> _node; ///< library instance pthread_mutex_t _node_mutex; px4_sem_t _server_command_sem; UavcanEscController _esc_controller; diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 713a4e00f7..8ae013ce48 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -85,8 +85,8 @@ UavcanServers *UavcanServers::_instance; UavcanServers::UavcanServers(uavcan::INode &main_node) : _subnode_thread(-1), - _vdriver(NumIfaces, uavcan_stm32::SystemClock::instance()), - _subnode(_vdriver, uavcan_stm32::SystemClock::instance()), + _vdriver(NumIfaces, uavcan_stm32::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota), + _subnode(_vdriver, uavcan_stm32::SystemClock::instance(), main_node.getAllocator()), _main_node(main_node), _tracer(), _storage_backend(), diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 0b83baa941..9bf9705771 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -81,24 +81,10 @@ class UavcanServers { static constexpr unsigned NumIfaces = 1; // UAVCAN_STM32_NUM_IFACES - static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; - - static constexpr unsigned MaxCanFramesPerTransfer = 63; - - /** - * This number is based on the worst case max number of frames per interface. With - * MemPoolBlockSize set at 48 this is 6048 Bytes. - * - * The servers can be forced to use the primary interface only, this can be achieved simply by passing - * 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver. - */ - static constexpr unsigned QueuePoolSize = - (NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer); - static constexpr unsigned StackSize = 6000; static constexpr unsigned Priority = 120; - typedef uavcan::SubNode SubNode; + static constexpr unsigned VirtualIfaceBlockAllocationQuota = 80; public: UavcanServers(uavcan::INode &main_node); @@ -108,7 +94,7 @@ public: static int start(uavcan::INode &main_node); static int stop(); - SubNode &get_node() { return _subnode; } + uavcan::SubNode<> &get_node() { return _subnode; } static UavcanServers *instance() { return _instance; } @@ -131,12 +117,10 @@ private: static UavcanServers *_instance; ///< singleton pointer - typedef VirtualCanDriver vCanDriver; + VirtualCanDriver _vdriver; - vCanDriver _vdriver; - - uavcan::SubNode _subnode; ///< library instance - uavcan::INode &_main_node; ///< library instance + uavcan::SubNode<> _subnode; + uavcan::INode &_main_node; uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer; uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend; diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp index 1c861e481c..500750c441 100644 --- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -329,11 +329,7 @@ public: /** * Objects of this class are owned by the sub-node thread. * This class does not use heap memory. - * @tparam SharedMemoryPoolSize Amount of memory, in bytes, that will be statically allocated for the - * memory pool that will be shared across all interfaces for RX/TX queues. - * Typically this value should be no less than 4K per interface. */ -template class VirtualCanDriver : public uavcan::ICanDriver, public uavcan::IRxFrameListener, public ITxQueueInjector, @@ -402,7 +398,7 @@ class VirtualCanDriver : public uavcan::ICanDriver, Event event_; ///< Used to unblock the select() call when IO happens. pthread_mutex_t driver_mutex_; ///< Shared across all ifaces - uavcan::PoolAllocator allocator_; ///< Shared across all ifaces + uavcan::IPoolAllocator& allocator_; ///< Shared across all ifaces uavcan::LazyConstructor ifaces_[uavcan::MaxCanIfaces]; const unsigned num_ifaces_; uavcan::ISystemClock &clock_; @@ -476,7 +472,11 @@ class VirtualCanDriver : public uavcan::ICanDriver, } public: - VirtualCanDriver(unsigned arg_num_ifaces, uavcan::ISystemClock &system_clock) : + VirtualCanDriver(unsigned arg_num_ifaces, + uavcan::ISystemClock &system_clock, + uavcan::IPoolAllocator& allocator, + unsigned virtual_iface_block_allocation_quota) : + allocator_(allocator), num_ifaces_(arg_num_ifaces), clock_(system_clock) { @@ -485,11 +485,7 @@ public: assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces); - const unsigned quota_per_iface = allocator_.getNumBlocks() / num_ifaces_; - const unsigned quota_per_queue = quota_per_iface; // 2x overcommit - - UAVCAN_TRACE("VirtualCanDriver", "Total blocks: %u, quota per queue: %u", - unsigned(allocator_.getNumBlocks()), unsigned(quota_per_queue)); + const unsigned quota_per_queue = virtual_iface_block_allocation_quota; // 2x overcommit for (unsigned i = 0; i < num_ifaces_; i++) { ifaces_[i].template construct Date: Fri, 16 Oct 2015 20:37:47 +0300 Subject: [PATCH 004/136] Libuavcan update --- src/modules/uavcan/libuavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/libuavcan b/src/modules/uavcan/libuavcan index 4e4d9b7854..9a432c0323 160000 --- a/src/modules/uavcan/libuavcan +++ b/src/modules/uavcan/libuavcan @@ -1 +1 @@ -Subproject commit 4e4d9b7854b7821f42fd83d89885cc489cc18c33 +Subproject commit 9a432c03231781403f2a4808846183623cbfc49e From a570d1de7d20f6d0817fd6210473a58db9c3aa13 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Oct 2015 22:59:17 +0300 Subject: [PATCH 005/136] UAVCAN memory usage status and shrink --- src/modules/uavcan/uavcan_main.cpp | 21 ++++++++++++++++++++- src/modules/uavcan/uavcan_main.hpp | 2 ++ 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 871551dd2c..6508e339f0 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -1120,6 +1120,13 @@ UavcanNode::print_info() (void)pthread_mutex_lock(&_node_mutex); + // Memory status + printf("Pool allocator status:\n"); + printf("\tCapacity hard/soft: %u/%u blocks\n", + _pool_allocator.getBlockCapacityHardLimit(), _pool_allocator.getBlockCapacity()); + printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks()); + printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks()); + // ESC mixer status printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); @@ -1170,13 +1177,20 @@ UavcanNode::print_info() (void)pthread_mutex_unlock(&_node_mutex); } +void UavcanNode::shrink() +{ + (void)pthread_mutex_lock(&_node_mutex); + _pool_allocator.shrink(); + (void)pthread_mutex_unlock(&_node_mutex); +} + /* * App entry point */ 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]|reset nodeid}"); + "\tuavcan {start [fw]|status|stop [all|fw]|shrink|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -1252,6 +1266,11 @@ int uavcan_main(int argc, char *argv[]) ::exit(0); } + if (!std::strcmp(argv[1], "shrink")) { + inst->shrink(); + ::exit(0); + } + if (!std::strcmp(argv[1], "arm")) { inst->arm_actuators(true); ::exit(0); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 05ceca83f6..00cdaf418f 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -123,6 +123,8 @@ public: void print_info(); + void shrink(); + static UavcanNode *instance() { return _instance; } static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver); int fw_server(eServerAction action); From 1ace017cb88319829c80e9571d71c903a4bc78a4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Oct 2015 23:17:07 +0300 Subject: [PATCH 006/136] Deallocating memory used by UAVCAN virtual iface on destruction --- src/modules/uavcan/uavcan_virtual_can_driver.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp index 500750c441..57b664068c 100644 --- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -114,6 +114,14 @@ public: uavcan::IsDynamicallyAllocatable::check(); } + ~Queue() + { + while (!isEmpty()) + { + pop(); + } + } + bool isEmpty() const { return list_.isEmpty(); } /** From edfb19d9afa3bcba28dbf28227ee11a13a3dc250 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 01:06:35 +0300 Subject: [PATCH 007/136] Libuavcan update --- src/modules/uavcan/libuavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/libuavcan b/src/modules/uavcan/libuavcan index 9a432c0323..9b092509c9 160000 --- a/src/modules/uavcan/libuavcan +++ b/src/modules/uavcan/libuavcan @@ -1 +1 @@ -Subproject commit 9a432c03231781403f2a4808846183623cbfc49e +Subproject commit 9b092509c93794ec60484b2864be7a2995aefac6 From ca4e55fec301bb80af20a94fdfcdbf8d9551a83d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 03:24:59 +0300 Subject: [PATCH 008/136] UAVCAN allocator as a dedicated type; reporting a warning if memory leak is deetcted upon destruction --- src/modules/uavcan/allocator.hpp | 73 ++++++++++++++++++++++++++++++ src/modules/uavcan/uavcan_main.cpp | 1 - src/modules/uavcan/uavcan_main.hpp | 12 +---- 3 files changed, 75 insertions(+), 11 deletions(-) create mode 100644 src/modules/uavcan/allocator.hpp diff --git a/src/modules/uavcan/allocator.hpp b/src/modules/uavcan/allocator.hpp new file mode 100644 index 0000000000..db34aa04fb --- /dev/null +++ b/src/modules/uavcan/allocator.hpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (C) 2015 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. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#pragma once + +#include +#include +#include + +// TODO: Entire UAVCAN application should be moved into a namespace later; this is the first step. +namespace uavcan_node +{ + +struct AllocatorSynchronizer +{ + const ::irqstate_t state = ::irqsave(); + ~AllocatorSynchronizer() { ::irqrestore(state); } +}; + +struct Allocator : public uavcan::HeapBasedPoolAllocator +{ + static constexpr unsigned CapacitySoftLimit = 250; + static constexpr unsigned CapacityHardLimit = 500; + + Allocator() : + uavcan::HeapBasedPoolAllocator(CapacitySoftLimit, CapacityHardLimit) + { } + + ~Allocator() + { + if (getNumAllocatedBlocks() > 0) + { + warnx("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST", + getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize); + } + } +}; + +} diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 6508e339f0..affaa36b0a 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -77,7 +77,6 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), - _pool_allocator(MemoryPoolBlockCapacitySoftLimit, MemoryPoolBlockCapacityHardLimit), _node(can_driver, system_clock, _pool_allocator), _node_mutex(), _esc_controller(_node), diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 00cdaf418f..b91765591d 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -56,6 +56,7 @@ #include "sensors/sensor_bridge.hpp" #include "uavcan_servers.hpp" +#include "allocator.hpp" /** * @file uavcan_main.hpp @@ -96,9 +97,6 @@ class UavcanNode : public device::CDev static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At static constexpr unsigned StackSize = 1800; - static constexpr unsigned MemoryPoolBlockCapacitySoftLimit = 250; - static constexpr unsigned MemoryPoolBlockCapacityHardLimit = 500; - public: typedef uavcan_stm32::CanInitHelper CanInitHelper; enum eServerAction {None, Start, Stop, CheckFW , Busy}; @@ -171,13 +169,7 @@ private: static UavcanNode *_instance; ///< singleton pointer - struct MemoryPoolSynchronizer - { - const ::irqstate_t state = ::irqsave(); - ~MemoryPoolSynchronizer() { ::irqrestore(state); } - }; - - uavcan::HeapBasedPoolAllocator _pool_allocator; + uavcan_node::Allocator _pool_allocator; uavcan::Node<> _node; ///< library instance pthread_mutex_t _node_mutex; From 109bee855b3d734b63fe88bd3eec5fdc8d443b58 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 03:49:02 +0300 Subject: [PATCH 009/136] Node on leaked memory in UAVCAN driver --- src/modules/uavcan/uavcan_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index affaa36b0a..ed5814a3f4 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -1069,6 +1069,7 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) unsigned buflen = strnlen(buf, 1024); if (_mixers == nullptr) { + // TODO: Do we have to delete it when stopping? _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); } From 6c4f09c0e4fe3a243bb68c9bcac03110b1c2a4d1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 04:05:35 +0300 Subject: [PATCH 010/136] Fixed memory leak in UAVCAN baro driver --- src/modules/uavcan/sensors/baro.cpp | 15 ++++----------- src/modules/uavcan/sensors/baro.hpp | 4 +++- 2 files changed, 7 insertions(+), 12 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 694a3988a1..36f28a423c 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -44,7 +44,7 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) : UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)), _sub_air_pressure_data(node), _sub_air_temperature_data(node), - _reports(nullptr) + _reports(2, sizeof(baro_report)) { } int UavcanBarometerBridge::init() @@ -55,13 +55,6 @@ int UavcanBarometerBridge::init() return res; } - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); - - if (_reports == nullptr) { - return -1; - } - res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb)); if (res < 0) { @@ -91,7 +84,7 @@ ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t bufl } while (count--) { - if (_reports->get(baro_buf)) { + if (_reports.get(baro_buf)) { ret += sizeof(*baro_buf); baro_buf++; } @@ -132,7 +125,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { + if (!_reports.resize(arg)) { irqrestore(flags); return -ENOMEM; } @@ -186,7 +179,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; // add to the ring buffer - _reports->force(&report); + _reports.force(&report); publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 38cf6ab89a..ed1e308515 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -76,8 +76,10 @@ private: uavcan::Subscriber _sub_air_pressure_data; uavcan::Subscriber _sub_air_temperature_data; + + ringbuffer::RingBuffer _reports; + unsigned _msl_pressure = 101325; - ringbuffer::RingBuffer *_reports; float last_temperature_kelvin = 0.0f; }; From 9d86dbb6a1c3625f912ffc9470c0184c274d0da2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 19:50:01 +0300 Subject: [PATCH 011/136] Fixed memory leaks in the primary UAVCAN thread --- src/modules/uavcan/uavcan_main.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ed5814a3f4..3d7580b534 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -144,10 +144,9 @@ UavcanNode::~UavcanNode() } while (_task != -1); } - /* clean up the alternate device node */ - // unregister_driver(PWM_OUTPUT_DEVICE_PATH); - - ::close(_armed_sub); + (void)::close(_armed_sub); + (void)::close(_test_motor_sub); + (void)::close(_actuator_direct_sub); // Removing the sensor bridges auto br = _sensor_bridges.getHead(); @@ -166,6 +165,10 @@ UavcanNode::~UavcanNode() pthread_mutex_destroy(&_node_mutex); px4_sem_destroy(&_server_command_sem); + // Is it allowed to delete it like that? + if (_mixers != nullptr) { + delete _mixers; + } } int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver) @@ -965,6 +968,8 @@ int UavcanNode::run() } } + (void)::close(busevent_fd); + teardown(); warnx("exiting."); @@ -1069,7 +1074,6 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) unsigned buflen = strnlen(buf, 1024); if (_mixers == nullptr) { - // TODO: Do we have to delete it when stopping? _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); } From 5b9fc2d9fa16a7ac9f6ddfb4f0a29d8ef4b3ebfb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 20:29:49 +0300 Subject: [PATCH 012/136] Proper termination of UAVACN server thread --- src/modules/uavcan/uavcan_servers.cpp | 17 ++++++++++------- src/modules/uavcan/uavcan_servers.hpp | 1 + 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 8ae013ce48..ca5bcede66 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -141,13 +141,14 @@ int UavcanServers::stop() return -1; } - _instance = nullptr; - - if (server->_subnode_thread != -1) { - pthread_cancel(server->_subnode_thread); - pthread_join(server->_subnode_thread, NULL); + if (server->_subnode_thread) { + warnx("stopping fw srv thread..."); + server->_subnode_thread_should_exit = true; + (void)pthread_join(server->_subnode_thread, NULL); } + _instance = nullptr; + server->_main_node.getDispatcher().removeRxFrameListener(); delete server; @@ -334,7 +335,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids)); _esc_enumeration_index = 0; - while (1) { + while (!_subnode_thread_should_exit) { if (_check_fw == true) { _check_fw = false; @@ -554,7 +555,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } } - warnx("exiting."); + _subnode_thread_should_exit = false; + + warnx("exiting"); return (pthread_addr_t) 0; } diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 9bf9705771..3cf4cbb4da 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -110,6 +110,7 @@ public: private: pthread_t _subnode_thread; pthread_mutex_t _subnode_mutex; + volatile bool _subnode_thread_should_exit = false; int init(); From 53ed0425daee0acaea4179af848f9763bfb0b11e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 20:49:08 +0300 Subject: [PATCH 013/136] Libuavcan set to master --- src/modules/uavcan/libuavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/libuavcan b/src/modules/uavcan/libuavcan index 9b092509c9..ed1d71e639 160000 --- a/src/modules/uavcan/libuavcan +++ b/src/modules/uavcan/libuavcan @@ -1 +1 @@ -Subproject commit 9b092509c93794ec60484b2864be7a2995aefac6 +Subproject commit ed1d71e639543ea5743a839c313c53237ab3a27d From 72e651bedac613c1b3e96b47c7d4feffc8aef10d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Oct 2015 21:08:03 +0300 Subject: [PATCH 014/136] UAVCAN virtual iface: removed a useless class field --- src/modules/uavcan/uavcan_virtual_can_driver.hpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp index 57b664068c..956caf11ee 100644 --- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -404,9 +404,8 @@ class VirtualCanDriver : public uavcan::ICanDriver, } }; - Event event_; ///< Used to unblock the select() call when IO happens. + Event event_; ///< Used to unblock the select() call when IO happens. pthread_mutex_t driver_mutex_; ///< Shared across all ifaces - uavcan::IPoolAllocator& allocator_; ///< Shared across all ifaces uavcan::LazyConstructor ifaces_[uavcan::MaxCanIfaces]; const unsigned num_ifaces_; uavcan::ISystemClock &clock_; @@ -484,7 +483,6 @@ public: uavcan::ISystemClock &system_clock, uavcan::IPoolAllocator& allocator, unsigned virtual_iface_block_allocation_quota) : - allocator_(allocator), num_ifaces_(arg_num_ifaces), clock_(system_clock) { @@ -496,8 +494,9 @@ public: const unsigned quota_per_queue = virtual_iface_block_allocation_quota; // 2x overcommit for (unsigned i = 0; i < num_ifaces_; i++) { - ifaces_[i].template construct(allocator_, clock_, driver_mutex_, quota_per_queue); + ifaces_[i].template + construct + (allocator, clock_, driver_mutex_, quota_per_queue); } } From 635bfb6a7b4df37add20af8f5582b23ed992e03d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Oct 2015 03:55:47 +0300 Subject: [PATCH 015/136] CAN driver RX queue reduced to 21 frames per interface; poll() timeout set to 3 ms --- src/modules/uavcan/uavcan_main.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index b91765591d..b236946e24 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -80,7 +80,7 @@ class UavcanNode : public device::CDev static constexpr unsigned FramePerSecond = MaxBitRatePerSec / bitPerFrame; static constexpr unsigned FramePerMSecond = ((FramePerSecond / 1000) + 1); - static constexpr unsigned PollTimeoutMs = 10; + static constexpr unsigned PollTimeoutMs = 3; /* * This memory is reserved for uavcan to use for queuing CAN frames. From e06c46da03d75dd01423485f91c0502724f1aa30 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Oct 2015 03:56:09 +0300 Subject: [PATCH 016/136] uavcan status output extended with CAN error reporting --- src/modules/uavcan/uavcan_main.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 3d7580b534..4b77c91648 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -1131,6 +1131,13 @@ UavcanNode::print_info() printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks()); printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks()); + // CAN driver status + printf("CAN status:\n"); + for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) { + auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i); + printf("\tCAN%u: errors: %llu\n", unsigned(i + 1), iface->getErrorCount()); + } + // ESC mixer status printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); From 96a12a60278f9c2aae7a46d0cafa7b2cf3fad457 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Oct 2015 04:13:25 +0300 Subject: [PATCH 017/136] UAVCAN extended status reporting --- src/modules/uavcan/uavcan_main.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 4b77c91648..00671decd2 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -1131,11 +1131,24 @@ UavcanNode::print_info() printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks()); printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks()); + // UAVCAN node perfcounters + printf("UAVCAN node status:\n"); + printf("\tInternal failures: %llu\n", _node.getInternalFailureCount()); + printf("\tTransfer errors: %llu\n", _node.getDispatcher().getTransferPerfCounter().getErrorCount()); + printf("\tRX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getRxTransferCount()); + printf("\tTX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getTxTransferCount()); + // CAN driver status - printf("CAN status:\n"); for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) { + printf("CAN%u status:\n", unsigned(i + 1)); + auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i); - printf("\tCAN%u: errors: %llu\n", unsigned(i + 1), iface->getErrorCount()); + printf("\tHW errors: %llu\n", iface->getErrorCount()); + + auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i); + printf("\tIO errors: %llu\n", iface_perf_cnt.errors); + printf("\tRX frames: %llu\n", iface_perf_cnt.frames_rx); + printf("\tTX frames: %llu\n", iface_perf_cnt.frames_tx); } // ESC mixer status From 1235c1f0bdd61bd5bb60df6a8e20c28b5dd4ac39 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 3 Nov 2015 08:30:14 +0100 Subject: [PATCH 018/136] gps re-init after loss --- .../position_estimator_inav/position_estimator_inav_main.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 04f3c4d918..d10ff2d0df 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -817,6 +817,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps_valid) { if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) { gps_valid = false; + ref_inited = false; //if gps gets lost it has to init again to avoid jumps mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } From dcc9e29b66e13f9a11a3a50ac6261045d8d463ca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Nov 2015 23:02:54 +0100 Subject: [PATCH 019/136] Updated injected XML file --- Tools/parameters_injected.xml | 75 ++++++++++++++++------------------- 1 file changed, 34 insertions(+), 41 deletions(-) diff --git a/Tools/parameters_injected.xml b/Tools/parameters_injected.xml index a5b6bc0883..e1fc51c619 100644 --- a/Tools/parameters_injected.xml +++ b/Tools/parameters_injected.xml @@ -15,7 +15,7 @@ 0 1 - + Speed (RPM) controller gain Speed (RPM) controller gain. Determines controller aggressiveness; units are amp-seconds per radian. Systems with @@ -24,59 +24,53 @@ decreased. Higher values result in faster response, but may result in oscillation and excessive overshoot. Lower values result in a slower, smoother response. - amp-seconds per radian - 3 + amp-seconds per radian + 3 0.00 1.00 - + Idle speed (e Hz) Idle speed (e Hz) - Hertz - 3 + Hertz + 3 0.0 100.0 - + Spin-up rate (e Hz/s) Spin-up rate (e Hz/s) - Hz/s - + Hz/s 5 1000 - + Index of this ESC in throttle command messages. Index of this ESC in throttle command messages. - Index - + Index 0 15 - + Extended status ID Extended status ID - - 1 1023 - + Extended status interval (µs) Extended status interval (µs) - µs - + µs 0 1000000 - + ESC status interval (µs) ESC status interval (µs) - µs - + µs 1000000 - + Motor current limit in amps Motor current limit in amps. This determines the maximum current controller setpoint, as well as the maximum allowable @@ -84,62 +78,61 @@ the continuous current rating listed in the motor’s specification sheet, or set equal to the motor’s specified continuous power divided by the motor voltage limit. - Amps - 3 + Amps + 3 1 80 - + Motor Kv in RPM per volt Motor Kv in RPM per volt. This can be taken from the motor’s specification sheet; accuracy will help control performance but some deviation from the specified value is acceptable. - RPM/v - 0 + RPM/v + 0 0 100 READ ONLY: Motor inductance in henries. READ ONLY: Motor inductance in henries. This is measured on start-up. - henries - 3 + henries + 3 - + Number of motor poles. Number of motor poles. Used to convert mechanical speeds to electrical speeds. This number should be taken from the motor’s specification sheet. - Poles - + Poles 2 40 - + READ ONLY: Motor resistance in ohms READ ONLY: Motor resistance in ohms. This is measured on start-up. When tuning a new motor, check that this value is approximately equal to the value shown in the motor’s specification sheet. - Ohms - 3 + Ohms + 3 - + Acceleration limit (V) Acceleration limit (V) - Volts - 3 + Volts + 3 0.01 1.00 - + Motor voltage limit in volts Motor voltage limit in volts. The current controller’s commanded voltage will never exceed this value. Note that this may safely be above the nominal voltage of the motor; to determine the actual motor voltage limit, divide the motor’s rated power by the motor current limit. - Volts - 3 + Volts + 3 0 From 8ae78ab09353a7e67fde980a9c488ee0d893a01a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 11 Nov 2015 00:06:26 +0100 Subject: [PATCH 020/136] Params: Fix max range for injected params --- Tools/parameters_injected.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/parameters_injected.xml b/Tools/parameters_injected.xml index e1fc51c619..6cf856017c 100644 --- a/Tools/parameters_injected.xml +++ b/Tools/parameters_injected.xml @@ -55,7 +55,7 @@ Extended status ID Extended status ID 1 - 1023 + 1000000 Extended status interval (µs) From 40defeeceda22715a7ac4fabcdb6c0771d82aa28 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Wed, 11 Nov 2015 15:35:55 +0530 Subject: [PATCH 021/136] commander : fix led reporting --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3573f5d6eb..fc755437da 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2536,7 +2536,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { rgbled_set_color(RGBLED_COLOR_RED); } else { - if (status_local->condition_home_position_valid) { + if (status_local->condition_home_position_valid && status_local->condition_global_position_valid) { rgbled_set_color(RGBLED_COLOR_GREEN); } else { From 5fcfdb759c2a83c3e13235b0f71d8d6299d46c33 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Tue, 10 Nov 2015 19:33:14 +0530 Subject: [PATCH 022/136] commander : hotplug sensor support, better failure reporting --- msg/vehicle_status.msg | 3 + src/modules/commander/PreflightCheck.cpp | 68 ++++++++--------- src/modules/commander/PreflightCheck.h | 2 +- src/modules/commander/commander.cpp | 76 ++++++++++++++----- .../commander/state_machine_helper.cpp | 46 +++++++---- src/modules/commander/state_machine_helper.h | 2 +- src/modules/systemlib/rc_check.c | 37 ++++++--- src/modules/systemlib/rc_check.h | 2 +- 8 files changed, 153 insertions(+), 83 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index e575b9d5e3..6d01b3b58e 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -120,6 +120,8 @@ bool in_transition_mode bool condition_battery_voltage_valid bool condition_system_in_air_restore # true if we can restore in mid air bool condition_system_sensors_initialized +bool condition_system_prearm_error_reported # true if errors have already been reported +bool condition_system_hotplug_timeout # true if the hotplug sensor search is over bool condition_system_returned_to_home bool condition_auto_mission_available bool condition_global_position_valid # set to true by the commander app if the quality of the position estimate is good enough to use it for navigation @@ -139,6 +141,7 @@ bool rc_signal_lost_cmd # true if RC lost mode is commanded bool rc_input_blocked # set if RC input should be ignored temporarily uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping. +bool data_link_found_new # new datalink to GCS found bool data_link_lost # datalink to GCS lost bool data_link_lost_cmd # datalink to GCS lost mode commanded uint8 data_link_lost_counter # counts unique data link lost events diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 2a8a0695cf..bfeabb9e7e 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -114,7 +114,7 @@ int check_calibration(int fd, const char* param_template, int &devid) return !calibration_found; } -static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) +static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail) { bool success = true; @@ -124,7 +124,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in if (fd < 0) { if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); } @@ -134,7 +134,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id); if (ret) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); success = false; goto out; @@ -143,7 +143,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in ret = px4_ioctl(fd, MAGIOCSELFTEST, 0); if (ret != OK) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance); success = false; goto out; @@ -154,7 +154,7 @@ out: return success; } -static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id) +static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail) { bool success = true; @@ -164,7 +164,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, if (fd < 0) { if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); } @@ -174,7 +174,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id); if (ret) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); success = false; goto out; @@ -183,7 +183,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance); success = false; goto out; @@ -200,13 +200,13 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); /* this is frickin' fatal */ success = false; goto out; } } else { - mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); + if(report_fail) mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); /* this is frickin' fatal */ success = false; goto out; @@ -219,7 +219,7 @@ out: return success; } -static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) +static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail) { bool success = true; @@ -229,7 +229,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev if (fd < 0) { if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance); } @@ -239,7 +239,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id); if (ret) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance); success = false; goto out; @@ -248,7 +248,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev ret = px4_ioctl(fd, GYROIOCSELFTEST, 0); if (ret != OK) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance); success = false; goto out; @@ -259,7 +259,7 @@ out: return success; } -static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) +static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail) { bool success = true; @@ -269,7 +269,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev if (fd < 0) { if (!optional) { - mavlink_and_console_log_critical(mavlink_fd, + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); } @@ -294,7 +294,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev return success; } -static bool airspeedCheck(int mavlink_fd, bool optional) +static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail) { bool success = true; int ret; @@ -304,13 +304,13 @@ static bool airspeedCheck(int mavlink_fd, bool optional) if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { - mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); success = false; goto out; } if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) { - mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); // XXX do not make this fatal yet } @@ -319,7 +319,7 @@ out: return success; } -static bool gnssCheck(int mavlink_fd) +static bool gnssCheck(int mavlink_fd, bool report_fail) { bool success = true; @@ -342,7 +342,7 @@ static bool gnssCheck(int mavlink_fd) //Report failure to detect module if(!success) { - mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); } px4_close(gpsSub); @@ -350,7 +350,7 @@ static bool gnssCheck(int mavlink_fd) } bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, - bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic) + bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures) { bool failed = false; @@ -365,7 +365,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro bool required = (i < max_mandatory_mag_count); int device_id = -1; - if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) { + if (!magnometerCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) { failed = true; } @@ -376,7 +376,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check if the primary device is present */ if (!prime_found && prime_id != 0) { - mavlink_log_critical(mavlink_fd, "warning: primary compass not operational"); + if(reportFailures) mavlink_log_critical(mavlink_fd, "Warning: Primary compass not found"); failed = true; } } @@ -392,7 +392,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro bool required = (i < max_mandatory_accel_count); int device_id = -1; - if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) { + if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id, reportFailures) && required) { failed = true; } @@ -403,7 +403,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check if the primary device is present */ if (!prime_found && prime_id != 0) { - mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational"); + if(reportFailures) mavlink_log_critical(mavlink_fd, "Warning: Primary accelerometer not found"); failed = true; } } @@ -419,7 +419,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro bool required = (i < max_mandatory_gyro_count); int device_id = -1; - if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) { + if (!gyroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) { failed = true; } @@ -430,7 +430,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check if the primary device is present */ if (!prime_found && prime_id != 0) { - mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational"); + if(reportFailures) mavlink_log_critical(mavlink_fd, "Warning: Primary gyro not found"); failed = true; } } @@ -446,7 +446,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro bool required = (i < max_mandatory_baro_count); int device_id = -1; - if (!baroCheck(mavlink_fd, i, !required, device_id) && required) { + if (!baroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) { failed = true; } @@ -458,29 +458,29 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro // TODO there is no logic in place to calibrate the primary baro yet // // check if the primary device is present if (!prime_found && prime_id != 0) { - mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational"); + if(reportFailures) mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational"); failed = true; } } /* ---- AIRSPEED ---- */ if (checkAirspeed) { - if (!airspeedCheck(mavlink_fd, true)) { + if (!airspeedCheck(mavlink_fd, true, reportFailures)) { failed = true; } } /* ---- RC CALIBRATION ---- */ if (checkRC) { - if (rc_calibration_check(mavlink_fd) != OK) { - mavlink_log_critical(mavlink_fd, "RC calibration check failed"); + if (rc_calibration_check(mavlink_fd, reportFailures) != OK) { + if(reportFailures) mavlink_log_critical(mavlink_fd, "RC calibration check failed"); failed = true; } } /* ---- Global Navigation Satellite System receiver ---- */ if (checkGNSS) { - if(!gnssCheck(mavlink_fd)) { + if(!gnssCheck(mavlink_fd, reportFailures)) { failed = true; } } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index b6423a4d9a..058d6b8956 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -67,7 +67,7 @@ namespace Commander * true if the GNSS receiver should be checked **/ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, - bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic = false); + bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures = false); const unsigned max_mandatory_gyro_count = 1; const unsigned max_optional_gyro_count = 3; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3573f5d6eb..32b24a0be3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -148,6 +148,8 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 +#define HOTPLUG_SENS_TIMEOUT (8 * 1000 * 1000) /**< wait for hotplug sensors to come online for upto 10 seconds */ + #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 10000000 @@ -377,9 +379,12 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); - int checkres = prearm_check(&status, mavlink_fd_local); + int checkres = 0; + checkres = preflight_check(&status, mavlink_fd_local, false); + warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED"); + checkres = preflight_check(&status, mavlink_fd_local, true); + warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); px4_close(mavlink_fd_local); - warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); return 0; } @@ -891,6 +896,7 @@ int commander_thread_main(int argc, char *argv[]) /* not yet initialized */ commander_initialized = false; + bool sensor_fail_tune_played = false; bool arm_tune_played = false; bool was_landed = true; bool was_armed = false; @@ -1020,6 +1026,9 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; + + status.condition_system_prearm_error_reported = true; + status.condition_system_hotplug_timeout = false; status.counter++; status.timestamp = hrt_absolute_time(); @@ -1101,7 +1110,7 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - rc_calibration_check(mavlink_fd); + rc_calibration_check(mavlink_fd, true); /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -1243,6 +1252,7 @@ int commander_thread_main(int argc, char *argv[]) // Run preflight check int32_t rc_in_off = 0; + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; param_get(_param_autostart_id, &autostart_id); param_get(_param_rc_in_off, &rc_in_off); status.rc_input_mode = rc_in_off; @@ -1251,14 +1261,10 @@ int commander_thread_main(int argc, char *argv[]) status.condition_system_sensors_initialized = false; set_tune_override(TONE_STARTUP_TUNE); //normal boot tune } else { - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, - checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check); - if (!status.condition_system_sensors_initialized) { - set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune - } - else { + // sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, + checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, false); set_tune_override(TONE_STARTUP_TUNE); //normal boot tune - } } commander_boot_timestamp = hrt_absolute_time(); @@ -1354,11 +1360,11 @@ int commander_thread_main(int argc, char *argv[]) if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) { status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); } /* re-check RC calibration */ - rc_calibration_check(mavlink_fd); + rc_calibration_check(mavlink_fd, true); } /* Safety parameters */ @@ -1446,6 +1452,8 @@ int commander_thread_main(int argc, char *argv[]) !armed.armed) { bool chAirspeed = false; + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { @@ -1456,11 +1464,11 @@ int commander_thread_main(int argc, char *argv[]) if (is_hil_setup(autostart_id)) { /* HIL configuration: check only RC input */ (void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false, true); } else { /* check sensors also */ (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); } } @@ -2177,9 +2185,14 @@ int commander_thread_main(int argc, char *argv[]) if (telemetry_lost[i] && hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { - /* only report a regain */ + /* report a regain */ if (telemetry_last_dl_loss[i] > 0) { mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i); + } else if (telemetry_last_dl_loss[i] == 0){ + /* report a new link */ + mavlink_and_console_log_info(mavlink_fd, "new data link connected : #%i", i); + status.data_link_found_new = true; + status_changed = true; } telemetry_lost[i] = false; @@ -2189,6 +2202,10 @@ int commander_thread_main(int argc, char *argv[]) /* telemetry was healthy also in last iteration * we don't have to check a timeout */ have_link = true; + if(status.data_link_found_new) { + status.data_link_found_new = false; + status_changed = true; + } } } else { @@ -2414,7 +2431,7 @@ int commander_thread_main(int argc, char *argv[]) } else { set_tune(TONE_STOP_TUNE); } - + /* reset arm_tune_played when disarmed */ if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { @@ -2425,6 +2442,21 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = false; } + + /* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */ + hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + + if (!sensor_fail_tune_played && (!status.condition_system_sensors_initialized && hotplug_timeout)) { + set_tune_override(TONE_GPS_WARNING_TUNE); + sensor_fail_tune_played = true; + status_changed = true; + } + + /* update timeout flag */ + if(!(hotplug_timeout == status.condition_system_hotplug_timeout)) { + status.condition_system_hotplug_timeout = hotplug_timeout; + status_changed = true; + } counter++; @@ -2508,19 +2540,24 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu /* driving rgbled */ if (changed) { bool set_normal_color = false; - + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + /* set mode */ if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); set_normal_color = true; - } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || !status.condition_system_sensors_initialized) { + } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR || (!status.condition_system_sensors_initialized && hotplug_timeout)) { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_color(RGBLED_COLOR_RED); } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { rgbled_set_mode(RGBLED_MODE_BREATHE); set_normal_color = true; + + } else if (!status.condition_system_sensors_initialized && !hotplug_timeout) { + rgbled_set_mode(RGBLED_MODE_BREATHE); + set_normal_color = true; } else { // STANDBY_ERROR and other states rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL); @@ -3197,6 +3234,7 @@ void *commander_low_prio_loop(void *arg) // so this would be prone to false negatives. bool checkAirspeed = false; + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { @@ -3204,7 +3242,7 @@ void *commander_low_prio_loop(void *arg) } status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, - !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 21789c9c66..bdf88920fa 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -93,8 +93,6 @@ static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { "ARMING_STATE_IN_AIR_RESTORE", }; -static bool sensor_feedback_provided = false; - transition_result_t arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status const struct safety_s *safety, ///< current safety settings @@ -122,10 +120,18 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s */ int prearm_ret = OK; - /* only perform the check if we have to */ + /* only perform the pre-arm check if we have to */ if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { - prearm_ret = prearm_check(status, mavlink_fd); + prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ ); + } + /* re-run the pre-flight check as long as sensors are failing */ + if (!status->condition_system_sensors_initialized + && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || + new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) + && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */); + status->condition_system_sensors_initialized = !prearm_ret; } /* @@ -243,9 +249,11 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && (!status->condition_system_sensors_initialized)) { - if (!sensor_feedback_provided || (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { + if ((!status->condition_system_prearm_error_reported && + status->condition_system_hotplug_timeout) || + (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); - sensor_feedback_provided = true; + status->condition_system_prearm_error_reported = true; } feedback_provided = true; valid_transition = false; @@ -262,7 +270,11 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* reset feedback state */ if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR && valid_transition) { - sensor_feedback_provided = false; + status->condition_system_prearm_error_reported = false; + } + if(status->data_link_found_new == true) + { + status->condition_system_prearm_error_reported = false; } /* end of atomic state update */ @@ -731,11 +743,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en return status->nav_state != nav_state_old; } -int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) +int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm) { - /* at this point this equals the preflight check, but might add additional - * quantities later. + /* */ + bool reportFailures = false; + reportFailures = !status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout; + bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ @@ -743,12 +757,12 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) checkAirspeed = true; } - bool prearm_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true); - - if (status->usb_connected) { - prearm_ok = false; - mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); + bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures); + + if (status->usb_connected && prearm) { + preflight_ok = false; + if(reportFailures) mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); } - return !prearm_ok; + return !preflight_ok; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 084813f8cb..bdb7b13f28 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); -int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); +int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 25dea7a9b0..0f5d96f2b8 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -52,7 +52,7 @@ #define RC_INPUT_MAP_UNMAPPED 0 -int rc_calibration_check(int mavlink_fd) +int rc_calibration_check(int mavlink_fd, bool report_fail) { char nbuf[20]; @@ -74,7 +74,8 @@ int rc_calibration_check(int mavlink_fd) param_t map_parm = param_find(rc_map_mandatory[j]); if (map_parm == PARAM_INVALID) { - mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); + if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); } + /* give system time to flush error message in case there are more */ usleep(100000); map_fail_count++; @@ -86,14 +87,16 @@ int rc_calibration_check(int mavlink_fd) param_get(map_parm, &mapping); if (mapping > RC_INPUT_MAX_CHANNELS) { - mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); + if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); } + /* give system time to flush error message in case there are more */ usleep(100000); map_fail_count++; } if (mapping == 0) { - mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); + if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); } + /* give system time to flush error message in case there are more */ usleep(100000); map_fail_count++; @@ -144,35 +147,44 @@ int rc_calibration_check(int mavlink_fd) /* assert min..center..max ordering */ if (param_min < RC_INPUT_LOWEST_MIN_US) { count++; - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); + + if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); } + /* give system time to flush error message in case there are more */ usleep(100000); } if (param_max > RC_INPUT_HIGHEST_MAX_US) { count++; - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); + + if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); } + /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim < param_min) { count++; - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); + + if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); } + /* give system time to flush error message in case there are more */ usleep(100000); } if (param_trim > param_max) { count++; - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); + + if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); } + /* give system time to flush error message in case there are more */ usleep(100000); } /* assert deadzone is sane */ if (param_dz > RC_INPUT_MAX_DEADZONE_US) { - mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); + if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); } + /* give system time to flush error message in case there are more */ usleep(100000); count++; @@ -187,8 +199,11 @@ int rc_calibration_check(int mavlink_fd) if (channels_failed) { sleep(2); - mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", total_fail_count, - (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); + + if (report_fail) mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", + total_fail_count, + (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); + usleep(100000); } diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index 035f63bef4..239c8a6a13 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -47,6 +47,6 @@ __BEGIN_DECLS * @return 0 / OK if RC calibration is ok, index + 1 of the first * channel that failed else (so 1 == first channel failed) */ -__EXPORT int rc_calibration_check(int mavlink_fd); +__EXPORT int rc_calibration_check(int mavlink_fd, bool report_fail); __END_DECLS From de39e7de62cc7bc4006d890fbd808708d83e8eac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Nov 2015 11:33:36 +0100 Subject: [PATCH 023/136] Fix param meta info --- Tools/parameters_injected.xml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Tools/parameters_injected.xml b/Tools/parameters_injected.xml index 6cf856017c..2f164ee52b 100644 --- a/Tools/parameters_injected.xml +++ b/Tools/parameters_injected.xml @@ -89,9 +89,8 @@ specification sheet; accuracy will help control performance but some deviation from the specified value is acceptable. RPM/v - 0 0 - 100 + 4000 READ ONLY: Motor inductance in henries. @@ -116,7 +115,7 @@ Ohms 3 - + Acceleration limit (V) Acceleration limit (V) Volts From e8c7ee7ff2c5ccd818df35bce31f997d0c16d645 Mon Sep 17 00:00:00 2001 From: Bartosz Wawrzacz Date: Thu, 12 Nov 2015 15:03:06 +0100 Subject: [PATCH 024/136] enable logging of the RC RSSI value --- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index be08851c25..af77d955e9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1675,6 +1675,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_RC_MSG; /* Copy only the first 8 channels of 14 */ memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel)); + log_msg.body.log_RC.rssi = buf.rc.rssi; log_msg.body.log_RC.channel_count = buf.rc.channel_count; log_msg.body.log_RC.signal_lost = buf.rc.signal_lost; LOGBUFFER_WRITE_AND_COUNT(RC); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 462b753f98..6ccbec2fa9 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -189,6 +189,7 @@ struct log_STAT_s { #define LOG_RC_MSG 11 struct log_RC_s { float channel[8]; + uint8_t rssi; uint8_t channel_count; uint8_t signal_lost; }; @@ -532,7 +533,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(STAT, "BBBfBBf", "MainState,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"), LOG_FORMAT(VTOL, "f", "Arsp"), LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"), - LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), + LOG_FORMAT(RC, "ffffffffBBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,RSSI,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), From 132a57c314c94a1cb10c229d1b880b62a4e6e005 Mon Sep 17 00:00:00 2001 From: Bartosz Wawrzacz Date: Thu, 12 Nov 2015 15:01:29 +0100 Subject: [PATCH 025/136] fix the scaling when using "RSSI from input channel" functionality --- src/drivers/px4io/px4io.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 719bacb41f..609320a214 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1780,8 +1780,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) /* get RSSI from input channel */ if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { - int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) / - ((_rssi_pwm_max - _rssi_pwm_min) / 100); + int rssi = ((input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) * 100) / + (_rssi_pwm_max - _rssi_pwm_min); rssi = rssi > 100 ? 100 : rssi; rssi = rssi < 0 ? 0 : rssi; input_rc.rssi = rssi; From c655b51d363194bf8399908a218d91e3b623ef35 Mon Sep 17 00:00:00 2001 From: Bartosz Wawrzacz Date: Thu, 12 Nov 2015 16:21:11 +0100 Subject: [PATCH 026/136] fixed formatting in px4io driver --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 609320a214..2cabe98a23 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1781,7 +1781,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) /* get RSSI from input channel */ if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { int rssi = ((input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) * 100) / - (_rssi_pwm_max - _rssi_pwm_min); + (_rssi_pwm_max - _rssi_pwm_min); rssi = rssi > 100 ? 100 : rssi; rssi = rssi < 0 ? 0 : rssi; input_rc.rssi = rssi; From fc3ea7143c831452adfcadc7f176f6352ad854b3 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Thu, 12 Nov 2015 14:20:22 -0500 Subject: [PATCH 027/136] Matrix lib update. --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index 2c7a375e3d..7656385ea1 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 2c7a375e3d7ce143dd1991c9135a5a55952a8977 +Subproject commit 7656385ea1d3f0a374a8146430bc63cf02e66d6b From 199f6478ee87cb4b7ed1631793a1b7c29e8dc07b Mon Sep 17 00:00:00 2001 From: Youssef Demitri Date: Thu, 12 Nov 2015 15:30:03 +0100 Subject: [PATCH 028/136] enable online model lookup in gazebo --- Tools/sitl_run.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index b4617e40cf..8767f9f841 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -56,8 +56,8 @@ then export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:$curr_dir/Tools/sitl_gazebo/Build # Set the model path so Gazebo finds the airframes export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models - # Disable online model lookup since this is quite experimental and unstable - export GAZEBO_MODEL_DATABASE_URI="" + # The next line would disable online model lookup, can be commented in, in case of unstable behaviour. + # export GAZEBO_MODEL_DATABASE_URI="" export SITL_GAZEBO_PATH=$curr_dir/Tools/sitl_gazebo mkdir -p Tools/sitl_gazebo/Build cd Tools/sitl_gazebo/Build From 48c467f6027702c2c61f5d8a21c43201a4158ee7 Mon Sep 17 00:00:00 2001 From: Karl Schwabe Date: Sun, 8 Nov 2015 14:39:19 +0100 Subject: [PATCH 029/136] Remove CONFIG_ARCH_BOARD #ifdef dependencies from mtd Since mtd already checks whether CONFIG_MTD_RAMTRON is set in defconfig use this define to check whether to call ramtron_attach() or at24xxx_attach() in the main function, instead of checking CONFIG_ARCH_BOARD_PX4FMU-V1. Also instead of using #ifdef CONFIG_ARCH_BOARD_AEROCORE to determine whether to start mtd on spi 2 or 4, rather use a new board define PX4_SPI_BUS_RAMTRON in the board_config.h file. --- src/drivers/boards/aerocore/board_config.h | 3 +++ src/drivers/boards/px4fmu-v2/board_config.h | 1 + src/systemcmds/mtd/mtd.c | 15 ++++++--------- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index 0b52ca48ff..155173db5f 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -82,6 +82,9 @@ __BEGIN_DECLS #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) +/* SPI4--Ramtron */ +#define PX4_SPI_BUS_RAMTRON 4 + /* Nominal chip selects for devices on SPI bus #3 */ #define PX4_SPIDEV_ACCEL_MAG 0 #define PX4_SPIDEV_GYRO 1 diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 0f1f2fd4a2..ea60df9100 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -113,6 +113,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_EXT3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) #define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_RAMTRON 2 #define PX4_SPI_BUS_EXT 4 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 0f54c9c4c3..b38504c623 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -176,12 +176,9 @@ struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd, static void ramtron_attach(void) { - /* find the right spi */ -#ifdef CONFIG_ARCH_BOARD_AEROCORE - struct spi_dev_s *spi = up_spiinitialize(4); -#else - struct spi_dev_s *spi = up_spiinitialize(2); -#endif + /* initialize the right spi */ + struct spi_dev_s *spi = up_spiinitialize(PX4_SPI_BUS_RAMTRON); + /* this resets the spi bus, set correct bus speed again */ SPI_SETFREQUENCY(spi, 10 * 1000 * 1000); SPI_SETBITS(spi, 8); @@ -269,10 +266,10 @@ mtd_start(char *partition_names[], unsigned n_partitions) } if (!attached) { -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - at24xxx_attach(); -#else +#ifdef CONFIG_MTD_RAMTRON ramtron_attach(); +#else + at24xxx_attach(); #endif } From 3aa3bf8f5f8296a5ed7f620d53d70b810db53a1b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 10:33:20 +0100 Subject: [PATCH 030/136] Commander: Do not report every new data link --- src/modules/commander/commander.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5919e78f5c..817d44ab74 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2189,8 +2189,7 @@ int commander_thread_main(int argc, char *argv[]) if (telemetry_last_dl_loss[i] > 0) { mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i); } else if (telemetry_last_dl_loss[i] == 0){ - /* report a new link */ - mavlink_and_console_log_info(mavlink_fd, "new data link connected : #%i", i); + /* do not report a new data link in order to not spam the user */ status.data_link_found_new = true; status_changed = true; } From 58c04d2d319ffa00f2a99fca396bfe7affdcf2f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 11:18:30 +0100 Subject: [PATCH 031/136] sdlog2: Fix string overflow in file name --- src/modules/sdlog2/sdlog2.c | 34 ++++++++++++++++++---------------- 1 file changed, 18 insertions(+), 16 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index af77d955e9..6b98e64835 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -156,7 +156,9 @@ struct logbuffer_s lb; static pthread_mutex_t logbuffer_mutex; static pthread_cond_t logbuffer_cond; -static char log_dir[32]; +#define LOG_BASE_PATH_LEN 64 + +static char log_dir[LOG_BASE_PATH_LEN]; /* statistics counters */ static uint64_t start_time = 0; @@ -444,15 +446,15 @@ int create_log_dir() } /* print logging path, important to find log file later */ - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); + mavlink_and_console_log_info(mavlink_fd, "[log] log dir: %s", log_dir); return 0; } int open_log_file() { /* string to hold the path to the log */ - char log_file_name[32] = ""; - char log_file_path[64] = ""; + char log_file_name[64] = ""; + char log_file_path[sizeof(log_file_name) + LOG_BASE_PATH_LEN] = ""; struct tm tt; bool time_ok = get_log_time_utc_tt(&tt, false); @@ -480,7 +482,7 @@ int open_log_file() if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(mavlink_fd, "[log] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -492,10 +494,10 @@ int open_log_file() #endif if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[log] failed: %s", log_file_name); } else { - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] starting: %s", log_file_name); + mavlink_and_console_log_info(mavlink_fd, "[log] start: %s", log_file_name); } return fd; @@ -504,8 +506,8 @@ int open_log_file() int open_perf_file(const char* str) { /* string to hold the path to the log */ - char log_file_name[32] = ""; - char log_file_path[64] = ""; + char log_file_name[64] = ""; + char log_file_path[sizeof(log_file_name) + LOG_BASE_PATH_LEN] = ""; struct tm tt; bool time_ok = get_log_time_utc_tt(&tt, false); @@ -532,7 +534,7 @@ int open_perf_file(const char* str) if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(mavlink_fd, "[log] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -544,7 +546,7 @@ int open_perf_file(const char* str) #endif if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[log] failed: %s", log_file_name); } @@ -671,7 +673,7 @@ void sdlog2_start_log() /* create log dir if needed */ if (create_log_dir() != 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); + mavlink_and_console_log_critical(mavlink_fd, "[log] error creating log dir"); return; } @@ -761,7 +763,7 @@ void sdlog2_stop_log() /* free log writer performance counter */ perf_free(perf_write); - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] logging stopped"); + mavlink_and_console_log_info(mavlink_fd, "[log] logging stopped"); sdlog2_status(); } @@ -1912,7 +1914,7 @@ void sdlog2_status() float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); - mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); + mavlink_log_info(mavlink_fd, "[log] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); } } @@ -1937,7 +1939,7 @@ int check_free_space() /* use a threshold of 50 MiB */ if (statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(50 * 1024 * 1024 / statfs_buf.f_bsize)) { mavlink_and_console_log_critical(mavlink_fd, - "[sdlog2] no space on MicroSD: %u MiB", + "[log] no space on MicroSD: %u MiB", (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we do not need a flag to remember that we sent this warning because we will exit anyway */ return PX4_ERROR; @@ -1945,7 +1947,7 @@ int check_free_space() /* use a threshold of 100 MiB to send a warning */ } else if (!space_warning_sent && statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(100 * 1024 * 1024 / statfs_buf.f_bsize)) { mavlink_and_console_log_critical(mavlink_fd, - "[sdlog2] space on MicroSD low: %u MiB", + "[log] space on MicroSD low: %u MiB", (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we don't want to flood the user with warnings */ space_warning_sent = true; From 9b758e7285448e4eb6f5afc84a30388481da73d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 12:07:19 +0100 Subject: [PATCH 032/136] Set minimum takeoff alt to 2.5 meters --- src/modules/navigator/mission_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 7c2942f53e..1ef0c57e3e 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -52,7 +52,7 @@ * @unit meters * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); +PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f); /** * Enable persistent onboard mission storage From ff88fc00c06d22c94616841cfb75138bf7559d31 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:02:56 +0100 Subject: [PATCH 033/136] Commander: Preflight check reporting cleanup, add USB breaker --- src/modules/commander/PreflightCheck.cpp | 54 ++++++++++++++----- src/modules/commander/commander.cpp | 13 +++-- .../commander/state_machine_helper.cpp | 18 +++---- 3 files changed, 59 insertions(+), 26 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index bfeabb9e7e..3423ad8340 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -206,7 +206,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, goto out; } } else { - if(report_fail) mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ"); + } /* this is frickin' fatal */ success = false; goto out; @@ -229,8 +231,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev if (fd < 0) { if (!optional) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance); + } } return false; @@ -239,8 +243,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id); if (ret) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance); + } success = false; goto out; } @@ -248,8 +254,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev ret = px4_ioctl(fd, GYROIOCSELFTEST, 0); if (ret != OK) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance); + } success = false; goto out; } @@ -269,8 +277,10 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev if (fd < 0) { if (!optional) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); + } } return false; @@ -304,13 +314,17 @@ static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail) if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + } success = false; goto out; } if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); + } // XXX do not make this fatal yet } @@ -341,8 +355,10 @@ static bool gnssCheck(int mavlink_fd, bool report_fail) } //Report failure to detect module - if(!success) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + if (!success) { + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + } } px4_close(gpsSub); @@ -376,7 +392,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check if the primary device is present */ if (!prime_found && prime_id != 0) { - if(reportFailures) mavlink_log_critical(mavlink_fd, "Warning: Primary compass not found"); + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary compass not found"); + } failed = true; } } @@ -403,7 +421,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check if the primary device is present */ if (!prime_found && prime_id != 0) { - if(reportFailures) mavlink_log_critical(mavlink_fd, "Warning: Primary accelerometer not found"); + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary accelerometer not found"); + } failed = true; } } @@ -430,7 +450,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check if the primary device is present */ if (!prime_found && prime_id != 0) { - if(reportFailures) mavlink_log_critical(mavlink_fd, "Warning: Primary gyro not found"); + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary gyro not found"); + } failed = true; } } @@ -458,7 +480,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro // TODO there is no logic in place to calibrate the primary baro yet // // check if the primary device is present if (!prime_found && prime_id != 0) { - if(reportFailures) mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational"); + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "warning: primary barometer not operational"); + } failed = true; } } @@ -473,7 +497,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* ---- RC CALIBRATION ---- */ if (checkRC) { if (rc_calibration_check(mavlink_fd, reportFailures) != OK) { - if(reportFailures) mavlink_log_critical(mavlink_fd, "RC calibration check failed"); + if (reportFailures) { + mavlink_log_critical(mavlink_fd, "RC calibration check failed"); + } failed = true; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 817d44ab74..7a65e69e30 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -390,15 +390,20 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "arm")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); - arm_disarm(true, mavlink_fd_local, "command line"); - warnx("note: not updating home position on commandline arming!"); + if (TRANSITION_CHANGED == arm_disarm(true, mavlink_fd_local, "command line")) { + warnx("note: not updating home position on commandline arming!"); + } else { + warnx("arming failed"); + } px4_close(mavlink_fd_local); return 0; } if (!strcmp(argv[1], "disarm")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); - arm_disarm(false, mavlink_fd_local, "command line"); + if (TRANSITION_DENIED == arm_disarm(false, mavlink_fd_local, "command line")) { + warnx("rejected disarm"); + } px4_close(mavlink_fd_local); return 0; } @@ -2513,6 +2518,8 @@ get_circuit_breaker_params() { status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status.cb_usb = + circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY); status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); status.circuit_breaker_engaged_enginefailure_check = diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index bdf88920fa..981644c342 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -177,7 +177,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); + mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!"); feedback_provided = true; valid_transition = false; } @@ -202,10 +202,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s feedback_provided = true; valid_transition = false; } else if (status->avionics_power_rail_voltage < 4.9f) { - mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } else if (status->avionics_power_rail_voltage > 5.4f) { - mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); + mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); feedback_provided = true; } } @@ -392,7 +392,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta switch (new_state) { case vehicle_status_s::HIL_STATE_OFF: /* we're in HIL and unexpected things can happen if we disable HIL now */ - mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); + mavlink_and_console_log_critical(mavlink_fd, "Not switching off HIL (safety)"); ret = TRANSITION_DENIED; break; @@ -465,7 +465,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta closedir(d); ret = TRANSITION_CHANGED; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state"); } else { /* failed opening dir */ @@ -502,11 +502,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta } ret = TRANSITION_CHANGED; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state"); #endif } else { - mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); + mavlink_and_console_log_critical(mavlink_fd, "Not switching to HIL when armed"); ret = TRANSITION_DENIED; } break; @@ -759,9 +759,9 @@ int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures); - if (status->usb_connected && prearm) { + if (!status->cb_usb && status->usb_connected && prearm) { preflight_ok = false; - if(reportFailures) mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); + if(reportFailures) mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); } return !preflight_ok; From 0fdc0e28c77f92fba0bfa2541dc6ec0a1a85b498 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:03:14 +0100 Subject: [PATCH 034/136] Messages: Add USB breaker and control state horizontal acceleration --- msg/control_state.msg | 1 + msg/vehicle_status.msg | 1 + 2 files changed, 2 insertions(+) diff --git a/msg/control_state.msg b/msg/control_state.msg index d976fbae8d..4f130353d3 100644 --- a/msg/control_state.msg +++ b/msg/control_state.msg @@ -16,3 +16,4 @@ float32[4] q # Attitude Quaternion float32 roll_rate # Roll body angular rate (rad/s, x forward/y right/z down) float32 pitch_rate # Pitch body angular rate (rad/s, x forward/y right/z down) float32 yaw_rate # Yaw body angular rate (rad/s, x forward/y right/z down) +float32 horz_acc_mag # Magnitude of the horizontal acceleration diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 6d01b3b58e..c91672c0be 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -182,3 +182,4 @@ bool circuit_breaker_engaged_power_check bool circuit_breaker_engaged_airspd_check bool circuit_breaker_engaged_enginefailure_check bool circuit_breaker_engaged_gpsfailure_check +bool cb_usb From 835ab4f10ff4941e68ee4ded91bc7429d68a6d72 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:03:28 +0100 Subject: [PATCH 035/136] Systemlib: Add USB circuit breaker --- src/modules/systemlib/circuit_breaker.h | 1 + .../systemlib/circuit_breaker_params.c | 13 ++++++++++++ src/modules/systemlib/rc_check.c | 20 ++++++++++--------- 3 files changed, 25 insertions(+), 9 deletions(-) diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index c76e6c37f3..b4d9b5d1c6 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -56,6 +56,7 @@ #define CBRK_FLIGHTTERM_KEY 121212 #define CBRK_ENGINEFAIL_KEY 284953 #define CBRK_GPSFAIL_KEY 240024 +#define CBRK_USB_CHK_KEY 197848 #include diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index 003b73bb62..a3eaebbb33 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -147,3 +147,16 @@ PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); * @group Circuit Breaker */ PARAM_DEFINE_INT32(CBRK_BUZZER, 0); + +/** + * Circuit breaker for USB link check + * + * Setting this parameter to 197848 will disable the USB connected + * checks in the commander. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 197848 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_USB_CHK, 0); diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 0f5d96f2b8..3456fafc68 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -74,7 +74,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) param_t map_parm = param_find(rc_map_mandatory[j]); if (map_parm == PARAM_INVALID) { - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -87,7 +87,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) param_get(map_parm, &mapping); if (mapping > RC_INPUT_MAX_CHANNELS) { - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -95,7 +95,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) } if (mapping == 0) { - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -148,7 +148,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (param_min < RC_INPUT_LOWEST_MIN_US) { count++; - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -157,7 +157,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (param_max > RC_INPUT_HIGHEST_MAX_US) { count++; - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -166,7 +166,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (param_trim < param_min) { count++; - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -175,7 +175,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (param_trim > param_max) { count++; - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -183,7 +183,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) /* assert deadzone is sane */ if (param_dz > RC_INPUT_MAX_DEADZONE_US) { - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -200,9 +200,11 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (channels_failed) { sleep(2); - if (report_fail) mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", total_fail_count, (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); + } usleep(100000); } From d3365787af494e41144c138b3238f9e73646b066 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:03:46 +0100 Subject: [PATCH 036/136] sdlog2: Be less verbose --- src/modules/sdlog2/sdlog2.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6b98e64835..f15ec8969a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -425,7 +425,6 @@ int create_log_dir() mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); if (mkdir_ret == 0) { - warnx("log dir created: %s", log_dir); break; } else if (errno != EEXIST) { From 578b2500696ae2dfd97113bd68f90bbe47246249 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:04:52 +0100 Subject: [PATCH 037/136] Add RC check output --- src/modules/systemlib/rc_check.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 3456fafc68..622a3a0603 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -202,8 +202,8 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", - total_fail_count, - (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); + total_fail_count, + (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); } usleep(100000); From 0f489a194fe066848fe92add6cdcdeeca143d0d3 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Thu, 12 Nov 2015 12:18:19 +0530 Subject: [PATCH 038/136] attitude_q : more verbose failsafe output --- .../ecl/validation/data_validator_group.cpp | 2 +- .../attitude_estimator_q_main.cpp | 23 +++++++++++++++---- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp index ac4bcc8b9b..e82e476416 100644 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -199,7 +199,7 @@ void DataValidatorGroup::print() { /* print the group's state */ - ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)", + ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)", _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", _toggle_count); diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 1531e32778..b62c3b6cc5 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -369,12 +369,25 @@ void AttitudeEstimatorQ::task_main() _data_good = true; - if (!_failsafe && (_voter_gyro.failover_count() > 0 || - _voter_accel.failover_count() > 0 || - _voter_mag.failover_count() > 0)) { + if (!_failsafe) { + if (_voter_gyro.failover_count() > 0) { + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "Gyro failure!"); + } - _failsafe = true; - mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + if (_voter_accel.failover_count() > 0) { + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "Accel failure!"); + } + + if (_voter_mag.failover_count() > 0) { + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "Mag failure!"); + } + + if (_failsafe) { + mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + } } if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold || From 2d776aca14fdd2bcb97b80cee4e22dae50f90d5e Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Thu, 12 Nov 2015 14:50:49 +0530 Subject: [PATCH 039/136] attitude_q : ignore blank fields --- .../attitude_estimator_q_main.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index b62c3b6cc5..bbdf3ccc85 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -347,11 +347,16 @@ void AttitudeEstimatorQ::task_main() _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]); } - - _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], - sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); - _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], + /* ignore empty fields */ + if (sensors.accelerometer_timestamp[i] > 0) { + _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], + sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); + } + /* ignore empty fields */ + if (sensors.magnetometer_timestamp[i] > 0) { + _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); + } } int best_gyro, best_accel, best_mag; From 0d7cd22ae7f67452ed1fb1f9ed5c89a64fc1ee37 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Sat, 14 Nov 2015 11:58:51 +0530 Subject: [PATCH 040/136] data validator : verbose error state reporting --- src/lib/ecl/validation/data_validator.cpp | 45 +++++++++-------- src/lib/ecl/validation/data_validator.h | 24 +++++++++- .../ecl/validation/data_validator_group.cpp | 48 +++++++++++++++++-- src/lib/ecl/validation/data_validator_group.h | 14 ++++++ 4 files changed, 108 insertions(+), 23 deletions(-) diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp index 737d9ea1a3..299811ea43 100644 --- a/src/lib/ecl/validation/data_validator.cpp +++ b/src/lib/ecl/validation/data_validator.cpp @@ -43,6 +43,7 @@ #include DataValidator::DataValidator(DataValidator *prev_sibling) : + _error_mask(ERROR_FLAG_NO_ERROR), _time_last(0), _timeout_interval(20000), _event_count(0), @@ -111,39 +112,45 @@ DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, in float DataValidator::confidence(uint64_t timestamp) { + float ret = 1.0f; + /* check if we have any data */ if (_time_last == 0) { - return 0.0f; + _error_mask |= ERROR_FLAG_NO_DATA; + ret = 0.0f; } - - /* check error count limit */ - if (_error_count > NORETURN_ERRCOUNT) { - return 0.0f; + + /* timed out - that's it */ + if (timestamp - _time_last > _timeout_interval) { + _error_mask |= ERROR_FLAG_TIMEOUT; + ret = 0.0f; } /* we got the exact same sensor value N times in a row */ if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) { - return 0.0f; + _error_mask |= ERROR_FLAG_STALE_DATA; + ret = 0.0f; } - - /* timed out - that's it */ - if (timestamp - _time_last > _timeout_interval) { - return 0.0f; + + /* check error count limit */ + if (_error_count > NORETURN_ERRCOUNT) { + _error_mask |= ERROR_FLAG_HIGH_ERRCOUNT; + ret = 0.0f; } /* cap error density counter at window size */ if (_error_density > ERROR_DENSITY_WINDOW) { + _error_mask |= ERROR_FLAG_HIGH_ERRDENSITY; _error_density = ERROR_DENSITY_WINDOW; } - - /* return local error density for last N measurements */ - return 1.0f - (_error_density / ERROR_DENSITY_WINDOW); -} - -int -DataValidator::priority() -{ - return _priority; + + /* no critical errors */ + if(ret > 0.0f) { + /* return local error density for last N measurements */ + ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW); + } + + return ret; } void diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h index dde9cb51aa..31f6d9d989 100644 --- a/src/lib/ecl/validation/data_validator.h +++ b/src/lib/ecl/validation/data_validator.h @@ -91,7 +91,18 @@ public: * Get the priority of this validator * @return the stored priority */ - int priority(); + int priority() { return (_priority); } + + /** + * Get the error state of this validator + * @return the bitmask with the error status + */ + uint32_t state() { return (_error_mask); } + + /** + * Reset the error state of this validator + */ + void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; } /** * Get the RMS values of this validator @@ -111,9 +122,20 @@ public: * @param timeout_interval_us The timeout interval in microseconds */ void set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; } + + /** + * Data validator error states + */ + static constexpr uint32_t ERROR_FLAG_NO_ERROR = (0x00000000U); + static constexpr uint32_t ERROR_FLAG_NO_DATA = (0x00000001U); + static constexpr uint32_t ERROR_FLAG_STALE_DATA = (0x00000001U << 1); + static constexpr uint32_t ERROR_FLAG_TIMEOUT = (0x00000001U << 2); + static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT = (0x00000001U << 3); + static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY = (0x00000001U << 4); private: static const unsigned _dimensions = 3; + uint32_t _error_mask; /**< sensor error state */ uint64_t _time_last; /**< last timestamp */ uint64_t _timeout_interval; /**< interval in which the datastream times out in us */ uint64_t _event_count; /**< total data counter */ diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp index e82e476416..1a22672181 100644 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -138,11 +138,13 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index) bool true_failsafe = true; - /* check wether the switch was a failsafe or preferring a higher priority sensor */ + /* check whether the switch was a failsafe or preferring a higher priority sensor */ if (pre_check_prio != -1 && pre_check_prio < max_priority && fabsf(pre_check_confidence - max_confidence) < 0.1f) { /* this is not a failover */ true_failsafe = false; + /* reset error flags, this is likely a hotplug sensor coming online late */ + best->reset_state(); } /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ @@ -203,13 +205,21 @@ DataValidatorGroup::print() _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", _toggle_count); - DataValidator *next = _first; unsigned i = 0; while (next != nullptr) { if (next->used()) { - ECL_INFO("sensor #%u, prio: %d", i, next->priority()); + uint32_t flags = next->state(); + + ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " NO_DATA" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE_DATA" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " DATA_TIMEOUT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " HIGH_ERRCOUNT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " HIGH_ERRDENSITY" : ""), + ((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : "")); + next->print(); } next = next->sibling(); @@ -222,3 +232,35 @@ DataValidatorGroup::failover_count() { return _toggle_count; } + +int +DataValidatorGroup::failover_index() +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == _prev_best)) { + return i; + } + next = next->sibling(); + i++; + } + return -1; +} + +uint32_t +DataValidatorGroup::failover_state() +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == _prev_best)) { + return next->state(); + } + next = next->sibling(); + i++; + } + return DataValidator::ERROR_FLAG_NO_ERROR; +} diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h index 3756be2638..855c3ed4ae 100644 --- a/src/lib/ecl/validation/data_validator_group.h +++ b/src/lib/ecl/validation/data_validator_group.h @@ -80,6 +80,20 @@ public: * @return the number of failovers */ unsigned failover_count(); + + /** + * Get the index of the failed sensor in the group + * + * @return index of the failed sensor + */ + int failover_index(); + + /** + * Get the error state of the failed sensor in the group + * + * @return bitmask with erro states of the failed sensor + */ + uint32_t failover_state(); /** * Print the validator value From 5a1f7ca95a29f53eaad39f774b0f9c2b66784179 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Sat, 14 Nov 2015 11:58:34 +0530 Subject: [PATCH 041/136] attitude_q : verbose failure reporting --- .../ecl/validation/data_validator_group.cpp | 4 +-- .../attitude_estimator_q_main.cpp | 35 ++++++++++++++++--- 2 files changed, 32 insertions(+), 7 deletions(-) diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp index 1a22672181..8bf060cf65 100644 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -240,7 +240,7 @@ DataValidatorGroup::failover_index() unsigned i = 0; while (next != nullptr) { - if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == _prev_best)) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { return i; } next = next->sibling(); @@ -256,7 +256,7 @@ DataValidatorGroup::failover_state() unsigned i = 0; while (next != nullptr) { - if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == _prev_best)) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { return next->state(); } next = next->sibling(); diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index bbdf3ccc85..4b4fba54c8 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -347,15 +347,17 @@ void AttitudeEstimatorQ::task_main() _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]); } + /* ignore empty fields */ if (sensors.accelerometer_timestamp[i] > 0) { _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], - sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); + sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); } + /* ignore empty fields */ if (sensors.magnetometer_timestamp[i] > 0) { _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], - sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); + sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); } } @@ -375,19 +377,42 @@ void AttitudeEstimatorQ::task_main() _data_good = true; if (!_failsafe) { + uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR; + if (_voter_gyro.failover_count() > 0) { _failsafe = true; - mavlink_and_console_log_emergency(_mavlink_fd, "Gyro failure!"); + flags = _voter_gyro.failover_state(); + mavlink_and_console_log_emergency(_mavlink_fd, "Gyro #%i failure :%s%s%s%s%s!", + _voter_gyro.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); } if (_voter_accel.failover_count() > 0) { _failsafe = true; - mavlink_and_console_log_emergency(_mavlink_fd, "Accel failure!"); + flags = _voter_accel.failover_state(); + mavlink_and_console_log_emergency(_mavlink_fd, "Accel #%i failure :%s%s%s%s%s!", + _voter_accel.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); } if (_voter_mag.failover_count() > 0) { _failsafe = true; - mavlink_and_console_log_emergency(_mavlink_fd, "Mag failure!"); + flags = _voter_mag.failover_state(); + mavlink_and_console_log_emergency(_mavlink_fd, "Mag #%i failure :%s%s%s%s%s!", + _voter_mag.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); } if (_failsafe) { From 82b3ea2501bfaa8c605de4bd0202bf9565ef84e4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 17 Nov 2015 02:00:42 +0300 Subject: [PATCH 042/136] Removed an excessive template disambiguator in the virtual CAN driver class (fixes 3190) --- src/modules/uavcan/uavcan_virtual_can_driver.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp index 956caf11ee..d395f2b76c 100644 --- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -494,9 +494,8 @@ public: const unsigned quota_per_queue = virtual_iface_block_allocation_quota; // 2x overcommit for (unsigned i = 0; i < num_ifaces_; i++) { - ifaces_[i].template - construct - (allocator, clock_, driver_mutex_, quota_per_queue); + ifaces_[i].construct(allocator, clock_, driver_mutex_, quota_per_queue); } } From 2b50297c2e4b0dd93eb7a883704aa0e56a672db3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 17 Nov 2015 09:14:40 +0100 Subject: [PATCH 043/136] vtol: allow switch to fw mode without airspeed reading to facilitate bench testing --- src/modules/vtol_att_control/standard.cpp | 2 +- src/modules/vtol_att_control/tiltrotor.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index c87ca630ac..05c1a525b1 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -154,7 +154,7 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode - if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans) { + if (_airspeed->true_airspeed_m_s >= _params_standard.airspeed_trans || !_armed->armed) { _vtol_schedule.flight_mode = FW_MODE; // we can turn off the multirotor motors now _flag_enable_mc_motors = false; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 9e170104c4..270aa065a9 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -205,7 +205,8 @@ void Tiltrotor::update_vtol_state() case TRANSITION_FRONT_P1: // check if we have reached airspeed to switch to fw mode - if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) { + // also allow switch if we are not armed for the sake of bench testing + if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans || !_armed->armed) { _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; _vtol_schedule.transition_start = hrt_absolute_time(); } From b90e8a71f03764ec6dc9a7d930535d6a67015520 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Nov 2015 09:28:18 +0100 Subject: [PATCH 044/136] FMU: Fix sensor reset command, by Dmitry Prokhorov --- src/drivers/px4fmu/fmu.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4d82ef9e08..64d9dced4a 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1469,6 +1469,10 @@ PX4FMU::sensor_reset(int ms) stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + stm32_configgpio(GPIO_SPI1_SCK); + stm32_configgpio(GPIO_SPI1_MISO); + stm32_configgpio(GPIO_SPI1_MOSI); + // // XXX bring up the EXTI pins again // stm32_configgpio(GPIO_GYRO_DRDY); // stm32_configgpio(GPIO_MAG_DRDY); From 907848452f412db829311fecaa4a635854868ddc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Nov 2015 09:30:18 +0100 Subject: [PATCH 045/136] Commander: Be less verbose during normal flight ops --- 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 7a65e69e30..8b86b9d7fc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1699,10 +1699,10 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.condition_landed) { - mavlink_log_critical(mavlink_fd, "LANDING DETECTED"); + mavlink_and_console_log_info(mavlink_fd, "LANDING DETECTED"); } else { - mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED"); + mavlink_and_console_log_info(mavlink_fd, "TAKEOFF DETECTED"); } } From 81ff8d8060e2584c3b61b952fb8bad3770d126d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Nov 2015 09:31:41 +0100 Subject: [PATCH 046/136] Navigator: Initialize home position at boot time --- src/modules/navigator/navigator.h | 6 ++---- src/modules/navigator/navigator_main.cpp | 13 ++++--------- 2 files changed, 6 insertions(+), 13 deletions(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 1ddedddff4..6ecf73a606 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -135,7 +135,7 @@ public: struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; } struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } struct home_position_s* get_home_position() { return &_home_pos; } - bool home_position_valid() { return _home_position_set; } + bool home_position_valid() { return (_home_pos.timestamp > 0); } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } struct geofence_result_s* get_geofence_result() { return &_geofence_result; } @@ -205,8 +205,6 @@ private: geofence_result_s _geofence_result; vehicle_attitude_setpoint_s _att_sp; - bool _home_position_set; - bool _mission_item_valid; /**< flags if the current mission item is valid */ int _mission_instance_count; /**< instance count for the current mission */ @@ -257,7 +255,7 @@ private: /** * Retrieve home position */ - void home_position_update(); + void home_position_update(bool force=false); /** * Retreive navigation capabilities diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 92f3e2fecb..9131fb16f0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -126,7 +126,6 @@ Navigator::Navigator() : _pos_sp_triplet{}, _mission_result{}, _att_sp{}, - _home_position_set(false), _mission_item_valid(false), _mission_instance_count(0), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -206,17 +205,13 @@ Navigator::sensor_combined_update() } void -Navigator::home_position_update() +Navigator::home_position_update(bool force) { bool updated = false; orb_check(_home_pos_sub, &updated); - if (updated) { + if (updated || force) { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); - - if (_home_pos.timestamp > 0) { - _home_position_set = true; - } } } @@ -298,7 +293,7 @@ Navigator::task_main() global_position_update(); gps_position_update(); sensor_combined_update(); - home_position_update(); + home_position_update(true); navigation_capabilities_update(); params_update(); @@ -408,7 +403,7 @@ Navigator::task_main() if (have_geofence_position_data && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { - bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, _home_position_set); + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; From b6a959ad3e5b2cab864d6a19177916c5c03dcc1f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Nov 2015 12:23:41 +0100 Subject: [PATCH 047/136] Navigator: Code style --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9131fb16f0..e44b8d0be5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -484,7 +484,7 @@ Navigator::task_main() } /* iterate through navigation modes and set active/inactive for each */ - for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { + for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } From e070f671ae2b61816934628a114e2e63486c6e4f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Nov 2015 12:24:51 +0100 Subject: [PATCH 048/136] Preflight: better reporting --- src/modules/commander/PreflightCheck.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 3423ad8340..8093ef56ba 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -498,7 +498,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro if (checkRC) { if (rc_calibration_check(mavlink_fd, reportFailures) != OK) { if (reportFailures) { - mavlink_log_critical(mavlink_fd, "RC calibration check failed"); + mavlink_and_console_log_critical(mavlink_fd, "RC calibration check failed"); } failed = true; } From 4947e89f52c90929cdf15143b7b5fed53b154582 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Nov 2015 12:25:05 +0100 Subject: [PATCH 049/136] VTOL attitude control: Fix return value on start --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index e15e950fd1..0248a20727 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -690,6 +690,7 @@ int vtol_att_control_main(int argc, char *argv[]) return 1; } + return 0; } if (!strcmp(argv[1], "stop")) { From 0509a5a9ea64568faee95bd3763d990a12906611 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Nov 2015 12:35:24 +0100 Subject: [PATCH 050/136] Enforce airspeed check for VTOLs --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 981644c342..1956a805c3 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -753,7 +753,7 @@ int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ - if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) { + if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol) { checkAirspeed = true; } From 092a51426f3cde2e5a04e204a3f3adef82cefeeb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 17 Nov 2015 12:59:45 +0100 Subject: [PATCH 051/136] Build fix and airspeed console cal --- .../commander/airspeed_calibration.cpp | 40 +++++++++---------- src/modules/commander/commander.cpp | 2 + .../commander/state_machine_helper.cpp | 2 +- 3 files changed, 23 insertions(+), 21 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index eb2bcd650e..4139a949e0 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -68,7 +68,7 @@ static const char *sensor_name = "dpress"; static void feedback_calibration_failed(int mavlink_fd) { sleep(5); - mavlink_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); } int do_airspeed_calibration(int mavlink_fd) @@ -78,7 +78,7 @@ int do_airspeed_calibration(int mavlink_fd) const unsigned maxcount = 2400; /* give directions */ - mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); const unsigned calibration_count = (maxcount * 2) / 3; @@ -101,7 +101,7 @@ int do_airspeed_calibration(int mavlink_fd) paramreset_successful = true; } else { - mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset zero failed"); } px4_close(fd); @@ -115,18 +115,18 @@ int do_airspeed_calibration(int mavlink_fd) float analog_scaling = 0.0f; param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { - mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd"); goto error_return; } /* set scaling offset parameter */ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1); goto error_return; } } - mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind"); usleep(500 * 1000); while (calibration_counter < calibration_count) { @@ -149,7 +149,7 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); } } else if (poll_ret == 0) { @@ -167,14 +167,14 @@ int do_airspeed_calibration(int mavlink_fd) airscale.offset_pa = diff_pres_offset; if (fd_scale > 0) { if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { - mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset update failed"); } px4_close(fd_scale); } if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1); goto error_return; } @@ -183,7 +183,7 @@ int do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); - mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); goto error_return; } @@ -192,12 +192,12 @@ int do_airspeed_calibration(int mavlink_fd) goto error_return; } - mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset); + mavlink_and_console_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); - mavlink_log_critical(mavlink_fd, "[cal] Create airflow now"); + mavlink_and_console_log_critical(mavlink_fd, "[cal] Create airflow now"); calibration_counter = 0; @@ -222,7 +222,7 @@ int do_airspeed_calibration(int mavlink_fd) if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 500 == 0) { - mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", + mavlink_and_console_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_raw_pa); } continue; @@ -230,26 +230,26 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)", + mavlink_and_console_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); - mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!"); + mavlink_and_console_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!"); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ diff_pres_offset = 0.0f; if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1); goto error_return; } /* save */ - mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0); (void)param_save_default(); feedback_calibration_failed(mavlink_fd); goto error_return; } else { - mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)", + mavlink_and_console_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); break; } @@ -266,9 +266,9 @@ int do_airspeed_calibration(int mavlink_fd) goto error_return; } - mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); - mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); tune_neutral(true); normal_return: diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8b86b9d7fc..83e79b77fc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -362,6 +362,8 @@ int commander_main(int argc, char *argv[]) calib_ret = do_level_calibration(mavlink_fd); } else if (!strcmp(argv[2], "esc")) { calib_ret = do_esc_calibration(mavlink_fd, &armed); + } else if (!strcmp(argv[2], "airspeed")) { + calib_ret = do_airspeed_calibration(mavlink_fd); } else { warnx("argument %s unsupported.", argv[2]); } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1956a805c3..bcdb5d3f9d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -753,7 +753,7 @@ int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ - if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol) { + if (!status->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) { checkAirspeed = true; } From 2cc9f03c93223be4e3e155b806c59d2ab1df6b59 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 17 Nov 2015 16:30:27 +0100 Subject: [PATCH 052/136] SITL: fixed control callback --- src/drivers/pwm_out_sim/pwm_out_sim.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index 993958a050..373e7505b6 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -548,7 +548,7 @@ PWMSim::control_callback(uintptr_t handle, const actuator_controls_s *controls = (actuator_controls_s *)handle; if (_armed) { - input = controls->control[control_index]; + input = controls[control_group].control[control_index]; } else { /* clamp actuator to zero if not armed */ From dfdf7dce4d6dac46016fdd849968e6eb8d08a461 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 17 Nov 2015 16:38:11 +0100 Subject: [PATCH 053/136] new mode for fake gps --- ROMFS/px4fmu_common/init.d/rcS | 4 ++++ src/modules/mavlink/mavlink_main.cpp | 9 ++++++++- src/modules/mavlink/mavlink_main.h | 1 + 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c4b25987be..411eb0d892 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -564,6 +564,10 @@ then then mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000 fi + if param compare SYS_COMPANION 257600 + then + mavlink start -d /dev/ttyS2 -b 57600 -m magic -r 5000 -x + fi # Sensors on the PWM interface bank # clear pins 5 and 6 if param compare SENS_EN_LL40LS 1 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3a2f1d583b..1b04eea1a0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1523,6 +1523,9 @@ Mavlink::task_main(int argc, char *argv[]) } else if (strcmp(myoptarg, "osd") == 0) { _mode = MAVLINK_MODE_OSD; + } else if (strcmp(myoptarg, "magic") == 0) { + _mode = MAVLINK_MODE_MAGIC; + } else if (strcmp(myoptarg, "config") == 0) { _mode = MAVLINK_MODE_CONFIG; } @@ -1709,7 +1712,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); - /* camera trigger is rate limited at the source, do not limit here */ + //camera trigger is rate limited at the source, do not limit here configure_stream("CAMERA_TRIGGER", 500.0f); configure_stream("EXTENDED_SYS_STATE", 2.0f); break; @@ -1729,6 +1732,10 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("EXTENDED_SYS_STATE", 1.0f); break; + case MAVLINK_MODE_MAGIC: + //stream nothing + break; + case MAVLINK_MODE_CONFIG: // Enable a number of interesting streams we want via USB configure_stream("SYS_STATUS", 1.0f); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 95e0fdb91a..37c66b5963 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -149,6 +149,7 @@ public: MAVLINK_MODE_CUSTOM, MAVLINK_MODE_ONBOARD, MAVLINK_MODE_OSD, + MAVLINK_MODE_MAGIC, MAVLINK_MODE_CONFIG }; From b0515ef07dabd5e26e84332ba8501d40a1c7f15b Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 17 Nov 2015 16:47:32 +0100 Subject: [PATCH 054/136] added a disabling flag for mocap when using fake gps --- .../position_estimator_inav/CMakeLists.txt | 2 +- .../position_estimator_inav_main.c | 23 +++++++++++++------ .../position_estimator_inav_params.c | 17 ++++++++++++-- .../position_estimator_inav_params.h | 2 ++ 4 files changed, 34 insertions(+), 10 deletions(-) diff --git a/src/modules/position_estimator_inav/CMakeLists.txt b/src/modules/position_estimator_inav/CMakeLists.txt index 2d5315b92b..60429f5d22 100644 --- a/src/modules/position_estimator_inav/CMakeLists.txt +++ b/src/modules/position_estimator_inav/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ if(${OS} STREQUAL "nuttx") - list(APPEND MODULE_CFLAGS -Wframe-larger-than=3890) + list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000) endif() px4_add_module( MODULE modules__position_estimator_inav diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 4696d4712b..32cb74ec8b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -586,8 +586,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; - //this flag is not working --> - flow_accurate = true; //already checked if flow_q > 0.3 /*calculate offset of flow-gyro using already calibrated gyro from autopilot*/ @@ -817,7 +815,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps_valid) { if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) { gps_valid = false; - ref_inited = false; //if gps gets lost it has to init again to avoid jumps mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } @@ -841,7 +838,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; - + /* set position estimate to (0, 0, 0), use GPS velocity for XY */ x_est[0] = 0.0f; x_est[1] = gps.vel_n_m_s; @@ -866,7 +863,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float gps_proj[2]; map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); - /* reset position estimate when GPS becomes good */ + /* reset position estimate and ref when GPS becomes good */ if (reset_est) { x_est[0] = gps_proj[0]; x_est[1] = gps.vel_n_m_s; @@ -880,6 +877,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) est_i += EST_BUF_SIZE; } + //mavlink_log_info(mavlink_fd, "est_i = %d\n", (int)est_i); + /* calculate correction for position */ corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0]; corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0]; @@ -954,9 +953,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) t_prev = t; /* increase EPH/EPV on each step */ + if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0 + eph = 0.001; + } if (eph < max_eph_epv) { eph *= 1.0f + dt; } + if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0 + epv = 0.001; + } if (epv < max_eph_epv) { epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift) } @@ -969,11 +974,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W; /* use MOCAP if it's valid and has a valid weight parameter */ bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W; + if(params.disable_mocap) { //disable mocap if fake gps is used + use_mocap = false; + } /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); + bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; - + //mavlink_log_info(mavlink_fd, "eph = %2.6f\t gps eph = %2.4f\n", (double)eph, (double)gps.eph); bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout); if (dist_bottom_valid) { @@ -1167,7 +1176,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); - //mavlink_log_info(mavlink_fd, "w_flow = %2.4f\t w_xy_flow = %2.4f\n", (double)w_flow, (double)params.w_xy_flow); } if (use_gps_xy) { @@ -1265,6 +1273,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) buf_ptr = 0; } + /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index be8a136c4d..f303abaf6d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -158,10 +158,10 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f); * Weight (cutoff frequency) for optical flow (velocity) measurements. * * @min 0.0 - * @max 30.0 + * @max 10.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 10.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 9.0f); /** * XY axis weight for resetting velocity @@ -313,6 +313,17 @@ PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f); */ PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f); +/** + * Disable mocap (set 0 if using fake gps) + * + * Disable mocap + * + * @min 0 + * @max 1 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_DISAB_MOCAP, 0); + /** * Disable vision input * @@ -364,6 +375,7 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h) h->delay_gps = param_find("INAV_DELAY_GPS"); h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X"); h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y"); + h->disable_mocap = param_find("INAV_DISAB_MOCAP"); return 0; } @@ -394,6 +406,7 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h param_get(h->delay_gps, &(p->delay_gps)); param_get(h->flow_module_offset_x, &(p->flow_module_offset_x)); param_get(h->flow_module_offset_y, &(p->flow_module_offset_y)); + param_get(h->disable_mocap, &(p->disable_mocap)); return 0; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 43601568f8..07b78104bf 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -67,6 +67,7 @@ struct position_estimator_inav_params { float delay_gps; float flow_module_offset_x; float flow_module_offset_y; + int32_t disable_mocap; }; struct position_estimator_inav_param_handles { @@ -95,6 +96,7 @@ struct position_estimator_inav_param_handles { param_t delay_gps; param_t flow_module_offset_x; param_t flow_module_offset_y; + param_t disable_mocap; }; #define CBRK_NO_VISION_KEY 328754 From 8044c5aaf660f174e878bce0116d599165ad9558 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 17 Nov 2015 16:59:44 +0100 Subject: [PATCH 055/136] remove unnecessary lines --- .../position_estimator_inav/position_estimator_inav_main.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 32cb74ec8b..d8f29f1082 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -863,7 +863,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float gps_proj[2]; map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); - /* reset position estimate and ref when GPS becomes good */ + /* reset position estimate when GPS becomes good */ if (reset_est) { x_est[0] = gps_proj[0]; x_est[1] = gps.vel_n_m_s; @@ -877,8 +877,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) est_i += EST_BUF_SIZE; } - //mavlink_log_info(mavlink_fd, "est_i = %d\n", (int)est_i); - /* calculate correction for position */ corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0]; corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0]; @@ -982,7 +980,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; - //mavlink_log_info(mavlink_fd, "eph = %2.6f\t gps eph = %2.4f\n", (double)eph, (double)gps.eph); + bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout); if (dist_bottom_valid) { From 20ec727d9fdbec46be75d21a838fef9e1c20cb88 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 10 Sep 2015 14:22:00 +0200 Subject: [PATCH 056/136] added option for direct yaw control with rudder for fixed wing --- msg/vehicle_attitude_setpoint.msg | 2 ++ 1 file changed, 2 insertions(+) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 7bbb670b31..3bbb5efa7b 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -21,3 +21,5 @@ float32 thrust # Thrust in Newton the power system should generate bool roll_reset_integral # Reset roll integral part (navigation logic change) bool pitch_reset_integral # Reset pitch integral part (navigation logic change) bool yaw_reset_integral # Reset yaw integral part (navigation logic change) + +bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) From ee4249f30f72b11552d84fe98fe5c1d46a3624c3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 10 Sep 2015 14:22:39 +0200 Subject: [PATCH 057/136] extended ecl yaw controller for yaw tracking with rudder --- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 34 +++++++++++++++++++ src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 10 ++++-- 2 files changed, 41 insertions(+), 3 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 4849bdad79..240a426bd0 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -67,6 +67,9 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data case COORD_METHOD_CLOSEACC: return control_attitude_impl_accclosedloop(ctl_data); + case COORD_METHOD_HEADING: + return control_heading(ctl_data); + default: static hrt_abstime last_print = 0; @@ -238,3 +241,34 @@ float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_Co /* dont set a rate setpoint */ return 0.0f; } + +float ECL_YawController::control_heading(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && + PX4_ISFINITE(ctl_data.yaw) && + PX4_ISFINITE(ctl_data.airspeed))) { + perf_count(_nonfinite_input_perf); + warnx("not controlling yaw"); + return _rate_setpoint; + } + + /* Calculate the error */ + float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; + + /* Apply P controller: rate setpoint from current error and time constant */ + _rate_setpoint = yaw_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f) { + if (_rate_setpoint > 0.0f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + + } else { + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + } + + return _rate_setpoint; +} diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 81d8210549..e3e529b30f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -76,14 +76,16 @@ public: _coordinated_method = coordinated_method; } -protected: - float _coordinated_min_speed; - enum { COORD_METHOD_OPEN = 0, COORD_METHOD_CLOSEACC = 1, + COORD_METHOD_HEADING = 2 }; +protected: + float _coordinated_min_speed; + float _max_rate; + int32_t _coordinated_method; float control_bodyrate_impl(const struct ECL_ControlData &ctl_data); @@ -92,6 +94,8 @@ protected: float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data); + float control_heading(const struct ECL_ControlData &ctl_data); + }; #endif // ECL_YAW_CONTROLLER_H From f43d50fbc97a852dff084be29cf8580085f51cad Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 10 Sep 2015 14:35:40 +0200 Subject: [PATCH 058/136] implemented runway takeoff for fw --- .../fw_att_control/fw_att_control_main.cpp | 8 + .../fw_pos_control_l1_main.cpp | 202 ++++++++++++------ .../fw_pos_control_l1_params.c | 9 + 3 files changed, 150 insertions(+), 69 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 3ed5b3a5a3..081645c3ae 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -811,6 +811,7 @@ FixedwingAttitudeControl::task_main() float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; + float yaw_sp = 0.0f; float yaw_manual = 0.0f; float throttle_sp = 0.0f; @@ -824,6 +825,7 @@ FixedwingAttitudeControl::task_main() /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + yaw_sp = _att_sp.yaw_body; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ @@ -952,12 +954,18 @@ FixedwingAttitudeControl::task_main() control_input.acc_body_z = _accel.z; control_input.roll_setpoint = roll_sp; control_input.pitch_setpoint = pitch_sp; + control_input.yaw_setpoint = yaw_sp; control_input.airspeed_min = _parameters.airspeed_min; control_input.airspeed_max = _parameters.airspeed_max; control_input.airspeed = airspeed; control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; + if (_att_sp.fw_control_yaw == true) { + // this method controls heading directly with rudder. Used for auto takeoff on runway + _yaw_ctrl.set_coordinated_method(ECL_YawController::COORD_METHOD_HEADING); + } + /* Run attitude controllers */ if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { _roll_ctrl.control_attitude(control_input); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f21bfc5f2f..dbf26912ca 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -195,6 +195,8 @@ private: bool land_useterrain; bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ + bool _takeoff_initialized; + hrt_abstime _takeoff_runway_start; /**< time at which we start takeoff on runway */ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ /* takeoff/launch states */ @@ -278,6 +280,8 @@ private: float land_flare_pitch_max_deg; int land_use_terrain_estimate; + int do_runway_takeoff; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -326,6 +330,8 @@ private: param_t land_flare_pitch_max_deg; param_t land_use_terrain_estimate; + param_t do_runway_takeoff; + } _parameter_handles; /**< handles for interesting parameters */ @@ -520,6 +526,8 @@ FixedwingPositionControl::FixedwingPositionControl() : land_onslope(false), land_useterrain(false), _was_in_air(false), + _takeoff_initialized(false), + _takeoff_runway_start(0), _time_went_in_air(0), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), @@ -563,6 +571,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); + _parameter_handles.do_runway_takeoff = param_find("FW_RUNWAY_TKOFF"); + _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -666,6 +676,8 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); + param_get(_parameter_handles.do_runway_takeoff, &(_parameters.do_runway_takeoff)); + _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -1054,6 +1066,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; + _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder + float eas2tas = 1.0f; // XXX calculate actual number based on current measurements /* filter speed and altitude for controller */ @@ -1338,94 +1352,144 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - /* Perform launch detection */ - if (launchDetector.launchDetectionEnabled() && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { - /* Inform user that launchdetection is running */ - static hrt_abstime last_sent = 0; - if(hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_critical(_mavlink_fd, "Launchdetection running"); - last_sent = hrt_absolute_time(); + if (_parameters.do_runway_takeoff == true) { + if (!_takeoff_initialized) { + _hdg_hold_yaw = _att.yaw; + _takeoff_runway_start = hrt_absolute_time(); + _takeoff_initialized = true; } - /* Detect launch */ - launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); - - /* update our copy of the launch detection state */ - launch_detection_state = launchDetector.getLaunchDetected(); - } else { - /* no takeoff detection --> fly */ - launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; - } - - /* Set control values depending on the detection state */ - if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { - /* Launch has been detected, hence we have to control the plane. */ - - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - - /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use - * full throttle, otherwise we use the preTakeOff Throttle */ - float takeoff_throttle = launch_detection_state != - LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? - launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; - - /* select maximum pitch: the launchdetector may impose another limit for the pitch - * depending on the state of the launch */ - float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); - float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); - - /* apply minimum pitch and limit roll if target altitude is not within climbout_diff - * meters */ - if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { + if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) { + _att_sp.pitch_body = math::radians(10.0f); // XXX consider this magic value + // ramp up throttle to max throttle + _att_sp.thrust = _att_sp.thrust < _parameters.throttle_max ? hrt_elapsed_time(&_takeoff_runway_start) / (2000000) : _parameters.throttle_max; + } else { + float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas, math::radians(_parameters.pitch_limit_min), takeoff_pitch_max_rad, - _parameters.throttle_min, takeoff_throttle, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, true, - math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::max(math::radians(_pos_sp_triplet.current.pitch_min), math::radians(10.0f)), _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_TAKEOFF, takeoff_pitch_max_deg != _parameters.pitch_limit_max); - /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), - math::radians(15.0f)); - - } else { - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(_parameters.airspeed_trim), - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - takeoff_throttle, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed); + _att_sp.pitch_body = _tecs.get_pitch_demand(); + _att_sp.thrust = math::min(_tecs.get_throttle_demand(), _parameters.throttle_max); } - } else { - /* Tell the attitude controller to stop integrating while we are waiting - * for the launch */ - _att_sp.roll_reset_integral = true; - _att_sp.pitch_reset_integral = true; - _att_sp.yaw_reset_integral = true; - /* Set default roll and pitch setpoints during detection phase */ - _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), - math::radians(10.0f)); + if (_global_pos.alt - _global_pos.terrain_alt > 4.0f) { // XXX investigate on this + // start to navigate to takeoff waypoint as soon as we are high enough in the air + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + } else { + // for takeoff we want heading control with rudder, roll and pitch stabilization to zero + // throttle should be ramped up to max + _att_sp.roll_body = 0.0f; + _att_sp.yaw_body = _hdg_hold_yaw; + _att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly + } + + + } else { + /* Perform launch detection */ + if (launchDetector.launchDetectionEnabled() && + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + /* Inform user that launchdetection is running */ + static hrt_abstime last_sent = 0; + if(hrt_absolute_time() - last_sent > 4e6) { + mavlink_log_critical(_mavlink_fd, "Launchdetection running"); + last_sent = hrt_absolute_time(); + } + + /* Detect launch */ + launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + + /* update our copy of the launch detection state */ + launch_detection_state = launchDetector.getLaunchDetected(); + } else { + /* no takeoff detection --> fly */ + launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; + } + + /* Set control values depending on the detection state */ + if (launch_detection_state != LAUNCHDETECTION_RES_NONE) { + /* Launch has been detected, hence we have to control the plane. */ + + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use + * full throttle, otherwise we use the preTakeOff Throttle */ + float takeoff_throttle = launch_detection_state != + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? + launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; + + /* select maximum pitch: the launchdetector may impose another limit for the pitch + * depending on the state of the launch */ + float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); + + /* apply minimum pitch and limit roll if target altitude is not within climbout_diff + * meters */ + if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { + /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(1.3f * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, takeoff_throttle, + _parameters.throttle_cruise, + true, + math::max(math::radians(_pos_sp_triplet.current.pitch_min), + math::radians(10.0f)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); + + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); + + } else { + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(_parameters.airspeed_trim), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + takeoff_throttle, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); + } + } else { + /* Tell the attitude controller to stop integrating while we are waiting + * for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; + + /* Set default roll and pitch setpoints during detection phase */ + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::radians(10.0f)); + } } } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 5870039c24..2865ec7ce6 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -426,6 +426,15 @@ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); */ PARAM_DEFINE_INT32(FW_LND_USETER, 0); +/** + * Enable or disable runway takeoff with landing gear + * + * 0: disabled, 1: enabled + * + * @group L1 Control + */ +PARAM_DEFINE_INT32(FW_RUNWAY_TKOFF, 0); + /** * Flare, minimum pitch * From 059e40f78028cdf210f3dff2a2e0f7fcc86b2e8a Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 12 Sep 2015 01:57:09 +0200 Subject: [PATCH 059/136] - fixed throttle ramp-up - added parameter to specify which heading to keep on runway - validate terrain alt before using it --- .../fw_pos_control_l1_main.cpp | 76 +++++++++++++++---- .../fw_pos_control_l1_params.c | 9 +++ 2 files changed, 70 insertions(+), 15 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index dbf26912ca..56da2f7c6e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -196,6 +196,7 @@ private: bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ bool _takeoff_initialized; + bool _takeoff_on_runway; /**< true as long as runway takeoff is in progress */ hrt_abstime _takeoff_runway_start; /**< time at which we start takeoff on runway */ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ @@ -281,6 +282,7 @@ private: int land_use_terrain_estimate; int do_runway_takeoff; + int runway_takeoff_heading; } _parameters; /**< local copies of interesting parameters */ @@ -331,6 +333,7 @@ private: param_t land_use_terrain_estimate; param_t do_runway_takeoff; + param_t runway_takeoff_heading; } _parameter_handles; /**< handles for interesting parameters */ @@ -397,6 +400,11 @@ private: */ float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); + /** + * Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available + */ + float get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos); + /** * Check if we are in a takeoff situation */ @@ -527,7 +535,8 @@ FixedwingPositionControl::FixedwingPositionControl() : land_useterrain(false), _was_in_air(false), _takeoff_initialized(false), - _takeoff_runway_start(0), + _takeoff_on_runway(false), + _takeoff_runway_start(0), _time_went_in_air(0), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), @@ -572,6 +581,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); _parameter_handles.do_runway_takeoff = param_find("FW_RUNWAY_TKOFF"); + _parameter_handles.runway_takeoff_heading = param_find("FW_TKOFF_HDG"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -677,6 +687,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); param_get(_parameter_handles.do_runway_takeoff, &(_parameters.do_runway_takeoff)); + param_get(_parameter_handles.runway_takeoff_heading, &(_parameters.runway_takeoff_heading)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -970,6 +981,15 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint } } +float FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos) +{ + if (PX4_ISFINITE(global_pos.terrain_alt) && global_pos.terrain_alt_valid) { + return global_pos.terrain_alt; + } + + return takeoff_alt; +} + bool FixedwingPositionControl::update_desired_altitude(float dt) { /* @@ -1357,19 +1377,23 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _hdg_hold_yaw = _att.yaw; _takeoff_runway_start = hrt_absolute_time(); _takeoff_initialized = true; + _takeoff_on_runway = true; + /* need this already before takeoff is detected + * doesn't matter if it gets reset when takeoff is detected eventually */ + _takeoff_ground_alt = _global_pos.alt; } - - if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) { - _att_sp.pitch_body = math::radians(10.0f); // XXX consider this magic value - // ramp up throttle to max throttle - _att_sp.thrust = _att_sp.thrust < _parameters.throttle_max ? hrt_elapsed_time(&_takeoff_runway_start) / (2000000) : _parameters.throttle_max; + if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) { // XXX takeoff: magic value + _takeoff_on_runway = true; + _att_sp.pitch_body = math::radians(10.0f); // XXX takeoff: magic value } else { - float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max); + // min airspeed reached, let tecs control it + _takeoff_on_runway = false; + float takeoff_pitch_max_deg = _parameters.pitch_limit_max; float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); - /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(1.3f * _parameters.airspeed_min), + calculate_target_airspeed(1.3f * _parameters.airspeed_min), // XXX takeoff: magic value eas2tas, math::radians(_parameters.pitch_limit_min), takeoff_pitch_max_rad, @@ -1384,20 +1408,34 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi takeoff_pitch_max_deg != _parameters.pitch_limit_max); _att_sp.pitch_body = _tecs.get_pitch_demand(); - _att_sp.thrust = math::min(_tecs.get_throttle_demand(), _parameters.throttle_max); } - if (_global_pos.alt - _global_pos.terrain_alt > 4.0f) { // XXX investigate on this + // update navigation + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); + //PX4_WARN("talt: %f", (double)terrain_alt); + + if (_global_pos.alt - terrain_alt > 4.0f) { // XXX takeoff: magic value + // XXX once in here we should maybe not fall out again (although it shouldn't matter when we're climbing) // start to navigate to takeoff waypoint as soon as we are high enough in the air - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); } else { // for takeoff we want heading control with rudder, roll and pitch stabilization to zero // throttle should be ramped up to max _att_sp.roll_body = 0.0f; - _att_sp.yaw_body = _hdg_hold_yaw; - _att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly + //_att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly + + if (_parameters.runway_takeoff_heading == 0) { + // fix heading in the direction the airframe points + _att_sp.yaw_body = _hdg_hold_yaw; + //PX4_WARN("using initial heading: %.4f", (double)_att_sp.yaw_body); + } else if (_parameters.runway_takeoff_heading == 1) { + // or head into the direction of the takeoff waypoint + // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) + _att_sp.yaw_body = _l1_control.nav_bearing(); // XXX check if this is really the actual bearing + //PX4_WARN("using nav heading: %.4f", (double)_att_sp.yaw_body); + } } @@ -1703,10 +1741,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = 0.0f; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && + !_parameters.do_runway_takeoff) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + _parameters.do_runway_takeoff && _takeoff_on_runway == true) { + /* Ramp-up thrust and stay at max */ + _att_sp.thrust = _att_sp.thrust < _parameters.throttle_max ? hrt_elapsed_time(&_takeoff_runway_start) / (float)2000000 : _parameters.throttle_max; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; @@ -1889,6 +1933,8 @@ void FixedwingPositionControl::reset_takeoff_state() { launch_detection_state = LAUNCHDETECTION_RES_NONE; launchDetector.reset(); + _takeoff_initialized = false; + _takeoff_on_runway = false; } void FixedwingPositionControl::reset_landing_state() diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 2865ec7ce6..29fbeed386 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -435,6 +435,15 @@ PARAM_DEFINE_INT32(FW_LND_USETER, 0); */ PARAM_DEFINE_INT32(FW_RUNWAY_TKOFF, 0); +/** + * Specifies which heading should be held during runnway takeoff. + * + * 0: airframe heading, 1: heading towards takeoff waypoint + * + * @group L1 Control + */ +PARAM_DEFINE_INT32(FW_TKOFF_HDG, 0); + /** * Flare, minimum pitch * From ea884b34f0f13168fee6d36d0b61b0a027f6f8ef Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 12 Sep 2015 02:06:37 +0200 Subject: [PATCH 060/136] use and reset the new yaw control method --- src/modules/fw_att_control/fw_att_control_main.cpp | 2 ++ src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 081645c3ae..de8ee9173e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -964,6 +964,8 @@ FixedwingAttitudeControl::task_main() if (_att_sp.fw_control_yaw == true) { // this method controls heading directly with rudder. Used for auto takeoff on runway _yaw_ctrl.set_coordinated_method(ECL_YawController::COORD_METHOD_HEADING); + } else { + _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); } /* Run attitude controllers */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 56da2f7c6e..c8832d830a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1424,7 +1424,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // for takeoff we want heading control with rudder, roll and pitch stabilization to zero // throttle should be ramped up to max _att_sp.roll_body = 0.0f; - //_att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly + _att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly if (_parameters.runway_takeoff_heading == 0) { // fix heading in the direction the airframe points From 24179a0d936c0ba3b93f4fc438f4a9546c42a096 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 12 Sep 2015 10:29:14 +0200 Subject: [PATCH 061/136] consitent parameter naming for runway takeoff, added parameters for important values --- .../fw_pos_control_l1_main.cpp | 29 +++++++++---- .../fw_pos_control_l1_params.c | 41 ++++++++++++++++++- 2 files changed, 61 insertions(+), 9 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index c8832d830a..6771921b2f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -283,6 +283,9 @@ private: int do_runway_takeoff; int runway_takeoff_heading; + float runway_takeoff_nav_alt; + float runway_takeoff_throttle; + float runway_takeoff_pitch_sp; } _parameters; /**< local copies of interesting parameters */ @@ -334,6 +337,9 @@ private: param_t do_runway_takeoff; param_t runway_takeoff_heading; + param_t runway_takeoff_nav_alt; + param_t runway_takeoff_throttle; + param_t runway_takeoff_pitch_sp; } _parameter_handles; /**< handles for interesting parameters */ @@ -580,8 +586,11 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); - _parameter_handles.do_runway_takeoff = param_find("FW_RUNWAY_TKOFF"); - _parameter_handles.runway_takeoff_heading = param_find("FW_TKOFF_HDG"); + _parameter_handles.do_runway_takeoff = param_find("FW_RNW_TKOFF"); + _parameter_handles.runway_takeoff_heading = param_find("FW_RNW_HDG"); + _parameter_handles.runway_takeoff_nav_alt = param_find("FW_RNW_NAV_ALT"); + _parameter_handles.runway_takeoff_throttle = param_find("FW_RNW_MAX_THR"); + _parameter_handles.runway_takeoff_pitch_sp = param_find("FW_RNW_PSP"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -688,6 +697,9 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.do_runway_takeoff, &(_parameters.do_runway_takeoff)); param_get(_parameter_handles.runway_takeoff_heading, &(_parameters.runway_takeoff_heading)); + param_get(_parameter_handles.runway_takeoff_nav_alt, &(_parameters.runway_takeoff_nav_alt)); + param_get(_parameter_handles.runway_takeoff_throttle, &(_parameters.runway_takeoff_throttle)); + param_get(_parameter_handles.runway_takeoff_pitch_sp, &(_parameters.runway_takeoff_pitch_sp)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -1385,7 +1397,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) { // XXX takeoff: magic value _takeoff_on_runway = true; - _att_sp.pitch_body = math::radians(10.0f); // XXX takeoff: magic value + _att_sp.pitch_body = math::radians(_parameters.runway_takeoff_pitch_sp); } else { // min airspeed reached, let tecs control it _takeoff_on_runway = false; @@ -1397,11 +1409,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi eas2tas, math::radians(_parameters.pitch_limit_min), takeoff_pitch_max_rad, - _parameters.throttle_min, _parameters.throttle_max, + _parameters.throttle_min, + _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? _parameters.throttle_cruise, true, math::max(math::radians(_pos_sp_triplet.current.pitch_min), - math::radians(10.0f)), + math::radians(10.0f)), _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_TAKEOFF, @@ -1415,7 +1428,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); //PX4_WARN("talt: %f", (double)terrain_alt); - if (_global_pos.alt - terrain_alt > 4.0f) { // XXX takeoff: magic value + if (_global_pos.alt - terrain_alt > _parameters.runway_takeoff_nav_alt) { // XXX once in here we should maybe not fall out again (although it shouldn't matter when we're climbing) // start to navigate to takeoff waypoint as soon as we are high enough in the air _att_sp.roll_body = _l1_control.nav_roll(); @@ -1750,7 +1763,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && _parameters.do_runway_takeoff && _takeoff_on_runway == true) { /* Ramp-up thrust and stay at max */ - _att_sp.thrust = _att_sp.thrust < _parameters.throttle_max ? hrt_elapsed_time(&_takeoff_runway_start) / (float)2000000 : _parameters.throttle_max; + _att_sp.thrust = _att_sp.thrust < _parameters.runway_takeoff_throttle ? + hrt_elapsed_time(&_takeoff_runway_start) / (float)2000000 : + _parameters.runway_takeoff_throttle; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 29fbeed386..5e05c6b9ef 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -431,18 +431,55 @@ PARAM_DEFINE_INT32(FW_LND_USETER, 0); * * 0: disabled, 1: enabled * + * @min 0 + * @max 1 * @group L1 Control */ -PARAM_DEFINE_INT32(FW_RUNWAY_TKOFF, 0); +PARAM_DEFINE_INT32(FW_RNW_TKOFF, 0); /** * Specifies which heading should be held during runnway takeoff. * * 0: airframe heading, 1: heading towards takeoff waypoint * + * @min 0 + * @max 1 * @group L1 Control */ -PARAM_DEFINE_INT32(FW_TKOFF_HDG, 0); +PARAM_DEFINE_INT32(FW_RNW_HDG, 0); + +/** + * Altitude AGL at which navigation towards takeoff waypoint starts. + * Until FW_RNW_NAV_ALT is reached the plane is held level and only + * rudder is used to keep the heading (see FW_RNW_HDG). + * + * @min 0.0 + * @max 100.0 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_RNW_NAV_ALT, 5.0); + +/** + * Max throttle during runway takeoff. + * (Can be limited to test taxi on runway) + * + * @min 0.0 + * @max 1.0 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_RNW_MAX_THR, 1.0); + +/** + * Pitch setpoint during runway takeoff. + * A taildragger with stearable wheel might need to pitch up + * a little to keep it's wheel on the ground before airspeed + * to takeoff is reached. + * + * @min 0.0 + * @max 20.0 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_RNW_PSP, 0.0); /** * Flare, minimum pitch From b93f34c99bb5d71bc4260541235b9e906d9b2796 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 12 Sep 2015 10:42:35 +0200 Subject: [PATCH 062/136] use separate heading parameter beacause the other one gets reset in auto --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 6771921b2f..16ed16ad10 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -197,6 +197,7 @@ private: bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ bool _takeoff_initialized; bool _takeoff_on_runway; /**< true as long as runway takeoff is in progress */ + float _runway_takeoff_hdg_hold; hrt_abstime _takeoff_runway_start; /**< time at which we start takeoff on runway */ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ @@ -542,7 +543,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _was_in_air(false), _takeoff_initialized(false), _takeoff_on_runway(false), - _takeoff_runway_start(0), + _runway_takeoff_hdg_hold(0), + _takeoff_runway_start(0), _time_went_in_air(0), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), @@ -1386,7 +1388,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_parameters.do_runway_takeoff == true) { if (!_takeoff_initialized) { - _hdg_hold_yaw = _att.yaw; + _runway_takeoff_hdg_hold = _att.yaw; _takeoff_runway_start = hrt_absolute_time(); _takeoff_initialized = true; _takeoff_on_runway = true; @@ -1441,7 +1443,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_parameters.runway_takeoff_heading == 0) { // fix heading in the direction the airframe points - _att_sp.yaw_body = _hdg_hold_yaw; + _att_sp.yaw_body = _runway_takeoff_hdg_hold; //PX4_WARN("using initial heading: %.4f", (double)_att_sp.yaw_body); } else if (_parameters.runway_takeoff_heading == 1) { // or head into the direction of the takeoff waypoint From 1f8ebb71f51ed67aef6a14cc300d771fca789bb9 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 14 Sep 2015 10:08:33 +0200 Subject: [PATCH 063/136] fix comment --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 5e05c6b9ef..76b22f8350 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -461,7 +461,7 @@ PARAM_DEFINE_FLOAT(FW_RNW_NAV_ALT, 5.0); /** * Max throttle during runway takeoff. - * (Can be limited to test taxi on runway) + * (Can be used to test taxi on runway) * * @min 0.0 * @max 1.0 From 51ef8541739ffb864fd4797874df15fb5314fac0 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 16 Sep 2015 01:00:44 +0200 Subject: [PATCH 064/136] extracted runway takeoff logic into external class --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 208 ++++++++++++++++++ src/lib/runway_takeoff/RunwayTakeoff.h | 115 ++++++++++ src/lib/runway_takeoff/module.mk | 41 ++++ .../runway_takeoff/runway_takeoff_params.c | 100 +++++++++ .../fw_pos_control_l1_main.cpp | 153 +++++-------- .../fw_pos_control_l1_params.c | 55 ----- 6 files changed, 520 insertions(+), 152 deletions(-) create mode 100644 src/lib/runway_takeoff/RunwayTakeoff.cpp create mode 100644 src/lib/runway_takeoff/RunwayTakeoff.h create mode 100644 src/lib/runway_takeoff/module.mk create mode 100644 src/lib/runway_takeoff/runway_takeoff_params.c diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp new file mode 100644 index 0000000000..e1814cecc1 --- /dev/null +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -0,0 +1,208 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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 RunwayTakeoff.cpp + * Runway takeoff handling for fixed-wing UAVs with steerable wheels. + * + * @author Roman Bapst + * @author Andreas Antener + */ + +#include +#include +#include + +#include "RunwayTakeoff.h" +#include +#include +#include +#include + +namespace runwaytakeoff +{ + +RunwayTakeoff::RunwayTakeoff() : + SuperBlock(NULL, "RWTO"), + _state(), + _initialized(false), + _initialized_time(0), + _init_yaw(0), + _climbout(false), + _min_airspeed_scaling(1.3f), + _runway_takeoff_enabled(this, "TKOFF"), + _runway_takeoff_heading(this, "HDG"), + _runway_takeoff_nav_alt(this, "NAV_ALT"), + _runway_takeoff_throttle(this, "MAX_THR"), + _runway_takeoff_pitch_sp(this, "PSP"), + _airspeed_min(this, "FW_AIRSPD_MIN", false), + _climbout_diff(this, "FW_CLMBOUT_DIFF", false) +{ + + updateParams(); +} + +RunwayTakeoff::~RunwayTakeoff() +{ + +} + +void RunwayTakeoff::init(float yaw) +{ + _init_yaw = yaw; + _initialized = true; + _state = RunwayTakeoffState::THROTTLE_RAMP; + _initialized_time = hrt_absolute_time(); +} + +void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) +{ + if (_climbout_diff.get() > 0.0001f && alt_agl < _climbout_diff.get()) { + _climbout = true; + } + + else { + _climbout = false; + } + + switch (_state) { + case RunwayTakeoffState::THROTTLE_RAMP: + if (hrt_elapsed_time(&_initialized_time) > 1e6) { + _state = RunwayTakeoffState::CLAMPED_TO_RUNWAY; + } + + break; + + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { + _state = RunwayTakeoffState::TAKEOFF; + mavlink_log_info(mavlink_fd, "#audio: Takeoff airspeed reached"); + } + + break; + + case RunwayTakeoffState::TAKEOFF: + if (alt_agl > math::max(_runway_takeoff_nav_alt.get(), _climbout_diff.get())) { + _state = RunwayTakeoffState::FLY; + mavlink_log_info(mavlink_fd, "#audio: Navigating to waypoint"); + } + + break; + + default: + return; + } +} + +bool RunwayTakeoff::controlYaw() +{ + // keep controlling yaw with rudder until we have enough ground clearance + return _state < RunwayTakeoffState::FLY; +} + +float RunwayTakeoff::getPitch(float tecsPitch) +{ + if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { + return math::radians(_runway_takeoff_pitch_sp.get()); + } + + return tecsPitch; +} + +float RunwayTakeoff::getRoll(float navigatorRoll) +{ + if (_state < RunwayTakeoffState::FLY) { + return 0.0f; + } + + return navigatorRoll; +} + +float RunwayTakeoff::getYaw(float navigatorYaw) +{ + if (_state < RunwayTakeoffState::FLY) { + if (_runway_takeoff_heading.get() == 0) { + // fix heading in the direction the airframe points + return _init_yaw; + + } else if (_runway_takeoff_heading.get() == 1) { + // or head into the direction of the takeoff waypoint + // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) + return navigatorYaw; + } + } + + return navigatorYaw; +} + +float RunwayTakeoff::getThrottle(float tecsThrottle) +{ + switch (_state) { + case RunwayTakeoffState::THROTTLE_RAMP: { + float throttle = hrt_elapsed_time(&_initialized_time) / (float)2000000 * + _runway_takeoff_throttle.get(); + return throttle < _runway_takeoff_throttle.get() ? + throttle : + _runway_takeoff_throttle.get(); + } + + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + return _runway_takeoff_throttle.get(); + + default: + return tecsThrottle; + } +} + +bool RunwayTakeoff::resetIntegrators() +{ + return _state <= RunwayTakeoffState::TAKEOFF; +} + +float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) +{ + if (_climbout) { + return math::max(sp_min, climbout_min); + } + + else { + return min; + } +} + +void RunwayTakeoff::reset() +{ + _initialized = false; + _state = RunwayTakeoffState::THROTTLE_RAMP; +} + +} diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h new file mode 100644 index 0000000000..7a4dd17f50 --- /dev/null +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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 RunwayTakeoff.h + * Runway takeoff handling for fixed-wing UAVs with steerable wheels. + * + * @author Roman Bapst + * @author Andreas Antener + */ + +#ifndef RUNWAYTAKEOFF_H +#define RUNWAYTAKEOFF_H + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace runwaytakeoff +{ + +enum RunwayTakeoffState { + THROTTLE_RAMP = 0, /**< */ + CLAMPED_TO_RUNWAY = 1, /**< */ + TAKEOFF = 2, /**< */ + FLY = 3 /**< */ +}; + +class __EXPORT RunwayTakeoff : public control::SuperBlock +{ +public: + RunwayTakeoff(); + ~RunwayTakeoff(); + + void init(float yaw); + void update(float airspeed, float alt_agl, int mavlink_fd); + + RunwayTakeoffState getState() { return _state; }; + bool isInitialized() { return _initialized; }; + + bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; + + bool controlYaw(); + bool climbout() { return _climbout; }; + float getPitch(float tecsPitch); + float getRoll(float navigatorRoll); + float getYaw(float navigatorYaw); + float getThrottle(float tecsThrottle); + bool resetIntegrators(); + float getMinAirspeedScaling() { return _min_airspeed_scaling; }; + float getMinPitch(float sp_min, float climbout_min, float min); + + void reset(); + +protected: +private: + /** state variables **/ + RunwayTakeoffState _state; + bool _initialized; + hrt_abstime _initialized_time; + float _init_yaw; + bool _climbout; + + /** magic values **/ + float _min_airspeed_scaling; + + /** parameters **/ + control::BlockParamInt _runway_takeoff_enabled; + control::BlockParamInt _runway_takeoff_heading; + control::BlockParamFloat _runway_takeoff_nav_alt; + control::BlockParamFloat _runway_takeoff_throttle; + control::BlockParamFloat _runway_takeoff_pitch_sp; + control::BlockParamFloat _airspeed_min; + control::BlockParamFloat _climbout_diff; + +}; + +} + +#endif // RUNWAYTAKEOFF_H diff --git a/src/lib/runway_takeoff/module.mk b/src/lib/runway_takeoff/module.mk new file mode 100644 index 0000000000..95b9aea53e --- /dev/null +++ b/src/lib/runway_takeoff/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# 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. +# +############################################################################ + +# +# RunwayTakeoff Library +# + +SRCS = RunwayTakeoff.cpp \ + runway_takeoff_params.c + +MAXOPTIMIZATION = -Os diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c new file mode 100644 index 0000000000..6ccc21e76a --- /dev/null +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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 runway_takeoff_params.c + * + * Parameters for runway takeoff + * + * @author Andreas Antener + */ + +#include + +#include + +/** + * Enable or disable runway takeoff with landing gear + * + * 0: disabled, 1: enabled + * + * @min 0 + * @max 1 + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_TKOFF, 0); + +/** + * Specifies which heading should be held during runnway takeoff. + * + * 0: airframe heading, 1: heading towards takeoff waypoint + * + * @min 0 + * @max 1 + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_HDG, 0); + +/** + * Altitude AGL at which navigation towards takeoff waypoint starts. + * Until RWTO_NAV_ALT is reached the plane is held level and only + * rudder is used to keep the heading (see RWTO_HDG). If this is lower + * than FW_CLMBOUT_DIFF, FW_CLMBOUT_DIFF is used instead. + * + * @min 0.0 + * @max 100.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0); + +/** + * Max throttle during runway takeoff. + * (Can be used to test taxi on runway) + * + * @min 0.0 + * @max 1.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); + +/** + * Pitch setpoint during runway takeoff. + * A taildragger with stearable wheel might need to pitch up + * a little to keep it's wheel on the ground before airspeed + * to takeoff is reached. + * + * @min 0.0 + * @max 20.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 16ed16ad10..34f414f0d6 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -95,6 +95,7 @@ #include "landingslope.h" #include "mtecs/mTecs.h" #include +#include static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode @@ -195,12 +196,10 @@ private: bool land_useterrain; bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ - bool _takeoff_initialized; - bool _takeoff_on_runway; /**< true as long as runway takeoff is in progress */ - float _runway_takeoff_hdg_hold; - hrt_abstime _takeoff_runway_start; /**< time at which we start takeoff on runway */ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ + runwaytakeoff::RunwayTakeoff _runway_takeoff; + /* takeoff/launch states */ LaunchDetectionResult launch_detection_state; @@ -282,12 +281,6 @@ private: float land_flare_pitch_max_deg; int land_use_terrain_estimate; - int do_runway_takeoff; - int runway_takeoff_heading; - float runway_takeoff_nav_alt; - float runway_takeoff_throttle; - float runway_takeoff_pitch_sp; - } _parameters; /**< local copies of interesting parameters */ struct { @@ -336,12 +329,6 @@ private: param_t land_flare_pitch_max_deg; param_t land_use_terrain_estimate; - param_t do_runway_takeoff; - param_t runway_takeoff_heading; - param_t runway_takeoff_nav_alt; - param_t runway_takeoff_throttle; - param_t runway_takeoff_pitch_sp; - } _parameter_handles; /**< handles for interesting parameters */ @@ -541,11 +528,8 @@ FixedwingPositionControl::FixedwingPositionControl() : land_onslope(false), land_useterrain(false), _was_in_air(false), - _takeoff_initialized(false), - _takeoff_on_runway(false), - _runway_takeoff_hdg_hold(0), - _takeoff_runway_start(0), _time_went_in_air(0), + _runway_takeoff(), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), landingslope(), @@ -588,12 +572,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); - _parameter_handles.do_runway_takeoff = param_find("FW_RNW_TKOFF"); - _parameter_handles.runway_takeoff_heading = param_find("FW_RNW_HDG"); - _parameter_handles.runway_takeoff_nav_alt = param_find("FW_RNW_NAV_ALT"); - _parameter_handles.runway_takeoff_throttle = param_find("FW_RNW_MAX_THR"); - _parameter_handles.runway_takeoff_pitch_sp = param_find("FW_RNW_PSP"); - _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -697,12 +675,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); - param_get(_parameter_handles.do_runway_takeoff, &(_parameters.do_runway_takeoff)); - param_get(_parameter_handles.runway_takeoff_heading, &(_parameters.runway_takeoff_heading)); - param_get(_parameter_handles.runway_takeoff_nav_alt, &(_parameters.runway_takeoff_nav_alt)); - param_get(_parameter_handles.runway_takeoff_throttle, &(_parameters.runway_takeoff_throttle)); - param_get(_parameter_handles.runway_takeoff_pitch_sp, &(_parameters.runway_takeoff_pitch_sp)); - _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -752,6 +724,8 @@ FixedwingPositionControl::parameters_update() /* Update the mTecs */ _mTecs.updateParams(); + _runway_takeoff.updateParams(); + return OK; } @@ -1386,72 +1360,54 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - if (_parameters.do_runway_takeoff == true) { - if (!_takeoff_initialized) { - _runway_takeoff_hdg_hold = _att.yaw; - _takeoff_runway_start = hrt_absolute_time(); - _takeoff_initialized = true; - _takeoff_on_runway = true; + if (_runway_takeoff.runwayTakeoffEnabled()) { + if (!_runway_takeoff.isInitialized()) { + _runway_takeoff.init(_att.yaw); + /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ _takeoff_ground_alt = _global_pos.alt; - } - - if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) { // XXX takeoff: magic value - _takeoff_on_runway = true; - _att_sp.pitch_body = math::radians(_parameters.runway_takeoff_pitch_sp); - } else { - // min airspeed reached, let tecs control it - _takeoff_on_runway = false; - float takeoff_pitch_max_deg = _parameters.pitch_limit_max; - float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); - - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(1.3f * _parameters.airspeed_min), // XXX takeoff: magic value - eas2tas, - math::radians(_parameters.pitch_limit_min), - takeoff_pitch_max_rad, - _parameters.throttle_min, - _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? - _parameters.throttle_cruise, - true, - math::max(math::radians(_pos_sp_triplet.current.pitch_min), - math::radians(10.0f)), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_TAKEOFF, - takeoff_pitch_max_deg != _parameters.pitch_limit_max); - - _att_sp.pitch_body = _tecs.get_pitch_demand(); + mavlink_log_info(_mavlink_fd, "#audio: Takeoff on runway"); } // update navigation _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); - //PX4_WARN("talt: %f", (double)terrain_alt); - if (_global_pos.alt - terrain_alt > _parameters.runway_takeoff_nav_alt) { - // XXX once in here we should maybe not fall out again (although it shouldn't matter when we're climbing) - // start to navigate to takeoff waypoint as soon as we are high enough in the air - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - } else { - // for takeoff we want heading control with rudder, roll and pitch stabilization to zero - // throttle should be ramped up to max - _att_sp.roll_body = 0.0f; - _att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly + // update runway takeoff helper + _runway_takeoff.update( + _airspeed.true_airspeed_m_s, + _global_pos.alt - terrain_alt, + _mavlink_fd); - if (_parameters.runway_takeoff_heading == 0) { - // fix heading in the direction the airframe points - _att_sp.yaw_body = _runway_takeoff_hdg_hold; - //PX4_WARN("using initial heading: %.4f", (double)_att_sp.yaw_body); - } else if (_parameters.runway_takeoff_heading == 1) { - // or head into the direction of the takeoff waypoint - // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) - _att_sp.yaw_body = _l1_control.nav_bearing(); // XXX check if this is really the actual bearing - //PX4_WARN("using nav heading: %.4f", (double)_att_sp.yaw_body); - } - } + // update tecs + float takeoff_pitch_max_deg = _parameters.pitch_limit_max; + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); + + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed( + _runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, + _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? + _parameters.throttle_cruise, + _runway_takeoff.climbout(), + math::radians(_runway_takeoff.getMinPitch( + _pos_sp_triplet.current.pitch_min, + 10.0f, + _parameters.pitch_limit_min)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); + + // assign values + _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); + _att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing()); + _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); // tell attitude controller if he should control yaw directly + _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); } else { @@ -1553,6 +1509,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset takeoff/launch state */ + // FIXME: reset on arm/disarm cycle and mode switch if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } @@ -1754,23 +1711,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && - !_parameters.do_runway_takeoff) { + !_runway_takeoff.runwayTakeoffEnabled()) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); - } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto + + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - _parameters.do_runway_takeoff && _takeoff_on_runway == true) { - /* Ramp-up thrust and stay at max */ - _att_sp.thrust = _att_sp.thrust < _parameters.runway_takeoff_throttle ? - hrt_elapsed_time(&_takeoff_runway_start) / (float)2000000 : - _parameters.runway_takeoff_throttle; + _runway_takeoff.runwayTakeoffEnabled()) { + _att_sp.thrust = _runway_takeoff.getThrottle( + math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : + _tecs.get_throttle_demand(), throttle_max)); + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; + } else { /* Copy thrust and pitch values from tecs */ if (_vehicle_status.condition_landed && @@ -1950,8 +1910,7 @@ void FixedwingPositionControl::reset_takeoff_state() { launch_detection_state = LAUNCHDETECTION_RES_NONE; launchDetector.reset(); - _takeoff_initialized = false; - _takeoff_on_runway = false; + _runway_takeoff.reset(); } void FixedwingPositionControl::reset_landing_state() diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 76b22f8350..5870039c24 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -426,61 +426,6 @@ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); */ PARAM_DEFINE_INT32(FW_LND_USETER, 0); -/** - * Enable or disable runway takeoff with landing gear - * - * 0: disabled, 1: enabled - * - * @min 0 - * @max 1 - * @group L1 Control - */ -PARAM_DEFINE_INT32(FW_RNW_TKOFF, 0); - -/** - * Specifies which heading should be held during runnway takeoff. - * - * 0: airframe heading, 1: heading towards takeoff waypoint - * - * @min 0 - * @max 1 - * @group L1 Control - */ -PARAM_DEFINE_INT32(FW_RNW_HDG, 0); - -/** - * Altitude AGL at which navigation towards takeoff waypoint starts. - * Until FW_RNW_NAV_ALT is reached the plane is held level and only - * rudder is used to keep the heading (see FW_RNW_HDG). - * - * @min 0.0 - * @max 100.0 - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_RNW_NAV_ALT, 5.0); - -/** - * Max throttle during runway takeoff. - * (Can be used to test taxi on runway) - * - * @min 0.0 - * @max 1.0 - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_RNW_MAX_THR, 1.0); - -/** - * Pitch setpoint during runway takeoff. - * A taildragger with stearable wheel might need to pitch up - * a little to keep it's wheel on the ground before airspeed - * to takeoff is reached. - * - * @min 0.0 - * @max 20.0 - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_RNW_PSP, 0.0); - /** * Flare, minimum pitch * From 6c31421889f5bc4fe1b389a72713cd12274774fe Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 23 Sep 2015 12:30:38 +0200 Subject: [PATCH 065/136] extracted heading controller --- .../attitude_fw/ecl_heading_controller.cpp | 163 ++++++++++++++++++ .../ecl/attitude_fw/ecl_heading_controller.h | 70 ++++++++ .../ecl/attitude_fw/ecl_yaw_controller.cpp | 34 ---- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 5 +- src/lib/runway_takeoff/RunwayTakeoff.cpp | 4 +- .../fw_att_control/fw_att_control_main.cpp | 26 ++- .../fw_pos_control_l1_main.cpp | 19 +- 7 files changed, 265 insertions(+), 56 deletions(-) create mode 100644 src/lib/ecl/attitude_fw/ecl_heading_controller.cpp create mode 100644 src/lib/ecl/attitude_fw/ecl_heading_controller.h diff --git a/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp b/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp new file mode 100644 index 0000000000..97dfa2f617 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.cpp + * Implementation of a simple orthogonal coordinated turn yaw PID controller. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_heading_controller.h" +#include +#include +#include +#include +#include +#include +#include + +ECL_HeadingController::ECL_HeadingController() : + ECL_Controller("heading") +{ +} + +ECL_HeadingController::~ECL_HeadingController() +{ +} + +float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) && + PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && + PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && + PX4_ISFINITE(ctl_data.scaler))) { + perf_count(_nonfinite_input_perf); + return math::constrain(_last_output, -1.0f, 1.0f); + } + + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + bool lock_integrator = ctl_data.lock_integrator; + + if (dt_micros > 500000) { + lock_integrator = true; + } + + /* input conditioning */ + float airspeed = ctl_data.airspeed; + + if (!PX4_ISFINITE(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); + + } else if (airspeed < ctl_data.airspeed_min) { + airspeed = ctl_data.airspeed_min; + } + + + /* Transform setpoint to body angular rates (jacobian) */ + _bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint + + cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint; + + /* Calculate body angular rate error */ + _rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error + + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { + + float id = _rate_error * dt; + + /* + * anti-windup: do not allow integrator to increase if actuator is at limit + */ + if (_last_output < -1.0f) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + + } else if (_last_output > 1.0f) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + //xxx: until start detection is available: integral part in control signal is limited here + float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); + + /* Apply PI rate controller and store non-limited output */ + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler * + ctl_data.scaler; //scaler is proportional to 1/airspeed + warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); + + + return math::constrain(_last_output, -1.0f, 1.0f); +} + + +float ECL_HeadingController::control_attitude(const struct ECL_ControlData &ctl_data) +{ + /* Do not calculate control signal with bad inputs */ + if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && + PX4_ISFINITE(ctl_data.yaw) && + PX4_ISFINITE(ctl_data.airspeed))) { + perf_count(_nonfinite_input_perf); + warnx("not controlling yaw"); + return _rate_setpoint; + } + + /* Calculate the error */ + float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; + + /* Apply P controller: rate setpoint from current error and time constant */ + _rate_setpoint = yaw_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f) { + if (_rate_setpoint > 0.0f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + + } else { + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + } + + return _rate_setpoint; +} diff --git a/src/lib/ecl/attitude_fw/ecl_heading_controller.h b/src/lib/ecl/attitude_fw/ecl_heading_controller.h new file mode 100644 index 0000000000..5dd79cd7d4 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_heading_controller.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.h + * Definition of a simple orthogonal coordinated turn yaw PID controller. + * + * @author Lorenz Meier + * @author Thomas Gubler + * @author Andreas Antener + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ +#ifndef ECL_HEADING_CONTROLLER_H +#define ECL_HEADING_CONTROLLER_H + +#include +#include + +#include "ecl_controller.h" + +class __EXPORT ECL_HeadingController : + public ECL_Controller +{ +public: + ECL_HeadingController(); + + ~ECL_HeadingController(); + + float control_attitude(const struct ECL_ControlData &ctl_data); + + float control_bodyrate(const struct ECL_ControlData &ctl_data); +}; + +#endif // ECL_HEADING_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 240a426bd0..4849bdad79 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -67,9 +67,6 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data case COORD_METHOD_CLOSEACC: return control_attitude_impl_accclosedloop(ctl_data); - case COORD_METHOD_HEADING: - return control_heading(ctl_data); - default: static hrt_abstime last_print = 0; @@ -241,34 +238,3 @@ float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_Co /* dont set a rate setpoint */ return 0.0f; } - -float ECL_YawController::control_heading(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && - PX4_ISFINITE(ctl_data.yaw) && - PX4_ISFINITE(ctl_data.airspeed))) { - perf_count(_nonfinite_input_perf); - warnx("not controlling yaw"); - return _rate_setpoint; - } - - /* Calculate the error */ - float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; - - /* Apply P controller: rate setpoint from current error and time constant */ - _rate_setpoint = yaw_error / _tc; - - /* limit the rate */ - if (_max_rate > 0.01f) { - if (_rate_setpoint > 0.0f) { - _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; - - } else { - _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; - } - - } - - return _rate_setpoint; -} diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index e3e529b30f..511b5fd36f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -78,8 +78,7 @@ public: enum { COORD_METHOD_OPEN = 0, - COORD_METHOD_CLOSEACC = 1, - COORD_METHOD_HEADING = 2 + COORD_METHOD_CLOSEACC = 1 }; protected: @@ -94,8 +93,6 @@ protected: float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data); - float control_heading(const struct ECL_ControlData &ctl_data); - }; #endif // ECL_YAW_CONTROLLER_H diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index e1814cecc1..d7c33d7094 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -105,7 +105,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::CLAMPED_TO_RUNWAY: if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { _state = RunwayTakeoffState::TAKEOFF; - mavlink_log_info(mavlink_fd, "#audio: Takeoff airspeed reached"); + mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); } break; @@ -113,7 +113,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::TAKEOFF: if (alt_agl > math::max(_runway_takeoff_nav_alt.get(), _climbout_diff.get())) { _state = RunwayTakeoffState::FLY; - mavlink_log_info(mavlink_fd, "#audio: Navigating to waypoint"); + mavlink_log_info(mavlink_fd, "#Navigating to waypoint"); } break; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index de8ee9173e..938347ff95 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -82,6 +82,7 @@ #include #include #include +#include #include /** @@ -248,6 +249,7 @@ private: ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; + ECL_HeadingController _heading_ctrl; /** @@ -502,6 +504,13 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); + /* heading control parameters */ + _heading_ctrl.set_k_p(_parameters.y_p); + _heading_ctrl.set_k_i(_parameters.y_i); + _heading_ctrl.set_k_ff(_parameters.y_ff); + _heading_ctrl.set_integrator_max(_parameters.y_integrator_max); + _heading_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); + return OK; } @@ -961,18 +970,14 @@ FixedwingAttitudeControl::task_main() control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; - if (_att_sp.fw_control_yaw == true) { - // this method controls heading directly with rudder. Used for auto takeoff on runway - _yaw_ctrl.set_coordinated_method(ECL_YawController::COORD_METHOD_HEADING); - } else { - _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); - } + _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); /* Run attitude controllers */ if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude + _heading_ctrl.control_attitude(control_input); /* Update input data for rate controllers */ control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); @@ -1012,7 +1017,14 @@ FixedwingAttitudeControl::task_main() } } - float yaw_u = _yaw_ctrl.control_bodyrate(control_input); + float yaw_u = 0.0f; + if (_att_sp.fw_control_yaw == true) { + yaw_u = _heading_ctrl.control_bodyrate(control_input); + } + + else { + yaw_u = _yaw_ctrl.control_bodyrate(control_input); + } _actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 34f414f0d6..7f7d0b15b3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -960,7 +960,7 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint * for the whole landing */ if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) { if(!land_useterrain) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate"); + mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate"); land_useterrain = true; } return global_pos.terrain_alt; @@ -1229,7 +1229,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { target_bearing = _yaw; } - mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold"); + mavlink_log_info(_mavlink_fd, "#Landing, heading hold"); } // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw)); @@ -1290,7 +1290,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; - mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle"); + mavlink_log_info(_mavlink_fd, "#Landing, limiting throttle"); } } @@ -1314,7 +1314,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); if (!land_noreturn_vertical) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); + mavlink_log_info(_mavlink_fd, "#Landing, flaring"); land_noreturn_vertical = true; } //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); @@ -1336,7 +1336,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { - mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); + mavlink_log_info(_mavlink_fd, "#Landing, on slope"); land_onslope = true; } } else { @@ -1367,7 +1367,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ _takeoff_ground_alt = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "#audio: Takeoff on runway"); + mavlink_log_info(_mavlink_fd, "#Takeoff on runway"); } // update navigation @@ -1417,7 +1417,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Inform user that launchdetection is running */ static hrt_abstime last_sent = 0; if(hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_critical(_mavlink_fd, "Launchdetection running"); + mavlink_log_critical(_mavlink_fd, "#Launchdetection running"); last_sent = hrt_absolute_time(); } @@ -1509,7 +1509,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset takeoff/launch state */ - // FIXME: reset on arm/disarm cycle and mode switch if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } @@ -1750,7 +1749,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * already (not by tecs) */ if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state == LAUNCHDETECTION_RES_NONE)) { + (launch_detection_state == LAUNCHDETECTION_RES_NONE || + _runway_takeoff.runwayTakeoffEnabled()) + )) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } From 234a200e6081bc19c0489a5bd266ed4d00cee917 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 24 Sep 2015 15:57:51 +0200 Subject: [PATCH 066/136] renamed heading controller to wheel controller, added groundspeed dependency and separate parameters --- msg/vehicle_attitude_setpoint.msg | 2 +- src/lib/ecl/attitude_fw/ecl_controller.h | 1 + ...ontroller.cpp => ecl_wheel_controller.cpp} | 55 +++++++---------- ...ng_controller.h => ecl_wheel_controller.h} | 8 +-- src/lib/runway_takeoff/RunwayTakeoff.cpp | 7 ++- src/lib/runway_takeoff/RunwayTakeoff.h | 2 +- .../fw_att_control/fw_att_control_main.cpp | 51 ++++++++++++---- .../fw_att_control/fw_att_control_params.c | 60 +++++++++++++++++++ .../fw_pos_control_l1_main.cpp | 7 ++- 9 files changed, 138 insertions(+), 55 deletions(-) rename src/lib/ecl/attitude_fw/{ecl_heading_controller.cpp => ecl_wheel_controller.cpp} (67%) rename src/lib/ecl/attitude_fw/{ecl_heading_controller.h => ecl_wheel_controller.h} (95%) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 3bbb5efa7b..bcfe0f41da 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -22,4 +22,4 @@ bool roll_reset_integral # Reset roll integral part (navigation logic change) bool pitch_reset_integral # Reset pitch integral part (navigation logic change) bool yaw_reset_integral # Reset yaw integral part (navigation logic change) -bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) +bool fw_control_wheel # control heading with rudder (used for auto takeoff on runway) diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h index ac1ac88d04..0817204ccc 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_controller.h @@ -75,6 +75,7 @@ struct ECL_ControlData { float airspeed_max; float airspeed; float scaler; + float ground_speed; bool lock_integrator; }; diff --git a/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp similarity index 67% rename from src/lib/ecl/attitude_fw/ecl_heading_controller.cpp rename to src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index 97dfa2f617..b0df2a95d6 100644 --- a/src/lib/ecl/attitude_fw/ecl_heading_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -32,13 +32,13 @@ ****************************************************************************/ /** - * @file ecl_yaw_controller.cpp + * @file ecl_wheel_controller.cpp * Implementation of a simple orthogonal coordinated turn yaw PID controller. * * Authors and acknowledgements in header. */ -#include "ecl_heading_controller.h" +#include "ecl_wheel_controller.h" #include #include #include @@ -47,22 +47,20 @@ #include #include -ECL_HeadingController::ECL_HeadingController() : - ECL_Controller("heading") +ECL_WheelController::ECL_WheelController() : + ECL_Controller("wheel") { } -ECL_HeadingController::~ECL_HeadingController() +ECL_WheelController::~ECL_WheelController() { } -float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_data) +float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) && - PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && - PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && - PX4_ISFINITE(ctl_data.scaler))) { + if (!(PX4_ISFINITE(ctl_data.yaw_rate) && + PX4_ISFINITE(ctl_data.ground_speed))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -80,27 +78,21 @@ float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_ } /* input conditioning */ - float airspeed = ctl_data.airspeed; - - if (!PX4_ISFINITE(airspeed)) { - /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); - - } else if (airspeed < ctl_data.airspeed_min) { - airspeed = ctl_data.airspeed_min; + float min_speed = 1.0f; + /* assume minimum speed to prevent oscillations */ + float speed = min_speed; + if (ctl_data.ground_speed > speed) { + speed = ctl_data.ground_speed; } - - /* Transform setpoint to body angular rates (jacobian) */ - _bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint + - cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint; + float scaler = 1.0f / speed; /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error + _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error - if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && speed > min_speed) { - float id = _rate_error * dt; + float id = _rate_error * dt * scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -122,23 +114,22 @@ float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_ float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler * - ctl_data.scaler; //scaler is proportional to 1/airspeed - warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); + _last_output = (_rate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler; + /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, speed %.4f, _k_i %.4f, _k_p: %.4f", + (double)_last_output, (double)_integrator, (double)_integrator_max, (double)speed, (double)_k_i, (double)_k_p);*/ return math::constrain(_last_output, -1.0f, 1.0f); } -float ECL_HeadingController::control_attitude(const struct ECL_ControlData &ctl_data) +float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && - PX4_ISFINITE(ctl_data.yaw) && - PX4_ISFINITE(ctl_data.airspeed))) { + PX4_ISFINITE(ctl_data.yaw))) { perf_count(_nonfinite_input_perf); - warnx("not controlling yaw"); + warnx("not controlling wheel"); return _rate_setpoint; } diff --git a/src/lib/ecl/attitude_fw/ecl_heading_controller.h b/src/lib/ecl/attitude_fw/ecl_wheel_controller.h similarity index 95% rename from src/lib/ecl/attitude_fw/ecl_heading_controller.h rename to src/lib/ecl/attitude_fw/ecl_wheel_controller.h index 5dd79cd7d4..f153a8f8f1 100644 --- a/src/lib/ecl/attitude_fw/ecl_heading_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file ecl_yaw_controller.h + * @file ecl_wheel_controller.h * Definition of a simple orthogonal coordinated turn yaw PID controller. * * @author Lorenz Meier @@ -54,13 +54,13 @@ #include "ecl_controller.h" -class __EXPORT ECL_HeadingController : +class __EXPORT ECL_WheelController : public ECL_Controller { public: - ECL_HeadingController(); + ECL_WheelController(); - ~ECL_HeadingController(); + ~ECL_WheelController(); float control_attitude(const struct ECL_ControlData &ctl_data); diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index d7c33d7094..5067260d32 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -123,10 +123,10 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) } } -bool RunwayTakeoff::controlYaw() +bool RunwayTakeoff::controlWheel() { - // keep controlling yaw with rudder until we have enough ground clearance - return _state < RunwayTakeoffState::FLY; + // keep controlling wheel until takeoff + return _state < RunwayTakeoffState::TAKEOFF; } float RunwayTakeoff::getPitch(float tecsPitch) @@ -140,6 +140,7 @@ float RunwayTakeoff::getPitch(float tecsPitch) float RunwayTakeoff::getRoll(float navigatorRoll) { + // until we have enough ground clearance, set roll to 0 if (_state < RunwayTakeoffState::FLY) { return 0.0f; } diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 7a4dd17f50..2f84afee2b 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -75,7 +75,7 @@ public: bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; - bool controlYaw(); + bool controlWheel(); bool climbout() { return _climbout; }; float getPitch(float tecsPitch); float getRoll(float navigatorRoll); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 938347ff95..144a8ed048 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -82,7 +82,7 @@ #include #include #include -#include +#include #include /** @@ -182,6 +182,11 @@ private: float y_coordinated_min_speed; int32_t y_coordinated_method; float y_rmax; + float w_p; + float w_i; + float w_ff; + float w_integrator_max; + float w_rmax; float airspeed_min; float airspeed_trim; @@ -223,6 +228,11 @@ private: param_t y_coordinated_min_speed; param_t y_coordinated_method; param_t y_rmax; + param_t w_p; + param_t w_i; + param_t w_ff; + param_t w_integrator_max; + param_t w_rmax; param_t airspeed_min; param_t airspeed_trim; @@ -249,7 +259,7 @@ private: ECL_RollController _roll_ctrl; ECL_PitchController _pitch_ctrl; ECL_YawController _yaw_ctrl; - ECL_HeadingController _heading_ctrl; + ECL_WheelController _wheel_ctrl; /** @@ -382,6 +392,12 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.y_integrator_max = param_find("FW_YR_IMAX"); _parameter_handles.y_rmax = param_find("FW_Y_RMAX"); + _parameter_handles.w_p = param_find("FW_WR_P"); + _parameter_handles.w_i = param_find("FW_WR_I"); + _parameter_handles.w_ff = param_find("FW_WR_FF"); + _parameter_handles.w_integrator_max = param_find("FW_WR_IMAX"); + _parameter_handles.w_rmax = param_find("FW_W_RMAX"); + _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); @@ -460,6 +476,12 @@ FixedwingAttitudeControl::parameters_update() param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method)); param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax)); + param_get(_parameter_handles.w_p, &(_parameters.w_p)); + param_get(_parameter_handles.w_i, &(_parameters.w_i)); + param_get(_parameter_handles.w_ff, &(_parameters.w_ff)); + param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max)); + param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax)); + param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); @@ -504,12 +526,12 @@ FixedwingAttitudeControl::parameters_update() _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); - /* heading control parameters */ - _heading_ctrl.set_k_p(_parameters.y_p); - _heading_ctrl.set_k_i(_parameters.y_i); - _heading_ctrl.set_k_ff(_parameters.y_ff); - _heading_ctrl.set_integrator_max(_parameters.y_integrator_max); - _heading_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); + /* wheel control parameters */ + _wheel_ctrl.set_k_p(_parameters.w_p); + _wheel_ctrl.set_k_i(_parameters.w_i); + _wheel_ctrl.set_k_ff(_parameters.w_ff); + _wheel_ctrl.set_integrator_max(_parameters.w_integrator_max); + _wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax)); return OK; } @@ -846,6 +868,7 @@ FixedwingAttitudeControl::task_main() } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_velocity_enabled) { @@ -871,6 +894,7 @@ FixedwingAttitudeControl::task_main() } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_altitude_enabled) { @@ -890,6 +914,7 @@ FixedwingAttitudeControl::task_main() } if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } } else { /* @@ -940,6 +965,7 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); } /* Prepare speed_body_u and speed_body_w */ @@ -969,6 +995,8 @@ FixedwingAttitudeControl::task_main() control_input.airspeed = airspeed; control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; + control_input.ground_speed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + + _global_pos.vel_e * _global_pos.vel_e); _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); @@ -977,7 +1005,7 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude - _heading_ctrl.control_attitude(control_input); + _wheel_ctrl.control_attitude(control_input); /* Update input data for rate controllers */ control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); @@ -1018,8 +1046,8 @@ FixedwingAttitudeControl::task_main() } float yaw_u = 0.0f; - if (_att_sp.fw_control_yaw == true) { - yaw_u = _heading_ctrl.control_bodyrate(control_input); + if (_att_sp.fw_control_wheel == true) { + yaw_u = _wheel_ctrl.control_bodyrate(control_input); } else { @@ -1031,6 +1059,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[2] += yaw_manual; if (!PX4_ISFINITE(yaw_u)) { _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); if (_debug && loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 51c06fabb9..d5fdcd5e01 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -220,6 +220,55 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); */ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); +/** + * Wheel steering rate proportional gain + * + * This defines how much the wheel steering input will be commanded depending on the + * current body angular rate error. + * + * @min 0.005 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_P, 0.05f); + +/** + * Wheel steering rate integrator gain + * + * This gain defines how much control response will result out of a steady + * state error. It trims any constant error. + * + * @min 0.0 + * @max 50.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_I, 0.0f); + +/** + * Wheel steering rate integrator limit + * + * The portion of the integrator part in the control surface deflection is + * limited to this value + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_IMAX, 0.2f); + +/** + * Maximum wheel steering rate + * + * This limits the maximum wheel steering rate the controller will output (in degrees per + * second). Setting a value of zero disables the limit. + * + * @unit deg/s + * @min 0.0 + * @max 90.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_W_RMAX, 0.0f); + /** * Roll rate feed forward * @@ -255,6 +304,17 @@ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f); */ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); +/** + * Wheel steering rate feed forward + * + * Direct feed forward from rate setpoint to control surface output + * + * @min 0.0 + * @max 10.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f); + /** * Minimal speed for yaw coordination * diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7f7d0b15b3..08deeb60a9 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1074,7 +1074,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; - _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder + _att_sp.fw_control_wheel = false; // by default we don't want yaw to be contoller directly with rudder float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -1406,9 +1406,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // assign values _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); _att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing()); - _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); // tell attitude controller if he should control yaw directly + _att_sp.fw_control_wheel = _runway_takeoff.controlWheel(); _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); - + /*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body, + (double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/ } else { /* Perform launch detection */ From 0c875dd6d1091f6f49cea6dbc0a0b9513a378566 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 24 Sep 2015 20:56:42 +0200 Subject: [PATCH 067/136] calculate shortest yaw error --- src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index b0df2a95d6..12d2668f87 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -135,6 +135,9 @@ float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_da /* Calculate the error */ float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; + /* shortest angle (wrap around) */ + yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F; + /*warnx("yaw_error: %.4f", (double)yaw_error);*/ /* Apply P controller: rate setpoint from current error and time constant */ _rate_setpoint = yaw_error / _tc; From e987082292b82e2dce5065dc343867521741f47c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 24 Sep 2015 21:53:10 +0200 Subject: [PATCH 068/136] split takeoff into 2 phases, reseting integrators when still on runway --- msg/vehicle_attitude_setpoint.msg | 2 +- src/lib/runway_takeoff/RunwayTakeoff.cpp | 29 +++++++++++++++---- src/lib/runway_takeoff/RunwayTakeoff.h | 12 ++++---- .../runway_takeoff/runway_takeoff_params.c | 6 ++-- .../fw_att_control/fw_att_control_main.cpp | 2 +- .../fw_pos_control_l1_main.cpp | 10 +++++-- 6 files changed, 43 insertions(+), 18 deletions(-) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index bcfe0f41da..3bbb5efa7b 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -22,4 +22,4 @@ bool roll_reset_integral # Reset roll integral part (navigation logic change) bool pitch_reset_integral # Reset pitch integral part (navigation logic change) bool yaw_reset_integral # Reset yaw integral part (navigation logic change) -bool fw_control_wheel # control heading with rudder (used for auto takeoff on runway) +bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 5067260d32..77d2047882 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -59,6 +59,7 @@ RunwayTakeoff::RunwayTakeoff() : _init_yaw(0), _climbout(false), _min_airspeed_scaling(1.3f), + _max_takeoff_roll(15.0f), _runway_takeoff_enabled(this, "TKOFF"), _runway_takeoff_heading(this, "HDG"), _runway_takeoff_nav_alt(this, "NAV_ALT"), @@ -111,7 +112,15 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) break; case RunwayTakeoffState::TAKEOFF: - if (alt_agl > math::max(_runway_takeoff_nav_alt.get(), _climbout_diff.get())) { + if (alt_agl > _runway_takeoff_nav_alt.get()) { + _state = RunwayTakeoffState::CLIMBOUT; + mavlink_log_info(mavlink_fd, "#Climbout"); + } + + break; + + case RunwayTakeoffState::CLIMBOUT: + if (alt_agl > _climbout_diff.get()) { _state = RunwayTakeoffState::FLY; mavlink_log_info(mavlink_fd, "#Navigating to waypoint"); } @@ -123,10 +132,10 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) } } -bool RunwayTakeoff::controlWheel() +bool RunwayTakeoff::controlYaw() { - // keep controlling wheel until takeoff - return _state < RunwayTakeoffState::TAKEOFF; + // keep controlling yaw directly until we start navigation + return _state < RunwayTakeoffState::CLIMBOUT; } float RunwayTakeoff::getPitch(float tecsPitch) @@ -141,10 +150,17 @@ float RunwayTakeoff::getPitch(float tecsPitch) float RunwayTakeoff::getRoll(float navigatorRoll) { // until we have enough ground clearance, set roll to 0 - if (_state < RunwayTakeoffState::FLY) { + if (_state < RunwayTakeoffState::CLIMBOUT) { return 0.0f; } + // allow some roll during climbout + else if (_state < RunwayTakeoffState::FLY) { + return math::constrain(navigatorRoll, + math::radians(-_max_takeoff_roll), + math::radians(_max_takeoff_roll)); + } + return navigatorRoll; } @@ -186,7 +202,8 @@ float RunwayTakeoff::getThrottle(float tecsThrottle) bool RunwayTakeoff::resetIntegrators() { - return _state <= RunwayTakeoffState::TAKEOFF; + // reset integrators if we're still on runway + return _state < RunwayTakeoffState::TAKEOFF; } float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 2f84afee2b..265ff24cb3 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -55,10 +55,11 @@ namespace runwaytakeoff { enum RunwayTakeoffState { - THROTTLE_RAMP = 0, /**< */ - CLAMPED_TO_RUNWAY = 1, /**< */ - TAKEOFF = 2, /**< */ - FLY = 3 /**< */ + THROTTLE_RAMP = 0, /**< ramping up throttle */ + CLAMPED_TO_RUNWAY = 1, /**< clamped to runway, controlling yaw directly (wheel or rudder) */ + TAKEOFF = 2, /**< taking off, get ground clearance, roll 0 */ + CLIMBOUT = 3, /**< climbout to safe height before navigation, roll limited */ + FLY = 4 /**< fly towards takeoff waypoint */ }; class __EXPORT RunwayTakeoff : public control::SuperBlock @@ -75,7 +76,7 @@ public: bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; - bool controlWheel(); + bool controlYaw(); bool climbout() { return _climbout; }; float getPitch(float tecsPitch); float getRoll(float navigatorRoll); @@ -98,6 +99,7 @@ private: /** magic values **/ float _min_airspeed_scaling; + float _max_takeoff_roll; /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index 6ccc21e76a..d3ce5d238d 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -66,10 +66,10 @@ PARAM_DEFINE_INT32(RWTO_TKOFF, 0); PARAM_DEFINE_INT32(RWTO_HDG, 0); /** - * Altitude AGL at which navigation towards takeoff waypoint starts. + * Altitude AGL at which we have enough ground clearance to allow some roll. * Until RWTO_NAV_ALT is reached the plane is held level and only - * rudder is used to keep the heading (see RWTO_HDG). If this is lower - * than FW_CLMBOUT_DIFF, FW_CLMBOUT_DIFF is used instead. + * rudder is used to keep the heading (see RWTO_HDG). This should be below + * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0. * * @min 0.0 * @max 100.0 diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 144a8ed048..da91fda9f7 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1046,7 +1046,7 @@ FixedwingAttitudeControl::task_main() } float yaw_u = 0.0f; - if (_att_sp.fw_control_wheel == true) { + if (_att_sp.fw_control_yaw == true) { yaw_u = _wheel_ctrl.control_bodyrate(control_input); } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 08deeb60a9..ed71cf2955 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1074,7 +1074,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; - _att_sp.fw_control_wheel = false; // by default we don't want yaw to be contoller directly with rudder + _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -1371,6 +1371,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } // update navigation + // FIXME: after reselecting the takeoff wp, prev_wp is set to the next wp which causes wrong navigation behaviour _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); @@ -1406,8 +1407,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // assign values _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); _att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing()); - _att_sp.fw_control_wheel = _runway_takeoff.controlWheel(); + _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); + + _att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators(); + _att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators(); + _att_sp.yaw_reset_integral = _runway_takeoff.resetIntegrators(); + /*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body, (double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/ From 178ec7f4fc41fb136592212d1b645c822a0c1bad Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 25 Sep 2015 11:05:56 +0200 Subject: [PATCH 069/136] stay out of climbout once height has been reached, don't mix navigator roll with fixed heading --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 77d2047882..599cb721d0 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -87,13 +87,6 @@ void RunwayTakeoff::init(float yaw) void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) { - if (_climbout_diff.get() > 0.0001f && alt_agl < _climbout_diff.get()) { - _climbout = true; - } - - else { - _climbout = false; - } switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: @@ -105,6 +98,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::CLAMPED_TO_RUNWAY: if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { + _climbout = true; _state = RunwayTakeoffState::TAKEOFF; mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); } @@ -121,6 +115,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::CLIMBOUT: if (alt_agl > _climbout_diff.get()) { + _climbout = false; _state = RunwayTakeoffState::FLY; mavlink_log_info(mavlink_fd, "#Navigating to waypoint"); } @@ -154,11 +149,17 @@ float RunwayTakeoff::getRoll(float navigatorRoll) return 0.0f; } - // allow some roll during climbout + // allow some roll during climbout if waypoint heading is targeted else if (_state < RunwayTakeoffState::FLY) { - return math::constrain(navigatorRoll, - math::radians(-_max_takeoff_roll), - math::radians(_max_takeoff_roll)); + if (_runway_takeoff_heading.get() == 0) { + // otherwise stay at 0 roll + return 0.0f; + + } else if (_runway_takeoff_heading.get() == 1) { + return math::constrain(navigatorRoll, + math::radians(-_max_takeoff_roll), + math::radians(_max_takeoff_roll)); + } } return navigatorRoll; From 0769ec53454de195ed804c8c3b01d4d210b1d4c5 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 25 Sep 2015 14:30:01 +0200 Subject: [PATCH 070/136] added max pitch parameter for climbout phase --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 14 +++++++++++++- src/lib/runway_takeoff/RunwayTakeoff.h | 2 ++ src/lib/runway_takeoff/runway_takeoff_params.c | 13 ++++++++++++- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 4 files changed, 28 insertions(+), 3 deletions(-) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 599cb721d0..7b2ab97e8e 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -65,6 +65,7 @@ RunwayTakeoff::RunwayTakeoff() : _runway_takeoff_nav_alt(this, "NAV_ALT"), _runway_takeoff_throttle(this, "MAX_THR"), _runway_takeoff_pitch_sp(this, "PSP"), + _runway_takeoff_max_pitch(this, "MAX_PITCH"), _airspeed_min(this, "FW_AIRSPD_MIN", false), _climbout_diff(this, "FW_CLMBOUT_DIFF", false) { @@ -83,6 +84,7 @@ void RunwayTakeoff::init(float yaw) _initialized = true; _state = RunwayTakeoffState::THROTTLE_RAMP; _initialized_time = hrt_absolute_time(); + _climbout = true; } void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) @@ -98,7 +100,6 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::CLAMPED_TO_RUNWAY: if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { - _climbout = true; _state = RunwayTakeoffState::TAKEOFF; mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); } @@ -218,6 +219,17 @@ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) } } +float RunwayTakeoff::getMaxPitch(float max) +{ + if (_climbout && _runway_takeoff_max_pitch.get() > 0.1f) { + return _runway_takeoff_max_pitch.get(); + } + + else { + return max; + } +} + void RunwayTakeoff::reset() { _initialized = false; diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 265ff24cb3..86b8c01b6e 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -85,6 +85,7 @@ public: bool resetIntegrators(); float getMinAirspeedScaling() { return _min_airspeed_scaling; }; float getMinPitch(float sp_min, float climbout_min, float min); + float getMaxPitch(float max); void reset(); @@ -107,6 +108,7 @@ private: control::BlockParamFloat _runway_takeoff_nav_alt; control::BlockParamFloat _runway_takeoff_throttle; control::BlockParamFloat _runway_takeoff_pitch_sp; + control::BlockParamFloat _runway_takeoff_max_pitch; control::BlockParamFloat _airspeed_min; control::BlockParamFloat _climbout_diff; diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index d3ce5d238d..137732c074 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -88,7 +88,7 @@ PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0); PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); /** - * Pitch setpoint during runway takeoff. + * Pitch setpoint during taxi / before takeoff airspeed is reached. * A taildragger with stearable wheel might need to pitch up * a little to keep it's wheel on the ground before airspeed * to takeoff is reached. @@ -98,3 +98,14 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); + +/** + * Max pitch during takeoff. + * Fixed-wing settings are used if set to 0. Note that there is also a minimum + * pitch of 10 degrees during takeoff, so this must be larger if set. + * + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ed71cf2955..711e888416 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1382,7 +1382,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _mavlink_fd); // update tecs - float takeoff_pitch_max_deg = _parameters.pitch_limit_max; + float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max); float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, From 3eb0ce84df03c8abfbc1634a3183e4bc5eb6c5c3 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 25 Sep 2015 15:42:43 +0200 Subject: [PATCH 071/136] set disarmed/min/max pwm for throttle channel in default fw configs --- ROMFS/px4fmu_common/init.d/2101_fw_AERT | 4 +++- ROMFS/px4fmu_common/init.d/2104_fw_AETR | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/2101_fw_AERT b/ROMFS/px4fmu_common/init.d/2101_fw_AERT index 3631a1e36c..c5bb1477c5 100644 --- a/ROMFS/px4fmu_common/init.d/2101_fw_AERT +++ b/ROMFS/px4fmu_common/init.d/2101_fw_AERT @@ -21,6 +21,8 @@ sh /etc/init.d/rc.fw_defaults set MIXER AERT -# The ESC requires a specific pulse to arm. +# use PWM parameters for throttle channel set PWM_OUT 4 set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/init.d/2104_fw_AETR b/ROMFS/px4fmu_common/init.d/2104_fw_AETR index 3113ede87a..f533ce66fe 100644 --- a/ROMFS/px4fmu_common/init.d/2104_fw_AETR +++ b/ROMFS/px4fmu_common/init.d/2104_fw_AETR @@ -21,6 +21,8 @@ sh /etc/init.d/rc.fw_defaults set MIXER AETR -# The ESC requires a specific pulse to arm. +# use PWM parameters for throttle channel set PWM_OUT 3 set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX From ff57c809b87c1f0e234071c442e00f2ebae6d13a Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 25 Sep 2015 17:45:09 +0200 Subject: [PATCH 072/136] updated default wheel params after first test --- src/modules/fw_att_control/fw_att_control_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index d5fdcd5e01..f4cfa7fd8d 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_WR_P, 0.05f); +PARAM_DEFINE_FLOAT(FW_WR_P, 0.3f); /** * Wheel steering rate integrator gain @@ -242,7 +242,7 @@ PARAM_DEFINE_FLOAT(FW_WR_P, 0.05f); * @max 50.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_WR_I, 0.0f); +PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f); /** * Wheel steering rate integrator limit From d015fbd6782e942bb8f0a3afe0aa7772093ed021 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 2 Oct 2015 11:50:18 +0200 Subject: [PATCH 073/136] added startup config for Maja and new generic mixer that uses channel 5 for wheel steering --- ROMFS/px4fmu_common/init.d/2101_fw_AERT | 4 +- ROMFS/px4fmu_common/init.d/2105_maja | 48 ++++++++++ ROMFS/px4fmu_common/mixers/AERTW.main.mix | 95 +++++++++++++++++++ .../fw_pos_control_l1_main.cpp | 1 + 4 files changed, 146 insertions(+), 2 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/2105_maja create mode 100644 ROMFS/px4fmu_common/mixers/AERTW.main.mix diff --git a/ROMFS/px4fmu_common/init.d/2101_fw_AERT b/ROMFS/px4fmu_common/init.d/2101_fw_AERT index c5bb1477c5..d13514d03a 100644 --- a/ROMFS/px4fmu_common/init.d/2101_fw_AERT +++ b/ROMFS/px4fmu_common/init.d/2101_fw_AERT @@ -6,8 +6,8 @@ # # @output MAIN1 aileron # @output MAIN2 elevator -# @output MAIN4 rudder -# @output MAIN3 throttle +# @output MAIN3 rudder +# @output MAIN4 throttle # @output MAIN5 flaps # # @output AUX1 feed-through of RC AUX1 channel diff --git a/ROMFS/px4fmu_common/init.d/2105_maja b/ROMFS/px4fmu_common/init.d/2105_maja new file mode 100644 index 0000000000..af27eca5d9 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2105_maja @@ -0,0 +1,48 @@ +#!nsh +# +# @name Bormatec Maja +# +# @type Standard Plane +# +# @output MAIN1 aileron +# @output MAIN2 elevator +# @output MAIN3 rudder +# @output MAIN4 throttle +# @output MAIN5 wheel +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Andreas Antener +# + +sh /etc/init.d/rc.fw_defaults + +if [ $AUTOCNF == yes ] +then + param set FW_AIRSPD_MIN 10 + param set FW_AIRSPD_TRIM 15 + param set FW_AIRSPD_MAX 20 + + param set FW_MAN_P_MAX 55 + param set FW_MAN_R_MAX 55 + param set FW_R_LIM 55 + + param set FW_WR_FF 0.2 + param set FW_WR_I 0.2 + param set FW_WR_IMAX 0.8 + param set FW_WR_P 1 + param set FW_W_RMAX 0 + + # set disarmed value for the ESC + param set PWM_DISARMED 1000 +fi + +set MIXER AERTW + +# use PWM parameters for throttle channel +set PWM_OUT 4 +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/mixers/AERTW.main.mix b/ROMFS/px4fmu_common/mixers/AERTW.main.mix new file mode 100644 index 0000000000..af548826dc --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/AERTW.main.mix @@ -0,0 +1,95 @@ +Aileron/rudder/elevator/throttle/wheel mixer for PX4FMU +======================================================= + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, rudder, elevator, throttle and steerable wheel controls using PX4FMU. +The configuration assumes the aileron servo(s) are connected to PX4FMU servo +output 0, the elevator to output 1, the rudder to output 2, the throttle +to output 3 and the wheel to output 4. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch), 2 (yaw) and 3 (thrust). + +Aileron mixer +------------- +Two scalers total (output, roll). + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +As there is only one output, if using two servos adjustments to compensate for +differences between the servos must be made mechanically. To obtain the correct +motion using a Y cable, the servos can be positioned reversed from one another. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Rudder mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the rudder servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Wheel mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the wheel servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + + +Gimbal / flaps / payload mixer for last three channels, +using the payload control group +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 2 10000 10000 0 -10000 10000 diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 711e888416..b9f24a6eb2 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -49,6 +49,7 @@ * * @author Lorenz Meier * @author Thomas Gubler + * @author Andreas Antener */ #include From 9c70eb0b63e1079f1fd0fcf8714cbd2fd138baed Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 2 Oct 2015 16:25:14 +0200 Subject: [PATCH 074/136] reduce wheel control speed scaling --- src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index 12d2668f87..d67a46bb1c 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -85,7 +85,8 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da speed = ctl_data.ground_speed; } - float scaler = 1.0f / speed; + /* only scale a certain amount with speed else the corrections get to small */ + float scaler = 0.7f + 0.3f / speed; /* Calculate body angular rate error */ _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error From f5f61e42af0530142d41a8186f9fa71e6b87be6a Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 7 Oct 2015 10:12:35 +0200 Subject: [PATCH 075/136] replaced magic values with parameters, renamed internal representations --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 44 +++++++++---------- src/lib/runway_takeoff/RunwayTakeoff.h | 18 ++++---- .../runway_takeoff/runway_takeoff_params.c | 33 ++++++++++++++ 3 files changed, 63 insertions(+), 32 deletions(-) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 7b2ab97e8e..c04ad780f2 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -58,14 +58,14 @@ RunwayTakeoff::RunwayTakeoff() : _initialized_time(0), _init_yaw(0), _climbout(false), - _min_airspeed_scaling(1.3f), - _max_takeoff_roll(15.0f), _runway_takeoff_enabled(this, "TKOFF"), - _runway_takeoff_heading(this, "HDG"), - _runway_takeoff_nav_alt(this, "NAV_ALT"), - _runway_takeoff_throttle(this, "MAX_THR"), - _runway_takeoff_pitch_sp(this, "PSP"), - _runway_takeoff_max_pitch(this, "MAX_PITCH"), + _heading_mode(this, "HDG"), + _nav_alt(this, "NAV_ALT"), + _takeoff_throttle(this, "MAX_THR"), + _runway_pitch_sp(this, "PSP"), + _max_takeoff_pitch(this, "MAX_PITCH"), + _max_takeoff_roll(this, "MAX_ROLL"), + _min_airspeed_scaling(this, "AIRSPD_SCL"), _airspeed_min(this, "FW_AIRSPD_MIN", false), _climbout_diff(this, "FW_CLMBOUT_DIFF", false) { @@ -99,7 +99,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) break; case RunwayTakeoffState::CLAMPED_TO_RUNWAY: - if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { + if (airspeed > _airspeed_min.get() * _min_airspeed_scaling.get()) { _state = RunwayTakeoffState::TAKEOFF; mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached"); } @@ -107,7 +107,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) break; case RunwayTakeoffState::TAKEOFF: - if (alt_agl > _runway_takeoff_nav_alt.get()) { + if (alt_agl > _nav_alt.get()) { _state = RunwayTakeoffState::CLIMBOUT; mavlink_log_info(mavlink_fd, "#Climbout"); } @@ -137,7 +137,7 @@ bool RunwayTakeoff::controlYaw() float RunwayTakeoff::getPitch(float tecsPitch) { if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { - return math::radians(_runway_takeoff_pitch_sp.get()); + return math::radians(_runway_pitch_sp.get()); } return tecsPitch; @@ -152,14 +152,14 @@ float RunwayTakeoff::getRoll(float navigatorRoll) // allow some roll during climbout if waypoint heading is targeted else if (_state < RunwayTakeoffState::FLY) { - if (_runway_takeoff_heading.get() == 0) { + if (_heading_mode.get() == 0) { // otherwise stay at 0 roll return 0.0f; - } else if (_runway_takeoff_heading.get() == 1) { + } else if (_heading_mode.get() == 1) { return math::constrain(navigatorRoll, - math::radians(-_max_takeoff_roll), - math::radians(_max_takeoff_roll)); + math::radians(-_max_takeoff_roll.get()), + math::radians(_max_takeoff_roll).get()); } } @@ -169,11 +169,11 @@ float RunwayTakeoff::getRoll(float navigatorRoll) float RunwayTakeoff::getYaw(float navigatorYaw) { if (_state < RunwayTakeoffState::FLY) { - if (_runway_takeoff_heading.get() == 0) { + if (_heading_mode.get() == 0) { // fix heading in the direction the airframe points return _init_yaw; - } else if (_runway_takeoff_heading.get() == 1) { + } else if (_heading_mode.get() == 1) { // or head into the direction of the takeoff waypoint // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) return navigatorYaw; @@ -188,14 +188,14 @@ float RunwayTakeoff::getThrottle(float tecsThrottle) switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: { float throttle = hrt_elapsed_time(&_initialized_time) / (float)2000000 * - _runway_takeoff_throttle.get(); - return throttle < _runway_takeoff_throttle.get() ? + _takeoff_throttle.get(); + return throttle < _takeoff_throttle.get() ? throttle : - _runway_takeoff_throttle.get(); + _takeoff_throttle.get(); } case RunwayTakeoffState::CLAMPED_TO_RUNWAY: - return _runway_takeoff_throttle.get(); + return _takeoff_throttle.get(); default: return tecsThrottle; @@ -221,8 +221,8 @@ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) float RunwayTakeoff::getMaxPitch(float max) { - if (_climbout && _runway_takeoff_max_pitch.get() > 0.1f) { - return _runway_takeoff_max_pitch.get(); + if (_climbout && _max_takeoff_pitch.get() > 0.1f) { + return _max_takeoff_pitch.get(); } else { diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 86b8c01b6e..8e3cd35528 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -83,7 +83,7 @@ public: float getYaw(float navigatorYaw); float getThrottle(float tecsThrottle); bool resetIntegrators(); - float getMinAirspeedScaling() { return _min_airspeed_scaling; }; + float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; float getMinPitch(float sp_min, float climbout_min, float min); float getMaxPitch(float max); @@ -98,17 +98,15 @@ private: float _init_yaw; bool _climbout; - /** magic values **/ - float _min_airspeed_scaling; - float _max_takeoff_roll; - /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; - control::BlockParamInt _runway_takeoff_heading; - control::BlockParamFloat _runway_takeoff_nav_alt; - control::BlockParamFloat _runway_takeoff_throttle; - control::BlockParamFloat _runway_takeoff_pitch_sp; - control::BlockParamFloat _runway_takeoff_max_pitch; + control::BlockParamInt _heading_mode; + control::BlockParamFloat _nav_alt; + control::BlockParamFloat _takeoff_throttle; + control::BlockParamFloat _runway_pitch_sp; + control::BlockParamFloat _max_takeoff_pitch; + control::BlockParamFloat _max_takeoff_roll; + control::BlockParamFloat _min_airspeed_scaling; control::BlockParamFloat _airspeed_min; control::BlockParamFloat _climbout_diff; diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index 137732c074..05110dddc2 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -109,3 +109,36 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); + +/** + * Max pitch during takeoff. + * Fixed-wing settings are used if set to 0. Note that there is also a minimum + * pitch of 10 degrees during takeoff, so this must be larger if set. + * + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); + +/** + * Max roll during climbout. + * Roll is limited during climbout to ensure enough lift and prevents aggressive + * navigation before we're on a safe height. + * + * @min 0.0 + * @max 60.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0); + +/** + * Min. airspeed scaling factor for takeoff. + * Pitch up will be commanded when the following airspeed is reached: + * FW_AIRSPD_MIN * RWTO_AIRSPD_SCL + * + * @min 0.0 + * @max 2.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3); From e0cdf65fb446bc40ff032ea88b70ebd3f6a6d1ab Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 8 Oct 2015 22:23:18 +0200 Subject: [PATCH 076/136] use navigator to hold heading --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 53 +++++++++++-------- src/lib/runway_takeoff/RunwayTakeoff.h | 10 +++- .../runway_takeoff/runway_takeoff_params.c | 11 ---- .../fw_pos_control_l1_main.cpp | 26 +++++++-- 4 files changed, 63 insertions(+), 37 deletions(-) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index c04ad780f2..16cbb04e45 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -47,6 +47,7 @@ #include #include #include +#include namespace runwaytakeoff { @@ -58,6 +59,8 @@ RunwayTakeoff::RunwayTakeoff() : _initialized_time(0), _init_yaw(0), _climbout(false), + _start_sp{}, + _target_sp{}, _runway_takeoff_enabled(this, "TKOFF"), _heading_mode(this, "HDG"), _nav_alt(this, "NAV_ALT"), @@ -150,17 +153,11 @@ float RunwayTakeoff::getRoll(float navigatorRoll) return 0.0f; } - // allow some roll during climbout if waypoint heading is targeted + // allow some roll during climbout else if (_state < RunwayTakeoffState::FLY) { - if (_heading_mode.get() == 0) { - // otherwise stay at 0 roll - return 0.0f; - - } else if (_heading_mode.get() == 1) { - return math::constrain(navigatorRoll, - math::radians(-_max_takeoff_roll.get()), - math::radians(_max_takeoff_roll).get()); - } + return math::constrain(navigatorRoll, + math::radians(-_max_takeoff_roll.get()), + math::radians(_max_takeoff_roll.get())); } return navigatorRoll; @@ -168,18 +165,6 @@ float RunwayTakeoff::getRoll(float navigatorRoll) float RunwayTakeoff::getYaw(float navigatorYaw) { - if (_state < RunwayTakeoffState::FLY) { - if (_heading_mode.get() == 0) { - // fix heading in the direction the airframe points - return _init_yaw; - - } else if (_heading_mode.get() == 1) { - // or head into the direction of the takeoff waypoint - // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) - return navigatorYaw; - } - } - return navigatorYaw; } @@ -230,6 +215,30 @@ float RunwayTakeoff::getMaxPitch(float max) } } +math::Vector<2> RunwayTakeoff::getPrevWP() +{ + math::Vector<2> prev_wp; + prev_wp(0) = (float)_start_sp.lat; + prev_wp(1) = (float)_start_sp.lon; + return prev_wp; +} + +math::Vector<2> RunwayTakeoff::getCurrWP(math::Vector<2> sp_curr_wp) +{ + if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::FLY) { + // navigating towards calculated direction if heading mode 0 and as long as we're in climbout + math::Vector<2> curr_wp; + curr_wp(0) = (float)_target_sp.lat; + curr_wp(1) = (float)_target_sp.lon; + return curr_wp; + + } else { + // navigating towards next mission waypoint + return sp_curr_wp; + } + +} + void RunwayTakeoff::reset() { _initialized = false; diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 8e3cd35528..2913b2ad64 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -50,6 +50,7 @@ #include #include #include +#include namespace runwaytakeoff { @@ -75,6 +76,10 @@ public: bool isInitialized() { return _initialized; }; bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; + float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; + position_setpoint_s *getStartSP() { return &_start_sp; }; + position_setpoint_s *getTargetSP() { return &_target_sp; }; + float getInitYaw() { return _init_yaw; }; bool controlYaw(); bool climbout() { return _climbout; }; @@ -83,9 +88,10 @@ public: float getYaw(float navigatorYaw); float getThrottle(float tecsThrottle); bool resetIntegrators(); - float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; float getMinPitch(float sp_min, float climbout_min, float min); float getMaxPitch(float max); + math::Vector<2> getPrevWP(); + math::Vector<2> getCurrWP(math::Vector<2> sp_curr_wp); void reset(); @@ -97,6 +103,8 @@ private: hrt_abstime _initialized_time; float _init_yaw; bool _climbout; + struct position_setpoint_s _start_sp; + struct position_setpoint_s _target_sp; /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index 05110dddc2..7c64889e76 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -110,17 +110,6 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); */ PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); -/** - * Max pitch during takeoff. - * Fixed-wing settings are used if set to 0. Note that there is also a minimum - * pitch of 10 degrees during takeoff, so this must be larger if set. - * - * @min 0.0 - * @max 60.0 - * @group Runway Takeoff - */ -PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); - /** * Max roll during climbout. * Roll is limited during climbout to ensure enough lift and prevents aggressive diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index b9f24a6eb2..176d4ed11b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1368,12 +1368,32 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ _takeoff_ground_alt = _global_pos.alt; + + // draw a line from takeoff location into the direction the UAV is heading + get_waypoint_heading_distance( + _runway_takeoff.getInitYaw(), + HDG_HOLD_DIST_NEXT, + *_runway_takeoff.getStartSP(), + *_runway_takeoff.getTargetSP(), + true); + mavlink_log_info(_mavlink_fd, "#Takeoff on runway"); } - // update navigation - // FIXME: after reselecting the takeoff wp, prev_wp is set to the next wp which causes wrong navigation behaviour - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + // update takeoff path if we're reaching the end of it + if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, + _runway_takeoff.getTargetSP()->lat, _runway_takeoff.getTargetSP()->lon) < HDG_HOLD_REACHED_DIST) { + get_waypoint_heading_distance( + _runway_takeoff.getInitYaw(), + HDG_HOLD_DIST_NEXT, + *_runway_takeoff.getStartSP(), + *_runway_takeoff.getTargetSP(), + false); + } + + /* update navigation: _runway_takeoff decides if we target the current WP from setpoint triplet + * or the calculated one through initial heading */ + _l1_control.navigate_waypoints(_runway_takeoff.getPrevWP(), _runway_takeoff.getCurrWP(curr_wp), current_position, ground_speed_2d); float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); // update runway takeoff helper From 3b865624f1b302c256bf628de9013043d2df0a18 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 18 Sep 2015 08:42:08 +0200 Subject: [PATCH 077/136] added library for terrain_estimation Conflicts: makefiles/nuttx/config_aerocore_default.mk makefiles/nuttx/config_px4fmu-v1_default.mk makefiles/nuttx/config_px4fmu-v2_default.mk makefiles/nuttx/config_px4fmu-v2_multiplatform.mk makefiles/posix/config_posix_sitl.mk --- src/lib/terrain_estimation/module.mk | 36 ++++ .../terrain_estimation/terrain_estimator.cpp | 188 ++++++++++++++++++ .../terrain_estimation/terrain_estimator.h | 96 +++++++++ .../AttitudePositionEstimatorEKF.h | 3 + .../ekf_att_pos_estimator_main.cpp | 16 +- 5 files changed, 336 insertions(+), 3 deletions(-) create mode 100644 src/lib/terrain_estimation/module.mk create mode 100644 src/lib/terrain_estimation/terrain_estimator.cpp create mode 100644 src/lib/terrain_estimation/terrain_estimator.h diff --git a/src/lib/terrain_estimation/module.mk b/src/lib/terrain_estimation/module.mk new file mode 100644 index 0000000000..3a2047012a --- /dev/null +++ b/src/lib/terrain_estimation/module.mk @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2015 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. +# +############################################################################ + +SRCS = terrain_estimator.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp new file mode 100644 index 0000000000..f522e42047 --- /dev/null +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Roman Bapst. 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 terrain_estimator.cpp + * A terrain estimation kalman filter. + */ + +#include "terrain_estimator.h" + +TerrainEstimator::TerrainEstimator() : + _distance_last(0.0f), + _terrain_valid(false), + _time_last_distance(0), + _time_last_gps(0) +{ + _x.zero(); + _u_z = 0.0f; + _P.identity(); +} + +bool TerrainEstimator::is_distance_valid(float distance) { + if (distance > 40.0f || distance < 0.00001f) { + return false; + } else { + return true; + } +} + +void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, + const struct distance_sensor_s *distance) +{ + if (attitude->R_valid) { + math::Matrix<3, 3> R_att(attitude->R); + math::Vector<3> a(&sensor->accelerometer_m_s2[0]); + math::Vector<3> u; + u = R_att * a; + _u_z = u(2) + 9.81f; // compensate for gravity + + } else { + _u_z = 0.0f; + } + + // dynamics matrix + math::Matrix A; + A.zero(); + A(0,1) = 1; + A(1,2) = 1; + + // input matrix + math::Matrix B; + B.zero(); + B(1,0) = 1; + + // input noise variance + float R = 0.135f; + + // process noise convariance + math::Matrix Q; + Q(0,0) = 0; + Q(1,1) = 0; + + // do prediction + math::Vector dx = (A * _x) * dt; + dx(1) += B(1,0) * _u_z * dt; + + // propagate state and covariance matrix + _x += dx; + _P += (A * _P + _P * A.transposed() + + B * R * B.transposed() + Q) * dt; +} + +void TerrainEstimator::measurement_update(const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, + const struct vehicle_attitude_s *attitude) +{ + if (distance->timestamp > _time_last_distance) { + + float d = distance->current_distance; + + math::Matrix<1, n_x> C; + C(0, 0) = -1; // measured altitude, + + float R = 0.009f; + + math::Vector<1> y; + y(0) = d * cosf(attitude->roll) * cosf(attitude->pitch); + + // residual + math::Matrix<1, 1> S_I = (C * _P * C.transposed()); + S_I(0,0) += R; + S_I = S_I.inversed(); + math::Vector<1> r = y - C * _x; + + math::Matrix K = _P * C.transposed() * S_I; + + // some sort of outlayer rejection + if (fabsf(distance->current_distance - _distance_last) < 1.0f) { + _x += K * r; + _P -= K * C * _P; + } + + // if the current and the last range measurement are bad then we consider the terrain + // estimate to be invalid + if (!is_distance_valid(distance->current_distance) && !is_distance_valid(_distance_last)) { + _terrain_valid = false; + } else { + _terrain_valid = true; + } + + _time_last_distance = distance->timestamp; + _distance_last = distance->current_distance; + } + + if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) { + math::Matrix<1, n_x> C; + C(0,1) = 1; + + float R = 0.056f; + + math::Vector<1> y; + y(0) = gps->vel_d_m_s; + + // residual + math::Matrix<1, 1> S_I = (C * _P * C.transposed()); + S_I(0,0) += R; + S_I = S_I.inversed(); + math::Vector<1> r = y - C * _x; + + math::Matrix K = _P * C.transposed() * S_I; + _x += K * r; + _P -= K * C * _P; + + _time_last_gps = gps->timestamp_position; + } + + // reinitialise filter if we find bad data + bool reinit = false; + for (int i = 0; i < n_x; i++) { + if (!PX4_ISFINITE(_x(i))) { + reinit = true; + } + } + + for (int i = 0; i < n_x; i++) { + for (int j = 0; j < n_x; j++) { + if (!PX4_ISFINITE(_P(i,j))) { + reinit = true; + } + } + } + + if (reinit) { + _x.zero(); + _P.zero(); + _P(0,0) = _P(1,1) = _P(2,2) = 0.1f; + } + +} diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h new file mode 100644 index 0000000000..981e621236 --- /dev/null +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Roman Bapst. 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 terrain_estimator.h + */ + +#include +#include +#include +#include +#include + + +/* +* This class can be used to estimate distance to the ground using a laser range finder. +* It's assumed that the laser points down vertically if the vehicle is in it's neutral pose. +* The predict(...) function will do a state prediciton based on accelerometer inputs. It also +* considers accelerometer bias. +* The measurement_update(...) function does a measurement update based on range finder and gps +* velocity measurements. Both functions should always be called together when there is new +* acceleration data available. +* The is_valid() function provides information wether the estimate is valid. +*/ + +class __EXPORT TerrainEstimator +{ +public: + TerrainEstimator(); + ~TerrainEstimator(); + + bool is_valid() {return _terrain_valid;} + float get_distance_to_ground() {return -_x(0);} + float get_velocity() {return _x(1);}; + + void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, + const struct distance_sensor_s *distance); + void measurement_update(const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, + const struct vehicle_attitude_s *attitude); + +private: + enum {n_x=3}; + + float _distance_last; + bool _terrain_valid; + + // kalman filter variables + math::Vector _x; // state: ground distance, velocity, accel bias in z direction + float _u_z; // acceleration in earth z direction + math::Matrix<3, 3> _P; // covariance matrix + + // timestamps + uint64_t _time_last_distance; + uint64_t _time_last_gps; + + struct { + float var_acc_z; + float var_p_z; + float var_p_vz; + float var_lidar; + float var_gps_vz; + } _params; + + bool is_distance_valid(float distance); + +}; \ No newline at end of file diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 1429979d6d..fc11e0f505 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -69,6 +69,7 @@ #include #include +#include #include #include #include "estimator_22states.h" @@ -278,6 +279,8 @@ private: AttPosEKF *_ekf; + TerrainEstimator *_terrain_estimator; + /* Low pass filter for attitude rates */ math::LowPassFilter2p _LP_att_P; math::LowPassFilter2p _LP_att_Q; 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 f4fcf78e25..2193b3e945 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 @@ -212,6 +212,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _parameters{}, _parameter_handles{}, _ekf(nullptr), + _terrain_estimator(nullptr), _LP_att_P(250.0f, 20.0f), _LP_att_Q(250.0f, 20.0f), @@ -219,6 +220,8 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : { _voter_mag.set_timeout(200000); + _terrain_estimator = new TerrainEstimator(); + _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); _parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS"); @@ -697,6 +700,10 @@ void AttitudePositionEstimatorEKF::task_main() // Run EKF data fusion steps updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData); + // Run separate terrain estimator + _terrain_estimator->predict(_ekf->dtIMU, &_att, &_sensor_combined, &_distance); + _terrain_estimator->measurement_update(&_gps, &_distance, &_att); + // Publish attitude estimations publishAttitude(); @@ -997,9 +1004,12 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } /* terrain altitude */ - _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; - _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && - (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); + if (_terrain_estimator->is_valid()) { + _global_pos.terrain_alt = _global_pos.alt - _terrain_estimator->get_distance_to_ground(); + _global_pos.terrain_alt_valid = true; + } else { + _global_pos.terrain_alt_valid = false; + } _global_pos.yaw = _local_pos.yaw; From b16e6249e4189e077ce9d82973897a7698842181 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 11 Oct 2015 16:52:09 +0200 Subject: [PATCH 078/136] more correct groundspeed scaling for wheel controller --- src/lib/ecl/attitude_fw/ecl_controller.h | 3 ++- .../ecl/attitude_fw/ecl_wheel_controller.cpp | 19 +++++++------------ .../fw_att_control/fw_att_control_main.cpp | 13 ++++++++++--- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h index 0817204ccc..fed7d1d8fb 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_controller.h @@ -75,7 +75,8 @@ struct ECL_ControlData { float airspeed_max; float airspeed; float scaler; - float ground_speed; + float groundspeed; + float groundspeed_scaler; bool lock_integrator; }; diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index d67a46bb1c..76419ef6f7 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -60,7 +60,8 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da { /* Do not calculate control signal with bad inputs */ if (!(PX4_ISFINITE(ctl_data.yaw_rate) && - PX4_ISFINITE(ctl_data.ground_speed))) { + PX4_ISFINITE(ctl_data.groundspeed) && + PX4_ISFINITE(ctl_data.groundspeed_scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -79,21 +80,13 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da /* input conditioning */ float min_speed = 1.0f; - /* assume minimum speed to prevent oscillations */ - float speed = min_speed; - if (ctl_data.ground_speed > speed) { - speed = ctl_data.ground_speed; - } - - /* only scale a certain amount with speed else the corrections get to small */ - float scaler = 0.7f + 0.3f / speed; /* Calculate body angular rate error */ _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error - if (!lock_integrator && _k_i > 0.0f && speed > min_speed) { + if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) { - float id = _rate_error * dt * scaler; + float id = _rate_error * dt * ctl_data.groundspeed_scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -115,7 +108,9 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_rate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler; + _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + + _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler + + integrator_constrained; /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, speed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)speed, (double)_k_i, (double)_k_p);*/ diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index da91fda9f7..271dc86079 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -837,9 +837,16 @@ FixedwingAttitudeControl::task_main() * * Forcing the scaling to this value allows reasonable handheld tests. */ - float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); + /* Use min airspeed to calculate ground speed scaling region. + * Don't scale below gspd_scaling_trim + */ + float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + + _global_pos.vel_e * _global_pos.vel_e); + float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f); + float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed); + float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; float yaw_sp = 0.0f; @@ -995,8 +1002,8 @@ FixedwingAttitudeControl::task_main() control_input.airspeed = airspeed; control_input.scaler = airspeed_scaling; control_input.lock_integrator = lock_integrator; - control_input.ground_speed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + - _global_pos.vel_e * _global_pos.vel_e); + control_input.groundspeed = groundspeed; + control_input.groundspeed_scaler = groundspeed_scaler; _yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method); From 62122201138e369459fada1c2aa957a5465c142b Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 13 Oct 2015 22:46:00 +0200 Subject: [PATCH 079/136] replaced aileron mixer with flaperon mixer --- ROMFS/px4fmu_common/mixers/AERTW.main.mix | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/AERTW.main.mix b/ROMFS/px4fmu_common/mixers/AERTW.main.mix index af548826dc..a28dd9ec6e 100644 --- a/ROMFS/px4fmu_common/mixers/AERTW.main.mix +++ b/ROMFS/px4fmu_common/mixers/AERTW.main.mix @@ -10,22 +10,23 @@ to output 3 and the wheel to output 4. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (roll), 1 (pitch), 2 (yaw) and 3 (thrust). -Aileron mixer -------------- -Two scalers total (output, roll). +Flaperon mixer (ailerons + flaps) +--------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; depending on the actual configuration it may be necessary to reverse the scaling factors (to reverse the servo movement) and adjust the offset, scaling and endpoints to suit. -As there is only one output, if using two servos adjustments to compensate for -differences between the servos must be made mechanically. To obtain the correct -motion using a Y cable, the servos can be positioned reversed from one another. - -M: 1 +M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 -10000 -10000 0 -10000 10000 Elevator mixer ------------ From 0043c40b463c1e05703fcdadcbd5aba6cc535af6 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 13 Oct 2015 22:46:51 +0200 Subject: [PATCH 080/136] added more indexing variables --- msg/actuator_controls.msg | 3 +++ 1 file changed, 3 insertions(+) diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index 66e12325d2..a6ebda6aae 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -5,6 +5,9 @@ uint8 INDEX_PITCH = 1 uint8 INDEX_YAW = 2 uint8 INDEX_THROTTLE = 3 uint8 INDEX_FLAPS = 4 +uint8 INDEX_SPOILERS = 5 +uint8 INDEX_AIRBRAKES = 6 +uint8 INDEX_LANDING_GEAR = 7 uint8 GROUP_INDEX_ATTITUDE = 0 uint64 timestamp uint64 timestamp_sample # the timestamp the data this control response is based on was sampled From 18d9c061ba5b78b2c96ac78bd07b1e0ec5cfa06c Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 13 Oct 2015 22:47:26 +0200 Subject: [PATCH 081/136] added flag for applying flaps --- msg/vehicle_attitude_setpoint.msg | 2 ++ 1 file changed, 2 insertions(+) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 3bbb5efa7b..43ea237b47 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -23,3 +23,5 @@ bool pitch_reset_integral # Reset pitch integral part (navigation logic change bool yaw_reset_integral # Reset yaw integral part (navigation logic change) bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) + +bool apply_flaps From 7fc97ed147222668d996815e0feab887f9abf7ed Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 13 Oct 2015 22:47:50 +0200 Subject: [PATCH 082/136] implemented use of flaps for auto landings --- .../fw_att_control/fw_att_control_main.cpp | 30 +++++++++++++++++-- .../fw_att_control/fw_att_control_params.c | 18 +++++++++++ .../fw_pos_control_l1_main.cpp | 6 +++- 3 files changed, 50 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 271dc86079..579096268e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -202,6 +202,9 @@ private: float man_roll_max; /**< Max Roll in rad */ float man_pitch_max; /**< Max Pitch in rad */ + float flaps_scale; /**< Scale factor for flaps */ + float flaperon_scale; /**< Scale factor for flaperons */ + int vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ } _parameters; /**< local copies of interesting parameters */ @@ -246,6 +249,9 @@ private: param_t man_roll_max; param_t man_pitch_max; + param_t flaps_scale; + param_t flaperon_scale; + param_t vtol_type; } _parameter_handles; /**< handles for interesting parameters */ @@ -414,6 +420,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); + _parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL"); + _parameter_handles.flaperon_scale = param_find("FW_FLAPERONS_SCL"); + _parameter_handles.vtol_type = param_find("VT_TYPE"); /* fetch initial parameter values */ @@ -498,6 +507,9 @@ FixedwingAttitudeControl::parameters_update() _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); + param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale); + param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale); + param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); /* pitch control parameters */ @@ -810,8 +822,20 @@ FixedwingAttitudeControl::task_main() float flaps_control = 0.0f; /* map flaps by default to manual if valid */ - if (PX4_ISFINITE(_manual.flaps)) { - flaps_control = _manual.flaps; + if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) { + flaps_control = _manual.flaps * _parameters.flaps_scale; + } else if (_vcontrol_mode.flag_control_auto_enabled) { + flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f; + } + + /* default flaperon to center */ + float flaperon = 0.0f; + + /* map flaperons by default to manual if valid */ + if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) { + flaperon = _manual.aux2 * _parameters.flaperon_scale; + } else if (_vcontrol_mode.flag_control_auto_enabled) { + flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f; } /* decide if in stabilized or full manual control */ @@ -1119,7 +1143,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control; _actuators.control[5] = _manual.aux1; - _actuators.control[6] = _manual.aux2; + _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon; _actuators.control[7] = _manual.aux3; /* lazily publish the setpoint only once available */ diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index f4cfa7fd8d..41ca2edf44 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -434,3 +434,21 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); + +/** + * Scale factor for flaps + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPS_SCL, 1.0f); + +/** + * Scale factor for flaperons + * + * @min 0.0 + * @max 1.0 + * @group FW Attitude Control + */ +PARAM_DEFINE_FLOAT(FW_FLAPERON_SCL, 0.0f); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 176d4ed11b..0727c718e0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1076,7 +1076,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder - + _att_sp.apply_flaps = false; // by default we don't use flaps float eas2tas = 1.0f; // XXX calculate actual number based on current measurements /* filter speed and altitude for controller */ @@ -1207,6 +1207,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + // apply full flaps for landings. this flag will also trigger the use of flaperons + // if they have been enabled using the corresponding parameter + _att_sp.apply_flaps = true; + float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); From 42d03cb07695d832f06abeda448212714fa35dfe Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 13 Oct 2015 22:59:35 +0200 Subject: [PATCH 083/136] activate wheel controller as soon as plane flares --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 0727c718e0..8d8409445d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1291,6 +1291,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; + /* enable direct yaw control using rudder/wheel */ + _att_sp.yaw_body = target_bearing; + _att_sp.fw_control_yaw = true; + if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { From f3e0d91f24ed30b32647a10b30b82f1bda9b29f5 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 14 Oct 2015 07:47:39 +0200 Subject: [PATCH 084/136] added airspeed scale parameter for takeoff and landing --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 ++++++-- .../fw_pos_control_l1/fw_pos_control_l1_params.c | 12 ++++++++++++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 8d8409445d..121eae8af5 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -281,6 +281,7 @@ private: float land_flare_pitch_min_deg; float land_flare_pitch_max_deg; int land_use_terrain_estimate; + float land_airspeed_scale; } _parameters; /**< local copies of interesting parameters */ @@ -329,6 +330,7 @@ private: param_t land_flare_pitch_min_deg; param_t land_flare_pitch_max_deg; param_t land_use_terrain_estimate; + param_t land_airspeed_scale; } _parameter_handles; /**< handles for interesting parameters */ @@ -572,6 +574,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN"); _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); + _parameter_handles.land_airspeed_scale = param_find("FW_AIRSPD_SCALE"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -675,6 +678,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg)); param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); + param_get(_parameter_handles.land_airspeed_scale, &(_parameters.land_airspeed_scale)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -1263,8 +1267,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX this could make a great param float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = 1.3f * _parameters.airspeed_min; - float airspeed_approach = 1.3f * _parameters.airspeed_min; + float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min; + float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min; /* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be * equal to _pos_sp_triplet.current.alt */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 5870039c24..14b73038a0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -443,3 +443,15 @@ PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f); * */ PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f); + +/** + * Takeoff and landing airspeed scale factor + * + * Multiplying this factor with the minimum airspeed of the plane + * gives the target airspeed for takeoff and landing approach. + * + * @min 1.0 + * @max 1.5 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_AIRSPD_SCALE, 1.3f); From b30091be00edc5e9994cf12bc08d644d4c0f8d3b Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 14 Oct 2015 11:06:10 +0200 Subject: [PATCH 085/136] minor fixes --- ROMFS/px4fmu_common/init.d/2105_maja | 2 +- ROMFS/px4fmu_common/mixers/AERTW.main.mix | 8 ++++---- src/modules/fw_att_control/fw_att_control_main.cpp | 6 +++--- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/2105_maja b/ROMFS/px4fmu_common/init.d/2105_maja index af27eca5d9..2cbc11c275 100644 --- a/ROMFS/px4fmu_common/init.d/2105_maja +++ b/ROMFS/px4fmu_common/init.d/2105_maja @@ -42,7 +42,7 @@ fi set MIXER AERTW # use PWM parameters for throttle channel -set PWM_OUT 4 +set PWM_OUT 5 set PWM_DISARMED p:PWM_DISARMED set PWM_MIN p:PWM_MIN set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/mixers/AERTW.main.mix b/ROMFS/px4fmu_common/mixers/AERTW.main.mix index a28dd9ec6e..49665c5110 100644 --- a/ROMFS/px4fmu_common/mixers/AERTW.main.mix +++ b/ROMFS/px4fmu_common/mixers/AERTW.main.mix @@ -83,14 +83,14 @@ Gimbal / flaps / payload mixer for last three channels, using the payload control group ----------------------------------------------------- +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + M: 1 O: 10000 10000 0 -10000 10000 S: 2 0 10000 10000 0 -10000 10000 -M: 1 -O: 10000 10000 0 -10000 10000 -S: 2 1 10000 10000 0 -10000 10000 - M: 1 O: 10000 10000 0 -10000 10000 S: 2 2 10000 10000 0 -10000 10000 diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 579096268e..7600aca22b 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -421,7 +421,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); _parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL"); - _parameter_handles.flaperon_scale = param_find("FW_FLAPERONS_SCL"); + _parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL"); _parameter_handles.vtol_type = param_find("VT_TYPE"); @@ -823,7 +823,7 @@ FixedwingAttitudeControl::task_main() /* map flaps by default to manual if valid */ if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) { - flaps_control = _manual.flaps * _parameters.flaps_scale; + flaps_control = 0.5f * (_manual.flaps + 1.0f ) * _parameters.flaps_scale; } else if (_vcontrol_mode.flag_control_auto_enabled) { flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f; } @@ -833,7 +833,7 @@ FixedwingAttitudeControl::task_main() /* map flaperons by default to manual if valid */ if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) { - flaperon = _manual.aux2 * _parameters.flaperon_scale; + flaperon = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale; } else if (_vcontrol_mode.flag_control_auto_enabled) { flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f; } From 36df3a04996e0d6e69d02e8ae59768ce25aab416 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 14 Oct 2015 12:55:44 +0200 Subject: [PATCH 086/136] fixed minor things from review --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 52 +++++++++++++++++++++--- src/lib/runway_takeoff/RunwayTakeoff.h | 2 +- 2 files changed, 47 insertions(+), 7 deletions(-) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 16cbb04e45..90e3601e69 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -47,7 +47,6 @@ #include #include #include -#include namespace runwaytakeoff { @@ -61,6 +60,7 @@ RunwayTakeoff::RunwayTakeoff() : _climbout(false), _start_sp{}, _target_sp{}, + _throttle_ramp_time(2 * 1e6), _runway_takeoff_enabled(this, "TKOFF"), _heading_mode(this, "HDG"), _nav_alt(this, "NAV_ALT"), @@ -87,7 +87,7 @@ void RunwayTakeoff::init(float yaw) _initialized = true; _state = RunwayTakeoffState::THROTTLE_RAMP; _initialized_time = hrt_absolute_time(); - _climbout = true; + _climbout = true; // this is true until climbout is finished } void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) @@ -95,7 +95,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: - if (hrt_elapsed_time(&_initialized_time) > 1e6) { + if (hrt_elapsed_time(&_initialized_time) > _throttle_ramp_time) { _state = RunwayTakeoffState::CLAMPED_TO_RUNWAY; } @@ -131,12 +131,21 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) } } +/* + * Returns true as long as we're below navigation altitude + */ bool RunwayTakeoff::controlYaw() { // keep controlling yaw directly until we start navigation return _state < RunwayTakeoffState::CLIMBOUT; } +/* + * Returns pitch setpoint to use. + * + * Limited (parameter) as long as the plane is on runway. Otherwise + * use the one from TECS + */ float RunwayTakeoff::getPitch(float tecsPitch) { if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { @@ -146,6 +155,9 @@ float RunwayTakeoff::getPitch(float tecsPitch) return tecsPitch; } +/* + * Returns the roll setpoint to use. + */ float RunwayTakeoff::getRoll(float navigatorRoll) { // until we have enough ground clearance, set roll to 0 @@ -163,16 +175,25 @@ float RunwayTakeoff::getRoll(float navigatorRoll) return navigatorRoll; } +/* + * Returns the yaw setpoint to use. + */ float RunwayTakeoff::getYaw(float navigatorYaw) { return navigatorYaw; } +/* + * Returns the throttle setpoint to use. + * + * Ramps up in the beginning, until it lifts off the runway it is set to + * parameter value, then it returns the TECS throttle. + */ float RunwayTakeoff::getThrottle(float tecsThrottle) { switch (_state) { case RunwayTakeoffState::THROTTLE_RAMP: { - float throttle = hrt_elapsed_time(&_initialized_time) / (float)2000000 * + float throttle = hrt_elapsed_time(&_initialized_time) / (float)_throttle_ramp_time * _takeoff_throttle.get(); return throttle < _takeoff_throttle.get() ? throttle : @@ -193,9 +214,16 @@ bool RunwayTakeoff::resetIntegrators() return _state < RunwayTakeoffState::TAKEOFF; } +/* + * Returns the minimum pitch for TECS to use. + * + * In climbout we either want what was set on the waypoint (sp_min) but at least + * the climbtout minimum pitch (parameter). + * Otherwise use the minimum that is enforced generally (parameter). + */ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) { - if (_climbout) { + if (_state < RunwayTakeoffState::FLY) { return math::max(sp_min, climbout_min); } @@ -204,9 +232,15 @@ float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) } } +/* + * Returns the maximum pitch for TECS to use. + * + * Limited by parameter (if set) until climbout is done. + */ float RunwayTakeoff::getMaxPitch(float max) { - if (_climbout && _max_takeoff_pitch.get() > 0.1f) { + // use max pitch from parameter if set (> 0.1) + if (_state < RunwayTakeoffState::FLY && _max_takeoff_pitch.get() > 0.1f) { return _max_takeoff_pitch.get(); } @@ -215,6 +249,9 @@ float RunwayTakeoff::getMaxPitch(float max) } } +/* + * Returns the "previous" (start) WP for navigation. + */ math::Vector<2> RunwayTakeoff::getPrevWP() { math::Vector<2> prev_wp; @@ -223,6 +260,9 @@ math::Vector<2> RunwayTakeoff::getPrevWP() return prev_wp; } +/* + * Returns the "current" (target) WP for navigation. + */ math::Vector<2> RunwayTakeoff::getCurrWP(math::Vector<2> sp_curr_wp) { if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::FLY) { diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 2913b2ad64..cf0ab455d8 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -50,7 +50,6 @@ #include #include #include -#include namespace runwaytakeoff { @@ -105,6 +104,7 @@ private: bool _climbout; struct position_setpoint_s _start_sp; struct position_setpoint_s _target_sp; + unsigned _throttle_ramp_time; /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; From 1ae722159332da0fa0abf1cbd939ea0670e6b2fe Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 14 Oct 2015 13:56:16 +0200 Subject: [PATCH 087/136] make flaps and flaperons continuous --- .../fw_att_control/fw_att_control_main.cpp | 44 +++++++++++++++++-- 1 file changed, 41 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 7600aca22b..3e7a9f9aa8 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -161,6 +161,10 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ bool _debug; /**< if set to true, print debug output */ + float _flaps_cmd_last; + float _flaperons_cmd_last; + + struct { float tconst; float p_p; @@ -363,7 +367,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), /* states */ _setpoint_valid(false), - _debug(false) + _debug(false), + _flaps_cmd_last(0), + _flaperons_cmd_last(0) { /* safely initialize structs */ _ctrl_state = {}; @@ -821,6 +827,8 @@ FixedwingAttitudeControl::task_main() /* default flaps to center */ float flaps_control = 0.0f; + static float delta_flaps = 0; + /* map flaps by default to manual if valid */ if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) { flaps_control = 0.5f * (_manual.flaps + 1.0f ) * _parameters.flaps_scale; @@ -828,9 +836,25 @@ FixedwingAttitudeControl::task_main() flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f; } + // move the actual control value continuous with time + static hrt_abstime t_flaps_changed = 0; + if (fabsf(flaps_control - _flaps_cmd_last) > 0.01f) { + t_flaps_changed = hrt_absolute_time(); + delta_flaps = flaps_control - _flaps_cmd_last; + _flaps_cmd_last = flaps_control; + } + + static float flaps_applied = 0.0f; + + if (fabsf(flaps_applied - flaps_control) > 0.01f) { + flaps_applied = (flaps_control - delta_flaps) + (float)hrt_elapsed_time(&t_flaps_changed) * (delta_flaps) / 1000000; + } + /* default flaperon to center */ float flaperon = 0.0f; + static float delta_flaperon = 0.0f; + /* map flaperons by default to manual if valid */ if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) { flaperon = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale; @@ -838,6 +862,20 @@ FixedwingAttitudeControl::task_main() flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f; } + // move the actual control value continuous with time + static hrt_abstime t_flaperons_changed = 0; + if (fabsf(flaperon - _flaperons_cmd_last) > 0.01f) { + t_flaperons_changed = hrt_absolute_time(); + delta_flaperon = flaperon - _flaperons_cmd_last; + _flaperons_cmd_last = flaperon; + } + + static float flaperon_applied = 0.0f; + + if (fabsf(flaperon_applied - flaperon) > 0.01f) { + flaperon_applied = (flaperon - delta_flaperon) + (float)hrt_elapsed_time(&t_flaperons_changed) * (delta_flaperon) / 1000000; + } + /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_attitude_enabled) { /* scale around tuning airspeed */ @@ -1141,9 +1179,9 @@ FixedwingAttitudeControl::task_main() _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z; } - _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control; + _actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_applied; _actuators.control[5] = _manual.aux1; - _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon; + _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon_applied; _actuators.control[7] = _manual.aux3; /* lazily publish the setpoint only once available */ From 11c6ee2b5a9115b0b0e6727bd6f2df574ad447f3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 14 Oct 2015 17:14:38 +0200 Subject: [PATCH 088/136] make terrain estimate invalid after range sensor timeout --- src/lib/terrain_estimation/terrain_estimator.cpp | 9 ++++++++- src/lib/terrain_estimation/terrain_estimator.h | 2 +- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index f522e42047..16ab8de4a0 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -38,6 +38,8 @@ #include "terrain_estimator.h" +#define DISTANCE_TIMEOUT 100000 // time in usec after which laser is considered dead + TerrainEstimator::TerrainEstimator() : _distance_last(0.0f), _terrain_valid(false), @@ -100,9 +102,14 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu B * R * B.transposed() + Q) * dt; } -void TerrainEstimator::measurement_update(const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, +void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, const struct vehicle_attitude_s *attitude) { + // terrain estimate is invalid if we have range sensor timeout + if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) { + _terrain_valid = false; + } + if (distance->timestamp > _time_last_distance) { float d = distance->current_distance; diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index 981e621236..fc10cb8d63 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -65,7 +65,7 @@ public: void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, const struct distance_sensor_s *distance); - void measurement_update(const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, + void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, const struct vehicle_attitude_s *attitude); private: 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 2193b3e945..e4c8eeccde 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 @@ -702,7 +702,7 @@ void AttitudePositionEstimatorEKF::task_main() // Run separate terrain estimator _terrain_estimator->predict(_ekf->dtIMU, &_att, &_sensor_combined, &_distance); - _terrain_estimator->measurement_update(&_gps, &_distance, &_att); + _terrain_estimator->measurement_update(hrt_absolute_time(), &_gps, &_distance, &_att); // Publish attitude estimations publishAttitude(); From 5949b6615d52fe694f3f2a8824d19838158fd572 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 14 Oct 2015 18:08:35 +0200 Subject: [PATCH 089/136] don't reset the yaw integrator on takeoff --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 121eae8af5..02a61a20e7 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1443,9 +1443,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); + // reset integrals except yaw (which also counts for the wheel controller) _att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators(); _att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators(); - _att_sp.yaw_reset_integral = _runway_takeoff.resetIntegrators(); /*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body, (double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/ From 4e22d65325cc34423e0259402de3a720d71e78f1 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 14 Oct 2015 22:01:09 +0200 Subject: [PATCH 090/136] don't use virtual line anymore for takeoff but use correct starting point to navigate, updated default parameters for wheel controller --- .../ecl/attitude_fw/ecl_wheel_controller.cpp | 4 +- src/lib/runway_takeoff/RunwayTakeoff.cpp | 58 +++++++++---------- src/lib/runway_takeoff/RunwayTakeoff.h | 12 ++-- .../fw_att_control/fw_att_control_params.c | 4 +- .../fw_pos_control_l1_main.cpp | 32 +++------- 5 files changed, 45 insertions(+), 65 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index 76419ef6f7..7f8a9ca6c7 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -111,8 +111,8 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler + integrator_constrained; - /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, speed %.4f, _k_i %.4f, _k_p: %.4f", - (double)_last_output, (double)_integrator, (double)_integrator_max, (double)speed, (double)_k_i, (double)_k_p);*/ + /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f", + (double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/ return math::constrain(_last_output, -1.0f, 1.0f); diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 90e3601e69..46df5cd744 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -58,9 +58,8 @@ RunwayTakeoff::RunwayTakeoff() : _initialized_time(0), _init_yaw(0), _climbout(false), - _start_sp{}, - _target_sp{}, _throttle_ramp_time(2 * 1e6), + _start_wp(), _runway_takeoff_enabled(this, "TKOFF"), _heading_mode(this, "HDG"), _nav_alt(this, "NAV_ALT"), @@ -81,16 +80,19 @@ RunwayTakeoff::~RunwayTakeoff() } -void RunwayTakeoff::init(float yaw) +void RunwayTakeoff::init(float yaw, double current_lat, double current_lon) { _init_yaw = yaw; _initialized = true; _state = RunwayTakeoffState::THROTTLE_RAMP; _initialized_time = hrt_absolute_time(); _climbout = true; // this is true until climbout is finished + _start_wp(0) = (float)current_lat; + _start_wp(1) = (float)current_lon; } -void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) +void RunwayTakeoff::update(float airspeed, float alt_agl, + double current_lat, double current_lon, int mavlink_fd) { switch (_state) { @@ -112,6 +114,16 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::TAKEOFF: if (alt_agl > _nav_alt.get()) { _state = RunwayTakeoffState::CLIMBOUT; + + /* + * If we started in heading hold mode, move the navigation start WP to the current location now. + * The navigator will take this as starting point to navigate towards the takeoff WP. + */ + if (_heading_mode.get() == 0) { + _start_wp(0) = (float)current_lat; + _start_wp(1) = (float)current_lon; + } + mavlink_log_info(mavlink_fd, "#Climbout"); } @@ -127,7 +139,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) break; default: - return; + break; } } @@ -177,10 +189,18 @@ float RunwayTakeoff::getRoll(float navigatorRoll) /* * Returns the yaw setpoint to use. + * + * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the + * runway. When it has enough ground clearance we start navigation towards WP. */ float RunwayTakeoff::getYaw(float navigatorYaw) { - return navigatorYaw; + if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) { + return _init_yaw; + + } else { + return navigatorYaw; + } } /* @@ -252,31 +272,9 @@ float RunwayTakeoff::getMaxPitch(float max) /* * Returns the "previous" (start) WP for navigation. */ -math::Vector<2> RunwayTakeoff::getPrevWP() +math::Vector<2> RunwayTakeoff::getStartWP() { - math::Vector<2> prev_wp; - prev_wp(0) = (float)_start_sp.lat; - prev_wp(1) = (float)_start_sp.lon; - return prev_wp; -} - -/* - * Returns the "current" (target) WP for navigation. - */ -math::Vector<2> RunwayTakeoff::getCurrWP(math::Vector<2> sp_curr_wp) -{ - if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::FLY) { - // navigating towards calculated direction if heading mode 0 and as long as we're in climbout - math::Vector<2> curr_wp; - curr_wp(0) = (float)_target_sp.lat; - curr_wp(1) = (float)_target_sp.lon; - return curr_wp; - - } else { - // navigating towards next mission waypoint - return sp_curr_wp; - } - + return _start_wp; } void RunwayTakeoff::reset() diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index cf0ab455d8..0e1c5ed14b 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -68,16 +68,14 @@ public: RunwayTakeoff(); ~RunwayTakeoff(); - void init(float yaw); - void update(float airspeed, float alt_agl, int mavlink_fd); + void init(float yaw, double current_lat, double current_lon); + void update(float airspeed, float alt_agl, double current_lat, double current_lon, int mavlink_fd); RunwayTakeoffState getState() { return _state; }; bool isInitialized() { return _initialized; }; bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; - position_setpoint_s *getStartSP() { return &_start_sp; }; - position_setpoint_s *getTargetSP() { return &_target_sp; }; float getInitYaw() { return _init_yaw; }; bool controlYaw(); @@ -89,8 +87,7 @@ public: bool resetIntegrators(); float getMinPitch(float sp_min, float climbout_min, float min); float getMaxPitch(float max); - math::Vector<2> getPrevWP(); - math::Vector<2> getCurrWP(math::Vector<2> sp_curr_wp); + math::Vector<2> getStartWP(); void reset(); @@ -102,9 +99,8 @@ private: hrt_abstime _initialized_time; float _init_yaw; bool _climbout; - struct position_setpoint_s _start_sp; - struct position_setpoint_s _target_sp; unsigned _throttle_ramp_time; + math::Vector<2> _start_wp; /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 41ca2edf44..ce2db9ad5c 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_WR_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f); /** * Wheel steering rate integrator gain @@ -254,7 +254,7 @@ PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_WR_IMAX, 0.2f); +PARAM_DEFINE_FLOAT(FW_WR_IMAX, 1.0f); /** * Maximum wheel steering rate diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 02a61a20e7..45032d4735 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1375,45 +1375,31 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { - _runway_takeoff.init(_att.yaw); + _runway_takeoff.init(_att.yaw, _global_pos.lat, _global_pos.lon); /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ _takeoff_ground_alt = _global_pos.alt; - // draw a line from takeoff location into the direction the UAV is heading - get_waypoint_heading_distance( - _runway_takeoff.getInitYaw(), - HDG_HOLD_DIST_NEXT, - *_runway_takeoff.getStartSP(), - *_runway_takeoff.getTargetSP(), - true); - mavlink_log_info(_mavlink_fd, "#Takeoff on runway"); } - // update takeoff path if we're reaching the end of it - if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, - _runway_takeoff.getTargetSP()->lat, _runway_takeoff.getTargetSP()->lon) < HDG_HOLD_REACHED_DIST) { - get_waypoint_heading_distance( - _runway_takeoff.getInitYaw(), - HDG_HOLD_DIST_NEXT, - *_runway_takeoff.getStartSP(), - *_runway_takeoff.getTargetSP(), - false); - } - - /* update navigation: _runway_takeoff decides if we target the current WP from setpoint triplet - * or the calculated one through initial heading */ - _l1_control.navigate_waypoints(_runway_takeoff.getPrevWP(), _runway_takeoff.getCurrWP(curr_wp), current_position, ground_speed_2d); float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); // update runway takeoff helper _runway_takeoff.update( _airspeed.true_airspeed_m_s, _global_pos.alt - terrain_alt, + _global_pos.lat, + _global_pos.lon, _mavlink_fd); + /* + * Update navigation: _runway_takeoff returns the start WP according to mode and phase. + * If we use the navigator heading or not is decided later. + */ + _l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, current_position, ground_speed_2d); + // update tecs float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max); float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); From 73b1c186989f2c11426badb7a19e6417688efab1 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 19 Oct 2015 13:49:22 +0200 Subject: [PATCH 091/136] do not stick to terrain estimate if it's not valid --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 45032d4735..b608ea7f9e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -963,7 +963,7 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it * for the whole landing */ - if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) { + if (_parameters.land_use_terrain_estimate && global_pos.terrain_alt_valid) { if(!land_useterrain) { mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate"); land_useterrain = true; From 0bd23dd7c5511446451a491424ea58d296b9ca4b Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 19 Oct 2015 13:56:53 +0200 Subject: [PATCH 092/136] only go into heading hold mode after flaring --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index b608ea7f9e..122ce0fdda 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1227,8 +1227,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi wp_distance_save = 0.0f; } - //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); - if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { + // we want the plane to keep tracking the desired flight path until we start flaring + // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds + if (land_noreturn_vertical) { /* heading hold, along the line connecting this and the last waypoint */ From 2a2e2a27d321146aa06ae7588c5d2f3cad1375d2 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 19 Oct 2015 14:55:32 +0200 Subject: [PATCH 093/136] update comments --- ROMFS/px4fmu_common/mixers/AERTW.main.mix | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/AERTW.main.mix b/ROMFS/px4fmu_common/mixers/AERTW.main.mix index 49665c5110..2ab5bdb611 100644 --- a/ROMFS/px4fmu_common/mixers/AERTW.main.mix +++ b/ROMFS/px4fmu_common/mixers/AERTW.main.mix @@ -1,16 +1,16 @@ -Aileron/rudder/elevator/throttle/wheel mixer for PX4FMU +Aileron/rudder/elevator/throttle/wheel/flaps mixer for PX4FMU ======================================================= This file defines mixers suitable for controlling a fixed wing aircraft with aileron, rudder, elevator, throttle and steerable wheel controls using PX4FMU. The configuration assumes the aileron servo(s) are connected to PX4FMU servo -output 0, the elevator to output 1, the rudder to output 2, the throttle -to output 3 and the wheel to output 4. +output 0 and 1, the elevator to output 2, the rudder to output 3, the throttle +to output 4 and the wheel to output 5. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch), 2 (yaw) and 3 (thrust). +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). -Flaperon mixer (ailerons + flaps) +Aileron mixer (roll + flaperon) --------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; @@ -79,7 +79,7 @@ O: 10000 10000 0 -10000 10000 S: 0 2 10000 10000 0 -10000 10000 -Gimbal / flaps / payload mixer for last three channels, +Flaps / gimbal / payload mixer for last three channels, using the payload control group ----------------------------------------------------- From 8fa22c2efa7494956d32e8cd27961adc2c9de0c6 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 19 Oct 2015 14:57:34 +0200 Subject: [PATCH 094/136] renamed mixer file --- ROMFS/px4fmu_common/mixers/{AERTW.main.mix => AERTWF.main.mix} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename ROMFS/px4fmu_common/mixers/{AERTW.main.mix => AERTWF.main.mix} (100%) diff --git a/ROMFS/px4fmu_common/mixers/AERTW.main.mix b/ROMFS/px4fmu_common/mixers/AERTWF.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/AERTW.main.mix rename to ROMFS/px4fmu_common/mixers/AERTWF.main.mix From 5f400946855dc3cac6e1faf133a16018bed85588 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 19 Oct 2015 16:10:33 +0200 Subject: [PATCH 095/136] fixed bug which lead to direct yaw control in stabilized mode --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 3e7a9f9aa8..8772ceb835 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -800,6 +800,10 @@ FixedwingAttitudeControl::task_main() vehicle_status_poll(); + // the position controller will not emit attitude setpoints in some modes + // we need to make sure that this flag is reset + _att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled; + /* lock integrator until control is started */ bool lock_integrator; From 3d3398e33026bb5c12157d1490805a768313dd0e Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 27 Oct 2015 16:39:33 +0100 Subject: [PATCH 096/136] added code handling aborting landings --- .../fw_pos_control_l1_main.cpp | 73 ++++++++++++++++++- src/modules/navigator/navigator_main.cpp | 11 ++- 2 files changed, 78 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 122ce0fdda..61d29494db 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -105,6 +105,7 @@ static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_YAWRATE_THRESH 0.15f // max yawrate at which plane locks yaw for heading hold mode #define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading #define TAKEOFF_IDLE 0.2f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) +#define T_ALT_TIMEOUT 1 // time after which we abort landing if terrain estimate is not valid static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode @@ -196,6 +197,11 @@ private: bool land_onslope; bool land_useterrain; + // terrain estimation states + float _t_alt_prev_valid; //**< last terrain estimate which was valid */ + hrt_abstime _time_last_t_alt; //*< time at which we had last valid terrain alt */ + hrt_abstime _time_started_landing; //*< time at which landing started */ + bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ @@ -530,6 +536,9 @@ FixedwingPositionControl::FixedwingPositionControl() : land_motor_lim(false), land_onslope(false), land_useterrain(false), + _t_alt_prev_valid(0), + _time_last_t_alt(0), + _time_started_landing(0), _was_in_air(false), _time_went_in_air(0), _runway_takeoff(), @@ -1198,13 +1207,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - if (in_takeoff_situation()) { + float alt_sp; + if (_nav_capabilities.abort_landing == true) { + // if we entered loiter due to an aborted landing, demand + // altitude setpoint well above landing waypoint + alt_sp = _pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff; + } else { + // use altitude given by wapoint + alt_sp = _pos_sp_triplet.current.alt; + } + + if (in_takeoff_situation() || + ((_global_pos.alt < _pos_sp_triplet.current.alt + _parameters.climbout_diff ) && _nav_capabilities.abort_landing == true)) { /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); } - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + tecs_update_pitch_throttle(alt_sp, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); @@ -1215,6 +1235,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // if they have been enabled using the corresponding parameter _att_sp.apply_flaps = true; + // save time at which we started landing + if (_time_started_landing == 0) { + _time_started_landing = hrt_absolute_time(); + } + float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); @@ -1273,7 +1298,40 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be * equal to _pos_sp_triplet.current.alt */ - float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos); + float terrain_alt; + if (_parameters.land_use_terrain_estimate) { + if (_global_pos.terrain_alt_valid) { + // all good, have valid terrain altitude + terrain_alt = _global_pos.terrain_alt; + _t_alt_prev_valid = terrain_alt; + _time_last_t_alt = hrt_absolute_time(); + } else if (_time_last_t_alt == 0) { + // we have started landing phase but don't have valid terrain + // wait for some time, maybe we will soon get a valid estimate + // until then just use the altitude of the landing waypoint + if (hrt_elapsed_time(&_time_started_landing) < 5 * 1000 * 1000) { + terrain_alt = _pos_sp_triplet.current.alt; + } else { + // still no valid terrain, abort landing + terrain_alt = _pos_sp_triplet.current.alt; + _nav_capabilities.abort_landing = true; + } + } else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000) + || land_noreturn_vertical) { + // use previous terrain estimate for some time and hope to recover + // if we are already flaring (land_noreturn_vertical) then just + // go with the old estimate + terrain_alt = _t_alt_prev_valid; + } else { + // terrain alt was not valid for long time, abort landing + terrain_alt = _t_alt_prev_valid; + _nav_capabilities.abort_landing = true; + } + } else { + // no terrain estimation, just use landing waypoint altitude + terrain_alt = _pos_sp_triplet.current.alt; + } + /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_altitude_rel = _pos_sp_triplet.previous.valid ? @@ -1720,9 +1778,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /** MANUAL FLIGHT **/ - /* reset hold altitude */ + // reset hold altitude _hold_alt = _global_pos.alt; + // reset terrain estimation relevant values + _time_started_landing = 0; + _time_last_t_alt = 0; + + // reset lading abort state + _nav_capabilities.abort_landing = false; + /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e44b8d0be5..b056e3fd99 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -440,8 +440,15 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - _pos_sp_triplet_published_invalid_once = false; - _navigation_mode = &_mission; + if (_nav_caps.abort_landing) { + // pos controller aborted landing, requests loiter + // above landing waypoint + _navigation_mode = &_loiter; + _pos_sp_triplet_published_invalid_once = false; + } else { + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_mission; + } break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; From 0f03ae12d79f062a65de76ab6690def0fd1ac6f8 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 28 Oct 2015 07:03:45 +0100 Subject: [PATCH 097/136] added field for aborted fw landings --- msg/navigation_capabilities.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/navigation_capabilities.msg b/msg/navigation_capabilities.msg index 235b5df03b..6d12aaaed9 100644 --- a/msg/navigation_capabilities.msg +++ b/msg/navigation_capabilities.msg @@ -3,3 +3,4 @@ float32 turn_distance # the optimal distance to a waypoint to switch to the nex float32 landing_horizontal_slope_displacement float32 landing_slope_angle_rad float32 landing_flare_length +bool abort_landing From 896dff40cfbee4e93bb91d4c988bcceab32d9d27 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 28 Oct 2015 08:02:51 +0100 Subject: [PATCH 098/136] added geo functions to create new waypoints from given setting --- src/lib/geo/geo.c | 25 +++++++++++++++++++++++++ src/lib/geo/geo.h | 6 ++++++ 2 files changed, 31 insertions(+) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index a4b1599a9d..07d765c3bc 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -298,6 +298,31 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou return CONSTANTS_RADIUS_OF_EARTH * c; } +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target) +{ + float heading; + if (fabsf(dist) < FLT_EPSILON) { + *lat_target = lat_A; + *lon_target = lon_A; + } + else if (dist >= FLT_EPSILON) { + heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + } else { + heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + heading = _wrap_2pi(heading + 180.0f * M_PI_F); + waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + } +} + +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *lat_end, double *lon_end) +{ + bearing = _wrap_2pi(bearing); + double radius_ratio = (double)(dist / CONSTANTS_RADIUS_OF_EARTH); + + *lat_end = asin(sin(lat_start) * cos(radius_ratio) + cos(lat_start) * sin(radius_ratio) * cos((double)bearing)); + *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); +} __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { double lat_now_rad = lat_now * M_DEG_TO_RAD; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index f77a8b58a4..f9250fb5a3 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -236,6 +236,12 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al */ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); + +// TODO put description for both functions and improve naming +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target); + +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *end_lat, double *end_lon); + /** * Returns the bearing to the next waypoint in radians. * From 7f0c3a9b71ada3b422bbd8c339ba7696cd39beb3 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 28 Oct 2015 08:16:30 +0100 Subject: [PATCH 099/136] use virtual setpoint for landing line tracking --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 61d29494db..e08af118e9 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1252,6 +1252,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi wp_distance_save = 0.0f; } + // create virtual waypoint which is on the desired flight path but + // some distance behind landing waypoint. This will make sure that the plane + // will always follow the desired flight path even if we get close or past + // the landing waypoint + math::Vector<2> curr_wp_shifted; + double lat; + double lon; + create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat,pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); + curr_wp_shifted(0) = (float)lat; + curr_wp_shifted(1) = (float)lon; // we want the plane to keep tracking the desired flight path until we start flaring // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds if (land_noreturn_vertical) { @@ -1276,7 +1286,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); + _l1_control.navigate_waypoints(prev_wp, curr_wp_shifted, current_position, ground_speed_2d); } _att_sp.roll_body = _l1_control.nav_roll(); From 6f4c8d45ff5624f341a8ce017faca86bae47607a Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 28 Oct 2015 10:37:30 +0100 Subject: [PATCH 100/136] during flare control pitch setpoint based on distance to ground --- .../fw_pos_control_l1_main.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index e08af118e9..7a98859f81 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -197,10 +197,11 @@ private: bool land_onslope; bool land_useterrain; - // terrain estimation states + // landing relevant states float _t_alt_prev_valid; //**< last terrain estimate which was valid */ hrt_abstime _time_last_t_alt; //*< time at which we had last valid terrain alt */ hrt_abstime _time_started_landing; //*< time at which landing started */ + float height_flare; //*< estimated height to ground at which flare started */ bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ @@ -539,6 +540,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _t_alt_prev_valid(0), _time_last_t_alt(0), _time_started_landing(0), + height_flare(0.0f), _was_in_air(false), _time_went_in_air(0), _runway_takeoff(), @@ -1396,10 +1398,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); if (!land_noreturn_vertical) { + // just started with the flaring phase + _att_sp.pitch_body = 0.0f; + height_flare = _global_pos.alt - terrain_alt; mavlink_log_info(_mavlink_fd, "#Landing, flaring"); land_noreturn_vertical = true; + } else { + if (_global_pos.vel_d > 0.1f) { + _att_sp.pitch_body = _parameters.land_flare_pitch_min_deg * (height_flare - (_global_pos.alt - terrain_alt)) / height_flare; + } else { + _att_sp.pitch_body = _att_sp.pitch_body; + } } - //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); flare_curve_alt_rel_last = flare_curve_alt_rel; } else { @@ -1852,7 +1862,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && (launch_detection_state == LAUNCHDETECTION_RES_NONE || - _runway_takeoff.runwayTakeoffEnabled()) + _runway_takeoff.runwayTakeoffEnabled() || + land_noreturn_vertical) )) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } From 5b1c7321e732a20e39dc52bc0ce174c23ea858ad Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 28 Oct 2015 14:08:42 +0100 Subject: [PATCH 101/136] reverted navigation on landing back to old heading hold --- src/lib/geo/geo.c | 4 ++-- .../fw_pos_control_l1_main.cpp | 20 +++++++++++-------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 07d765c3bc..0d09e8526b 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -310,7 +310,7 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); } else { heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); - heading = _wrap_2pi(heading + 180.0f * M_PI_F); + heading = _wrap_2pi(heading + M_PI_F); waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); } } @@ -318,7 +318,7 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou __EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *lat_end, double *lon_end) { bearing = _wrap_2pi(bearing); - double radius_ratio = (double)(dist / CONSTANTS_RADIUS_OF_EARTH); + double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); *lat_end = asin(sin(lat_start) * cos(radius_ratio) + cos(lat_start) * sin(radius_ratio) * cos((double)bearing)); *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7a98859f81..080b96c5ce 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1264,9 +1264,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat,pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); curr_wp_shifted(0) = (float)lat; curr_wp_shifted(1) = (float)lon; + // we want the plane to keep tracking the desired flight path until we start flaring // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds - if (land_noreturn_vertical) { + //if (land_noreturn_vertical) { + if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ @@ -1288,7 +1290,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, curr_wp_shifted, current_position, ground_speed_2d); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); } _att_sp.roll_body = _l1_control.nav_roll(); @@ -1405,7 +1407,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_noreturn_vertical = true; } else { if (_global_pos.vel_d > 0.1f) { - _att_sp.pitch_body = _parameters.land_flare_pitch_min_deg * (height_flare - (_global_pos.alt - terrain_alt)) / height_flare; + _att_sp.pitch_body = math::radians(_parameters.land_flare_pitch_min_deg) * + math::constrain((height_flare - (_global_pos.alt - terrain_alt)) / height_flare, 0.0f, 1.0f); } else { _att_sp.pitch_body = _att_sp.pitch_body; } @@ -1859,12 +1862,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* During a takeoff waypoint while waiting for launch the pitch sp is set * already (not by tecs) */ - if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && ( + (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && (launch_detection_state == LAUNCHDETECTION_RES_NONE || - _runway_takeoff.runwayTakeoffEnabled() || - land_noreturn_vertical) - )) { + _runway_takeoff.runwayTakeoffEnabled())) || + (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND && + land_noreturn_vertical)) + )) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } From bc0fb69189ecf20c13c88fd81e86923ed1d3f345 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 28 Oct 2015 14:52:15 +0100 Subject: [PATCH 102/136] change mixer for maja --- ROMFS/px4fmu_common/init.d/2105_maja | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/2105_maja b/ROMFS/px4fmu_common/init.d/2105_maja index 2cbc11c275..10c5608eae 100644 --- a/ROMFS/px4fmu_common/init.d/2105_maja +++ b/ROMFS/px4fmu_common/init.d/2105_maja @@ -39,7 +39,7 @@ then param set PWM_DISARMED 1000 fi -set MIXER AERTW +set MIXER AERTWF # use PWM parameters for throttle channel set PWM_OUT 5 From d97ead81aa249ba82f518e6f7c59bd50e5f0d5d3 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 28 Oct 2015 17:06:38 +0100 Subject: [PATCH 103/136] set talt timeout to 10sec --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 080b96c5ce..0b07ceda39 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1323,7 +1323,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // we have started landing phase but don't have valid terrain // wait for some time, maybe we will soon get a valid estimate // until then just use the altitude of the landing waypoint - if (hrt_elapsed_time(&_time_started_landing) < 5 * 1000 * 1000) { + if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) { terrain_alt = _pos_sp_triplet.current.alt; } else { // still no valid terrain, abort landing From 5e4df86091f587ffcbb9fdb66fe87d00aaf2520b Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 2 Nov 2015 17:40:32 +0100 Subject: [PATCH 104/136] added albatross config and mixer --- ROMFS/px4fmu_common/init.d/2105_maja | 12 +-- ROMFS/px4fmu_common/init.d/2106_albatross | 51 +++++++++++ .../{AERTWF.main.mix => AAERTWF.main.mix} | 0 ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix | 84 +++++++++++++++++++ 4 files changed, 142 insertions(+), 5 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/2106_albatross rename ROMFS/px4fmu_common/mixers/{AERTWF.main.mix => AAERTWF.main.mix} (100%) create mode 100644 ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix diff --git a/ROMFS/px4fmu_common/init.d/2105_maja b/ROMFS/px4fmu_common/init.d/2105_maja index 10c5608eae..1f68428f3b 100644 --- a/ROMFS/px4fmu_common/init.d/2105_maja +++ b/ROMFS/px4fmu_common/init.d/2105_maja @@ -5,10 +5,12 @@ # @type Standard Plane # # @output MAIN1 aileron -# @output MAIN2 elevator -# @output MAIN3 rudder -# @output MAIN4 throttle -# @output MAIN5 wheel +# @output MAIN2 aileron +# @output MAIN3 elevator +# @output MAIN4 rudder +# @output MAIN5 throttle +# @output MAIN6 wheel +# @output MAIN7 flaps # # @output AUX1 feed-through of RC AUX1 channel # @output AUX2 feed-through of RC AUX2 channel @@ -39,7 +41,7 @@ then param set PWM_DISARMED 1000 fi -set MIXER AERTWF +set MIXER AAERTWF # use PWM parameters for throttle channel set PWM_OUT 5 diff --git a/ROMFS/px4fmu_common/init.d/2106_albatross b/ROMFS/px4fmu_common/init.d/2106_albatross new file mode 100644 index 0000000000..79e560fb5f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2106_albatross @@ -0,0 +1,51 @@ +#!nsh +# +# @name Applied Aeronautics Albatross +# +# @type Standard Plane +# +# @output MAIN1 aileron right +# @output MAIN2 aileron left +# @output MAIN3 v-tail right +# @output MAIN4 v-tail left +# @output MAIN5 throttle +# @output MAIN6 wheel +# @output MAIN7 flaps right +# @output MAIN8 flaps left +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Andreas Antener +# + +sh /etc/init.d/rc.fw_defaults + +if [ $AUTOCNF == yes ] +then + param set FW_AIRSPD_MIN 10 + param set FW_AIRSPD_TRIM 15 + param set FW_AIRSPD_MAX 20 + + param set FW_MAN_P_MAX 55 + param set FW_MAN_R_MAX 55 + param set FW_R_LIM 55 + + param set FW_WR_FF 0.2 + param set FW_WR_I 0.2 + param set FW_WR_IMAX 0.8 + param set FW_WR_P 1 + param set FW_W_RMAX 0 + + # set disarmed value for the ESC + param set PWM_DISARMED 1000 +fi + +set MIXER AAVVTWFF + +# use PWM parameters for throttle channel +set PWM_OUT 5 +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX diff --git a/ROMFS/px4fmu_common/mixers/AERTWF.main.mix b/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/AERTWF.main.mix rename to ROMFS/px4fmu_common/mixers/AAERTWF.main.mix diff --git a/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix new file mode 100644 index 0000000000..52146b84f0 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix @@ -0,0 +1,84 @@ +Aileron/v-tail/throttle/wheel/flaps mixer for PX4FMU +======================================================= + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, v-tail (rudder, elevator), throttle, steerable wheel and flaps +using PX4FMU. +The configuration assumes the aileron servos are connected to PX4FMU servo +output 0 and 1, the tail servos to output 2 and 3, the throttle +to output 4, the wheel to output 5 and the flaps to output 6 and 7. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). + +Aileron mixer (roll + flaperon) +--------------------------------- + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 +S: 0 6 -10000 -10000 0 -10000 10000 + +V-tail mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two tail servos are physically reversed, the pitch +input is inverted between the two servos. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 2 -7000 -7000 0 -10000 10000 +S: 0 1 -8000 -8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 2 -7000 -7000 0 -10000 10000 +S: 0 1 8000 8000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + +Wheel mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the wheel servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 -10000 -10000 0 -10000 10000 + +Flaps mixer +------------ +Flap servos are physically reversed. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 5000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 -10000 -5000 0 -10000 10000 + From 3a46487fa4f58889dae818075e6ed6dd73f6106c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 3 Nov 2015 15:35:28 +0100 Subject: [PATCH 105/136] fixed flaps offset to have correct neutral position --- ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix index 52146b84f0..e6520862d7 100644 --- a/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix +++ b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix @@ -76,9 +76,9 @@ Flap servos are physically reversed. M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 5000 0 -10000 10000 +S: 0 4 0 5000 -10000 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 -10000 -5000 0 -10000 10000 +S: 0 4 0 -5000 10000 -10000 10000 From 4abff89be0bd022c87e1be0a533c8c914478704e Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 6 Nov 2015 22:48:44 +0100 Subject: [PATCH 106/136] updated terrain estimator and runway takeoff libs to cmake build system --- cmake/configs/nuttx_px4fmu-v1_default.cmake | 2 ++ cmake/configs/nuttx_px4fmu-v2_default.cmake | 2 ++ cmake/configs/posix_eagle_default.cmake | 2 ++ cmake/configs/posix_sitl_simple.cmake | 2 ++ cmake/configs/qurt_eagle_hil.cmake | 2 ++ cmake/configs/qurt_eagle_release.cmake | 2 ++ cmake/configs/qurt_eagle_travis.cmake | 2 ++ src/lib/ecl/CMakeLists.txt | 1 + .../{module.mk => CMakeLists.txt} | 22 ++++++++++--------- .../{module.mk => CMakeLists.txt} | 14 ++++++++---- 10 files changed, 37 insertions(+), 14 deletions(-) rename src/lib/runway_takeoff/{module.mk => CMakeLists.txt} (86%) rename src/lib/terrain_estimation/{module.mk => CMakeLists.txt} (90%) diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake index ed48bd4f34..902447c282 100644 --- a/cmake/configs/nuttx_px4fmu-v1_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake @@ -111,6 +111,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff platforms/nuttx # had to add for cmake, not sure why wasn't in original config diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 0c6458cad1..c3a3586f66 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -118,6 +118,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff platforms/nuttx # had to add for cmake, not sure why wasn't in original config diff --git a/cmake/configs/posix_eagle_default.cmake b/cmake/configs/posix_eagle_default.cmake index fc97e70918..76d9711187 100644 --- a/cmake/configs/posix_eagle_default.cmake +++ b/cmake/configs/posix_eagle_default.cmake @@ -38,6 +38,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/conversion + lib/terrain_estimation + lib/runway_takeoff platforms/common platforms/posix/px4_layer diff --git a/cmake/configs/posix_sitl_simple.cmake b/cmake/configs/posix_sitl_simple.cmake index 581b82e827..a7291a04f3 100644 --- a/cmake/configs/posix_sitl_simple.cmake +++ b/cmake/configs/posix_sitl_simple.cmake @@ -56,6 +56,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff ) set(config_extra_builtin_cmds diff --git a/cmake/configs/qurt_eagle_hil.cmake b/cmake/configs/qurt_eagle_hil.cmake index 4222fff249..87dd29152e 100644 --- a/cmake/configs/qurt_eagle_hil.cmake +++ b/cmake/configs/qurt_eagle_hil.cmake @@ -47,6 +47,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/conversion + lib/terrain_estimation + lib/runway_takeoff modules/controllib # diff --git a/cmake/configs/qurt_eagle_release.cmake b/cmake/configs/qurt_eagle_release.cmake index 945e9da199..62a20263e5 100644 --- a/cmake/configs/qurt_eagle_release.cmake +++ b/cmake/configs/qurt_eagle_release.cmake @@ -70,6 +70,8 @@ set(config_module_list lib/geo lib/geo_lookup lib/conversion + lib/terrain_estimation + lib/runway_takeoff # # QuRT port diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake index b9fda6ec48..75cb5f2394 100644 --- a/cmake/configs/qurt_eagle_travis.cmake +++ b/cmake/configs/qurt_eagle_travis.cmake @@ -52,6 +52,8 @@ set(config_module_list lib/geo_lookup lib/conversion lib/ecl + lib/terrain_estimation + lib/runway_takeoff # # QuRT port diff --git a/src/lib/ecl/CMakeLists.txt b/src/lib/ecl/CMakeLists.txt index 0fd5b5f5e9..9f8e02ba2e 100644 --- a/src/lib/ecl/CMakeLists.txt +++ b/src/lib/ecl/CMakeLists.txt @@ -39,6 +39,7 @@ px4_add_module( attitude_fw/ecl_pitch_controller.cpp attitude_fw/ecl_roll_controller.cpp attitude_fw/ecl_yaw_controller.cpp + attitude_fw/ecl_wheel_controller.cpp l1/ecl_l1_pos_controller.cpp validation/data_validator.cpp validation/data_validator_group.cpp diff --git a/src/lib/runway_takeoff/module.mk b/src/lib/runway_takeoff/CMakeLists.txt similarity index 86% rename from src/lib/runway_takeoff/module.mk rename to src/lib/runway_takeoff/CMakeLists.txt index 95b9aea53e..7938279a0e 100644 --- a/src/lib/runway_takeoff/module.mk +++ b/src/lib/runway_takeoff/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved. +# Copyright (c) 2015 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 @@ -30,12 +30,14 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -# -# RunwayTakeoff Library -# - -SRCS = RunwayTakeoff.cpp \ - runway_takeoff_params.c - -MAXOPTIMIZATION = -Os +px4_add_module( + MODULE lib__runway_takeoff + COMPILE_FLAGS + -Os + SRCS + RunwayTakeoff.cpp + runway_takeoff_params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/terrain_estimation/module.mk b/src/lib/terrain_estimation/CMakeLists.txt similarity index 90% rename from src/lib/terrain_estimation/module.mk rename to src/lib/terrain_estimation/CMakeLists.txt index 3a2047012a..0c1177b96b 100644 --- a/src/lib/terrain_estimation/module.mk +++ b/src/lib/terrain_estimation/CMakeLists.txt @@ -30,7 +30,13 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -SRCS = terrain_estimator.cpp - -MAXOPTIMIZATION = -Os +px4_add_module( + MODULE lib__terrain_estimation + COMPILE_FLAGS + -Os + SRCS + terrain_estimator.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : From 2719789b2e773f4d4dfc347649d1bb71bc690b57 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 7 Nov 2015 11:41:15 +0100 Subject: [PATCH 107/136] use matrix lib to enable building for posix --- .../terrain_estimation/terrain_estimator.cpp | 54 +++++++++---------- .../terrain_estimation/terrain_estimator.h | 5 +- 2 files changed, 30 insertions(+), 29 deletions(-) diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index 16ab8de4a0..a92bbb136e 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -46,9 +46,9 @@ TerrainEstimator::TerrainEstimator() : _time_last_distance(0), _time_last_gps(0) { - _x.zero(); + memset(&_x._data[0], 0, sizeof(_x._data)); _u_z = 0.0f; - _P.identity(); + _P.setIdentity(); } bool TerrainEstimator::is_distance_valid(float distance) { @@ -63,9 +63,9 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu const struct distance_sensor_s *distance) { if (attitude->R_valid) { - math::Matrix<3, 3> R_att(attitude->R); - math::Vector<3> a(&sensor->accelerometer_m_s2[0]); - math::Vector<3> u; + matrix::Matrix R_att(attitude->R); + matrix::Vector a(&sensor->accelerometer_m_s2[0]); + matrix::Vector u; u = R_att * a; _u_z = u(2) + 9.81f; // compensate for gravity @@ -74,32 +74,32 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu } // dynamics matrix - math::Matrix A; - A.zero(); + matrix::Matrix A; + A.setZero(); A(0,1) = 1; A(1,2) = 1; // input matrix - math::Matrix B; - B.zero(); + matrix::Matrix B; + B.setZero(); B(1,0) = 1; // input noise variance float R = 0.135f; // process noise convariance - math::Matrix Q; + matrix::Matrix Q; Q(0,0) = 0; Q(1,1) = 0; // do prediction - math::Vector dx = (A * _x) * dt; + matrix::Vector dx = (A * _x) * dt; dx(1) += B(1,0) * _u_z * dt; // propagate state and covariance matrix _x += dx; - _P += (A * _P + _P * A.transposed() + - B * R * B.transposed() + Q) * dt; + _P += (A * _P + _P * A.transpose() + + B * R * B.transpose() + Q) * dt; } void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, @@ -114,21 +114,21 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl float d = distance->current_distance; - math::Matrix<1, n_x> C; + matrix::Matrix C; C(0, 0) = -1; // measured altitude, float R = 0.009f; - math::Vector<1> y; + matrix::Vector y; y(0) = d * cosf(attitude->roll) * cosf(attitude->pitch); // residual - math::Matrix<1, 1> S_I = (C * _P * C.transposed()); + matrix::Matrix S_I = (C * _P * C.transpose()); S_I(0,0) += R; - S_I = S_I.inversed(); - math::Vector<1> r = y - C * _x; + S_I = matrix::inv (S_I); + matrix::Vector r = y - C * _x; - math::Matrix K = _P * C.transposed() * S_I; + matrix::Matrix K = _P * C.transpose() * S_I; // some sort of outlayer rejection if (fabsf(distance->current_distance - _distance_last) < 1.0f) { @@ -149,21 +149,21 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl } if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) { - math::Matrix<1, n_x> C; + matrix::Matrix C; C(0,1) = 1; float R = 0.056f; - math::Vector<1> y; + matrix::Vector y; y(0) = gps->vel_d_m_s; // residual - math::Matrix<1, 1> S_I = (C * _P * C.transposed()); + matrix::Matrix S_I = (C * _P * C.transpose()); S_I(0,0) += R; - S_I = S_I.inversed(); - math::Vector<1> r = y - C * _x; + S_I = matrix::inv(S_I); + matrix::Vector r = y - C * _x; - math::Matrix K = _P * C.transposed() * S_I; + matrix::Matrix K = _P * C.transpose() * S_I; _x += K * r; _P -= K * C * _P; @@ -187,8 +187,8 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl } if (reinit) { - _x.zero(); - _P.zero(); + memset(&_x._data[0], 0, sizeof(_x._data)); + _P.setZero(); _P(0,0) = _P(1,1) = _P(2,2) = 0.1f; } diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index fc10cb8d63..697a053b88 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -36,6 +36,7 @@ */ #include +#include "matrix/Matrix.hpp" #include #include #include @@ -75,9 +76,9 @@ private: bool _terrain_valid; // kalman filter variables - math::Vector _x; // state: ground distance, velocity, accel bias in z direction + matrix::Vector _x; // state: ground distance, velocity, accel bias in z direction float _u_z; // acceleration in earth z direction - math::Matrix<3, 3> _P; // covariance matrix + matrix::Matrix _P; // covariance matrix // timestamps uint64_t _time_last_distance; From 120fd9d5225e1e09a19dc51c0f0ccc27b4a98766 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 7 Nov 2015 11:41:56 +0100 Subject: [PATCH 108/136] use control state topic for attitude and airspeed --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 0b07ceda39..24cf1f084f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1457,7 +1457,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { - _runway_takeoff.init(_att.yaw, _global_pos.lat, _global_pos.lon); + math::Quaternion q(&_ctrl_state.q[0]); + math::Vector<3> euler = q.to_euler(); + _runway_takeoff.init(euler(2), _global_pos.lat, _global_pos.lon); /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ @@ -1470,7 +1472,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // update runway takeoff helper _runway_takeoff.update( - _airspeed.true_airspeed_m_s, + _ctrl_state.airspeed, _global_pos.alt - terrain_alt, _global_pos.lat, _global_pos.lon, From e48cf53ce83ac7f1e30b3e662e6eae5e568688c3 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 7 Nov 2015 11:43:07 +0100 Subject: [PATCH 109/136] fix code style --- src/lib/geo/geo.c | 15 ++++--- src/lib/geo/geo.h | 6 ++- src/lib/runway_takeoff/RunwayTakeoff.cpp | 2 +- .../terrain_estimation/terrain_estimator.cpp | 40 +++++++++++-------- .../terrain_estimation/terrain_estimator.h | 7 ++-- 5 files changed, 42 insertions(+), 28 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 0d09e8526b..023337ff04 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -298,16 +298,19 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou return CONSTANTS_RADIUS_OF_EARTH * c; } -__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target) +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, + double *lat_target, double *lon_target) { float heading; + if (fabsf(dist) < FLT_EPSILON) { *lat_target = lat_A; *lon_target = lon_A; - } - else if (dist >= FLT_EPSILON) { + + } else if (dist >= FLT_EPSILON) { heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + } else { heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); heading = _wrap_2pi(heading + M_PI_F); @@ -315,13 +318,15 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou } } -__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *lat_end, double *lon_end) +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, + double *lat_end, double *lon_end) { bearing = _wrap_2pi(bearing); double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); *lat_end = asin(sin(lat_start) * cos(radius_ratio) + cos(lat_start) * sin(radius_ratio) * cos((double)bearing)); - *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); + *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), + cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index f9250fb5a3..8dc0836e34 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -238,9 +238,11 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou // TODO put description for both functions and improve naming -__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target); +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, + double *lat_target, double *lon_target); -__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *end_lat, double *end_lon); +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, + double *end_lat, double *end_lon); /** * Returns the bearing to the next waypoint in radians. diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 46df5cd744..7089889858 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -92,7 +92,7 @@ void RunwayTakeoff::init(float yaw, double current_lat, double current_lon) } void RunwayTakeoff::update(float airspeed, float alt_agl, - double current_lat, double current_lon, int mavlink_fd) + double current_lat, double current_lon, int mavlink_fd) { switch (_state) { diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index a92bbb136e..64a747f532 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -51,16 +51,19 @@ TerrainEstimator::TerrainEstimator() : _P.setIdentity(); } -bool TerrainEstimator::is_distance_valid(float distance) { +bool TerrainEstimator::is_distance_valid(float distance) +{ if (distance > 40.0f || distance < 0.00001f) { return false; + } else { return true; } } -void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, - const struct distance_sensor_s *distance) +void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude, + const struct sensor_combined_s *sensor, + const struct distance_sensor_s *distance) { if (attitude->R_valid) { matrix::Matrix R_att(attitude->R); @@ -76,25 +79,25 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu // dynamics matrix matrix::Matrix A; A.setZero(); - A(0,1) = 1; - A(1,2) = 1; + A(0, 1) = 1; + A(1, 2) = 1; // input matrix - matrix::Matrix B; + matrix::Matrix B; B.setZero(); - B(1,0) = 1; + B(1, 0) = 1; // input noise variance float R = 0.135f; // process noise convariance matrix::Matrix Q; - Q(0,0) = 0; - Q(1,1) = 0; + Q(0, 0) = 0; + Q(1, 1) = 0; // do prediction matrix::Vector dx = (A * _x) * dt; - dx(1) += B(1,0) * _u_z * dt; + dx(1) += B(1, 0) * _u_z * dt; // propagate state and covariance matrix _x += dx; @@ -102,8 +105,9 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu B * R * B.transpose() + Q) * dt; } -void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, - const struct vehicle_attitude_s *attitude) +void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, + const struct distance_sensor_s *distance, + const struct vehicle_attitude_s *attitude) { // terrain estimate is invalid if we have range sensor timeout if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) { @@ -124,7 +128,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl // residual matrix::Matrix S_I = (C * _P * C.transpose()); - S_I(0,0) += R; + S_I(0, 0) += R; S_I = matrix::inv (S_I); matrix::Vector r = y - C * _x; @@ -140,6 +144,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl // estimate to be invalid if (!is_distance_valid(distance->current_distance) && !is_distance_valid(_distance_last)) { _terrain_valid = false; + } else { _terrain_valid = true; } @@ -150,7 +155,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) { matrix::Matrix C; - C(0,1) = 1; + C(0, 1) = 1; float R = 0.056f; @@ -159,7 +164,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl // residual matrix::Matrix S_I = (C * _P * C.transpose()); - S_I(0,0) += R; + S_I(0, 0) += R; S_I = matrix::inv(S_I); matrix::Vector r = y - C * _x; @@ -172,6 +177,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl // reinitialise filter if we find bad data bool reinit = false; + for (int i = 0; i < n_x; i++) { if (!PX4_ISFINITE(_x(i))) { reinit = true; @@ -180,7 +186,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl for (int i = 0; i < n_x; i++) { for (int j = 0; j < n_x; j++) { - if (!PX4_ISFINITE(_P(i,j))) { + if (!PX4_ISFINITE(_P(i, j))) { reinit = true; } } @@ -189,7 +195,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl if (reinit) { memset(&_x._data[0], 0, sizeof(_x._data)); _P.setZero(); - _P(0,0) = _P(1,1) = _P(2,2) = 0.1f; + _P(0, 0) = _P(1, 1) = _P(2, 2) = 0.1f; } } diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index 697a053b88..25723f21d8 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -65,12 +65,13 @@ public: float get_velocity() {return _x(1);}; void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, - const struct distance_sensor_s *distance); - void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, + const struct distance_sensor_s *distance); + void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, + const struct distance_sensor_s *distance, const struct vehicle_attitude_s *attitude); private: - enum {n_x=3}; + enum {n_x = 3}; float _distance_last; bool _terrain_valid; From c989c21269e70399389af5a48a4222145f9512d6 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 10 Nov 2015 13:33:01 +0100 Subject: [PATCH 110/136] fixed geo functions --- src/lib/geo/geo.c | 15 +++++++++++---- src/lib/geo/geo.h | 25 +++++++++++++++++++++++-- 2 files changed, 34 insertions(+), 6 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 023337ff04..a8eee50435 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -319,14 +319,21 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou } __EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, - double *lat_end, double *lon_end) + double *lat_target, double *lon_target) { bearing = _wrap_2pi(bearing); double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); - *lat_end = asin(sin(lat_start) * cos(radius_ratio) + cos(lat_start) * sin(radius_ratio) * cos((double)bearing)); - *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), - cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); + double lat_start_rad = lat_start / (double)180.0 * M_PI; + double lon_start_rad = lon_start / (double)180.0 * M_PI; + + *lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos(( + double)bearing)); + *lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad), + cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target)); + + *lat_target *= (double)180.0 / M_PI; + *lon_target *= (double)180.0 / M_PI; } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 8dc0836e34..0aa6cf03a8 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -237,12 +237,33 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -// TODO put description for both functions and improve naming +/** + * Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance + * from waypoint A + * + * @param lat_A waypoint A latitude in degrees (47.1234567°, not 471234567°) + * @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°) + * @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°) + * @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°) + * @param dist distance of target waypoint from waypoint A (can be negative) + * @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°) + * @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°) + */ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target); +/** + * Creates a waypoint from given waypoint, distance and bearing + * + * @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°) + * @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°) + * @param bearing + * @param distance + * @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°) + * @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°) + */ __EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, - double *end_lat, double *end_lon); + double *lat_target, double *lon_target); /** * Returns the bearing to the next waypoint in radians. From 154fa07a4647534f2673e86870ae9b5d292f98d3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 16 Nov 2015 12:02:56 +0100 Subject: [PATCH 111/136] fixes after review --- src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp | 2 +- src/lib/geo/geo.c | 14 ++++++-------- src/lib/geo/geo.h | 7 ++++--- src/lib/runway_takeoff/runway_takeoff_params.c | 4 ++++ src/lib/terrain_estimation/terrain_estimator.h | 6 ++++-- .../ekf_att_pos_estimator_main.cpp | 2 +- 6 files changed, 20 insertions(+), 15 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index 7f8a9ca6c7..7e34ff2a61 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -33,7 +33,7 @@ /** * @file ecl_wheel_controller.cpp - * Implementation of a simple orthogonal coordinated turn yaw PID controller. + * Implementation of a simple PID wheel controller for heading tracking. * * Authors and acknowledgements in header. */ diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index a8eee50435..af49bf1504 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -301,18 +301,16 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target) { - float heading; - if (fabsf(dist) < FLT_EPSILON) { *lat_target = lat_A; *lon_target = lon_A; } else if (dist >= FLT_EPSILON) { - heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); } else { - heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); + float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); heading = _wrap_2pi(heading + M_PI_F); waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); } @@ -324,16 +322,16 @@ __EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_st bearing = _wrap_2pi(bearing); double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); - double lat_start_rad = lat_start / (double)180.0 * M_PI; - double lon_start_rad = lon_start / (double)180.0 * M_PI; + double lat_start_rad = lat_start * M_DEG_TO_RAD; + double lon_start_rad = lon_start * M_DEG_TO_RAD; *lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos(( double)bearing)); *lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad), cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target)); - *lat_target *= (double)180.0 / M_PI; - *lon_target *= (double)180.0 / M_PI; + *lat_target *= M_RAD_TO_DEG; + *lon_target *= M_RAD_TO_DEG; } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 0aa6cf03a8..11451fcf19 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -245,7 +245,7 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou * @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°) * @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°) * @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°) - * @param dist distance of target waypoint from waypoint A (can be negative) + * @param dist distance of target waypoint from waypoint A in meters (can be negative) * @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°) * @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°) */ @@ -254,11 +254,12 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou /** * Creates a waypoint from given waypoint, distance and bearing + * see http://www.movable-type.co.uk/scripts/latlong.html * * @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°) * @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°) - * @param bearing - * @param distance + * @param bearing in rad + * @param distance in meters * @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°) * @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°) */ diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index 7c64889e76..f5a1017f96 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -71,6 +71,7 @@ PARAM_DEFINE_INT32(RWTO_HDG, 0); * rudder is used to keep the heading (see RWTO_HDG). This should be below * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0. * + * @unit m * @min 0.0 * @max 100.0 * @group Runway Takeoff @@ -93,6 +94,7 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); * a little to keep it's wheel on the ground before airspeed * to takeoff is reached. * + * @unit deg * @min 0.0 * @max 20.0 * @group Runway Takeoff @@ -104,6 +106,7 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); * Fixed-wing settings are used if set to 0. Note that there is also a minimum * pitch of 10 degrees during takeoff, so this must be larger if set. * + * @unit deg * @min 0.0 * @max 60.0 * @group Runway Takeoff @@ -115,6 +118,7 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); * Roll is limited during climbout to ensure enough lift and prevents aggressive * navigation before we're on a safe height. * + * @unit deg * @min 0.0 * @max 60.0 * @group Runway Takeoff diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index 25723f21d8..ed9e9741f3 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -51,14 +51,14 @@ * The measurement_update(...) function does a measurement update based on range finder and gps * velocity measurements. Both functions should always be called together when there is new * acceleration data available. -* The is_valid() function provides information wether the estimate is valid. +* The is_valid() function provides information whether the estimate is valid. */ class __EXPORT TerrainEstimator { public: TerrainEstimator(); - ~TerrainEstimator(); + ~TerrainEstimator() {}; bool is_valid() {return _terrain_valid;} float get_distance_to_ground() {return -_x(0);} @@ -85,6 +85,7 @@ private: uint64_t _time_last_distance; uint64_t _time_last_gps; + /* struct { float var_acc_z; float var_p_z; @@ -92,6 +93,7 @@ private: float var_lidar; float var_gps_vz; } _params; + */ bool is_distance_valid(float distance); 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 e4c8eeccde..49f08f3cab 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 @@ -270,7 +270,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() } } while (_estimator_task != -1); } - + delete _terrain_estimator; delete _ekf; estimator::g_estimator = nullptr; From 2cf6d9f60576e911045edc6584d15caa75e5219a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 18 Nov 2015 10:15:20 +0100 Subject: [PATCH 112/136] Fix relative path of inject XML file. By Yi Yi --- Tools/px_process_params.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_process_params.py b/Tools/px_process_params.py index 3a11fc5c50..fcd5b994e9 100644 --- a/Tools/px_process_params.py +++ b/Tools/px_process_params.py @@ -130,7 +130,7 @@ def main(): # Output to XML file if args.xml: print("Creating XML file " + args.xml) - out = xmlout.XMLOutput(param_groups, args.board, args.inject_xml) + out = xmlout.XMLOutput(param_groups, args.board, os.path.join(args.src_path, args.inject_xml)) out.Save(args.xml) # Output to DokuWiki tables From c721d565a97c7f14a17211c715092192bc8e53e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 18 Nov 2015 11:21:08 +0100 Subject: [PATCH 113/136] Upgrade XCode version to what is working locally for Travis CI --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index e6b5239344..3745cc867a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -9,7 +9,7 @@ matrix: - os: linux sudo: false - os: osx - osx_image: beta-xcode6.3 + osx_image: xcode7 sudo: true cache: From 3c1c2ba34e600cad3d463847322a9123ffda9eb6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 18 Nov 2015 11:57:01 +0100 Subject: [PATCH 114/136] Travis CI: Install CMake --- .travis.yml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 3745cc867a..2aafa122e1 100644 --- a/.travis.yml +++ b/.travis.yml @@ -60,11 +60,9 @@ before_install: elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then brew tap PX4/homebrew-px4 && brew update - && brew install astyle - && brew install gcc-arm-none-eabi + && brew install cmake ninja astyle gcc-arm-none-eabi && brew install genromfs && brew install kconfig-frontends - && brew install ninja && sudo easy_install pip && sudo pip install pyserial empy ; From 041d6e704d8a0ee815590ffd532aefa4c544b8cf Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 18 Nov 2015 12:30:21 +0100 Subject: [PATCH 115/136] support for simplistic tailsitter --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index d362e661b4..d704f43029 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit d362e661b46474153f43f51a6eb947d4fda1bebe +Subproject commit d704f430297fffea766a42bda16436e3bd00d6ea From d86ca63c78c6b7fb0aa9b85ba33dbae83e38b907 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 18 Nov 2015 13:27:23 +0100 Subject: [PATCH 116/136] added gazebo startup script for tailsitter --- posix-configs/SITL/init/rcS_gazebo_tailsitter | 64 +++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 posix-configs/SITL/init/rcS_gazebo_tailsitter diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter new file mode 100644 index 0000000000..2bf7a78368 --- /dev/null +++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter @@ -0,0 +1,64 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 20 +param set MC_PITCHRATE_P 0.3 +param set MC_ROLLRATE_P 0.3 +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.35 +param set VT_TYPE 0 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.15 +param set MPC_XY_VEL_P 0.05 +param set MPC_XY_VEL_D 0.005 +param set MPC_XY_FF 0.1 +param set SENS_BOARD_ROT 8 +param set COM_RC_IN_MODE 2 +rgbled start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +measairspeedsim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +attitude_estimator_q start +position_estimator_inav start +vtol_att_control start +mc_pos_control start +mc_att_control start +fw_pos_control_l1 start +fw_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix +mavlink start -u 14556 -r 2000000 +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink boot_complete +sdlog2 start -r 100 -e -t -a From 6200cf6d869d1eb452519834f24557cfa346f4b3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 18 Nov 2015 13:27:44 +0100 Subject: [PATCH 117/136] reduce gyro update rate to 400 Hz --- src/platforms/posix/drivers/gyrosim/gyrosim.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 0ac023285c..c7f78dbca6 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -95,9 +95,9 @@ // Product Name Product Revision #define GYROSIMES_REV_C4 0x14 -#define GYROSIM_ACCEL_DEFAULT_RATE 1000 +#define GYROSIM_ACCEL_DEFAULT_RATE 400 -#define GYROSIM_GYRO_DEFAULT_RATE 1000 +#define GYROSIM_GYRO_DEFAULT_RATE 400 #define GYROSIM_ONE_G 9.80665f From 2f0b24feab7cac9c98ff80ff673868608c536cfd Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 18 Nov 2015 13:28:47 +0100 Subject: [PATCH 118/136] cmake: replace vtol model with tailsitter model --- Makefile | 2 +- Tools/sitl_run.sh | 4 ++-- src/firmware/posix/CMakeLists.txt | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Makefile b/Makefile index 096a058653..b0f49eda3e 100644 --- a/Makefile +++ b/Makefile @@ -184,7 +184,7 @@ clean: # targets handled by cmake cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \ run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim \ - jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_vtol gazebo_iris gazebo_vtol + jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_tailsitter gazebo_iris gazebo_tailsitter $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ)))) .PHONY: clean diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 8767f9f841..7dee02868a 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -88,9 +88,9 @@ then ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program} elif [ "$debugger" == "valgrind" ] then - valgrind ./mainapp ../../../../${rc_script}_${program} + valgrind ./mainapp ../../../../${rc_script}_${program}_${model} else - ./mainapp ../../../../${rc_script}_${program} + ./mainapp ../../../../${rc_script}_${program}_${model} fi if [ "$program" == "jmavsim" ] diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index fb7490ffe2..efc81d6335 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -34,7 +34,7 @@ add_dependencies(run_config mainapp) foreach(viewer none jmavsim gazebo) foreach(debugger none gdb lldb ddd valgrind) - foreach(model none iris vtol) + foreach(model none iris tailsitter) if (debugger STREQUAL "none") if (model STREQUAL "none") set(_targ_name "${viewer}") From 354e623318a0826398849eb7217b98c863bac6d9 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 18 Nov 2015 13:29:23 +0100 Subject: [PATCH 119/136] support for vtol simulation --- Tools/sitl_gazebo | 2 +- src/modules/simulator/simulator_mavlink.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index d704f43029..d7b4fcfabd 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit d704f430297fffea766a42bda16436e3bd00d6ea +Subproject commit d7b4fcfabde7d770cf0bf50287836d943eaf6714 diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index ca45cdf2b8..2a6efc7fa7 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -80,7 +80,7 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) // for now we only support quadrotors unsigned n = 4; - if (_vehicle_status.is_rotary_wing) { + if (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) { for (unsigned i = 0; i < 8; i++) { if (_actuators.output[i] > PWM_LOWEST_MIN / 2) { if (i < n) { @@ -113,7 +113,7 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) actuator_msg.time_usec = hrt_absolute_time(); actuator_msg.roll_ailerons = out[0]; - actuator_msg.pitch_elevator = _vehicle_status.is_rotary_wing ? out[1] : -out[1]; + actuator_msg.pitch_elevator = (_vehicle_status.is_rotary_wing || _vehicle_status.is_vtol) ? out[1] : -out[1]; actuator_msg.yaw_rudder = out[2]; actuator_msg.throttle = out[3]; actuator_msg.aux1 = out[4]; From cf338246fc971b78390cd659475432973cb857cd Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 18 Nov 2015 13:56:46 +0100 Subject: [PATCH 120/136] renamed SITL startup script for iris --- posix-configs/SITL/init/{rcS_gazebo => rcS_gazebo_iris} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename posix-configs/SITL/init/{rcS_gazebo => rcS_gazebo_iris} (100%) diff --git a/posix-configs/SITL/init/rcS_gazebo b/posix-configs/SITL/init/rcS_gazebo_iris similarity index 100% rename from posix-configs/SITL/init/rcS_gazebo rename to posix-configs/SITL/init/rcS_gazebo_iris From 16aa852c53f8ba5f5e4d0a67377adee4d362230f Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 18 Nov 2015 16:01:53 +0100 Subject: [PATCH 121/136] merged master into sitl_gazebo --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index d7b4fcfabd..7a2e18e786 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit d7b4fcfabde7d770cf0bf50287836d943eaf6714 +Subproject commit 7a2e18e7869e7d7c47ac7a97d956a64751429140 From 0fa0654f5a5cab33b1bb6788d9b57827a66f9bc5 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 18 Nov 2015 16:08:53 +0100 Subject: [PATCH 122/136] sitl_gazebo cleanup --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 7a2e18e786..6072426bbc 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 7a2e18e7869e7d7c47ac7a97d956a64751429140 +Subproject commit 6072426bbca9407f2f26698d3e20870c1744b4fb From 5103ba1c23f0b266c070b34b16fc8bf5d2c1f560 Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 19 Nov 2015 09:09:51 +0100 Subject: [PATCH 123/136] implemented better shell for SITL --- src/platforms/posix/main.cpp | 80 ++++++++++++++++++++++++++++++++---- 1 file changed, 72 insertions(+), 8 deletions(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 1b22627721..149ae6c185 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -35,6 +35,7 @@ * Basic shell to execute builtin "apps" * * @author Mark Charlebois + * @auther Roman Bapst */ #include @@ -43,6 +44,7 @@ #include #include #include +#include namespace px4 { @@ -56,6 +58,8 @@ typedef int (*px4_main_t)(int argc, char *argv[]); #include "apps.h" #include "px4_middleware.h" +#define CMD_BUFF_SIZE 100 + static bool _ExitFlag = false; extern "C" { void _SigIntHandler(int sig_num); @@ -203,22 +207,82 @@ int main(int argc, char **argv) } if (!daemon_mode) { - string mystr; + string mystr = ""; + string string_buffer[CMD_BUFF_SIZE]; + int buf_ptr_write = 0; + int buf_ptr_read = 0; print_prompt(); + // change input mode so that we can manage shell + struct termios term; + tcgetattr(0, &term); + term.c_lflag &= ~ICANON; + term.c_lflag &= ~ECHO; + tcsetattr(0, TCSANOW, &term); + setbuf(stdin, NULL); + while (!_ExitFlag) { - struct pollfd fds; - int ret; - fds.fd = 0; /* stdin */ - fds.events = POLLIN; - ret = poll(&fds, 1, 100); + char c = getchar(); - if (ret > 0) { - getline(cin, mystr); + switch (c) { + case 127: // backslash + if (mystr.length() > 0) { + mystr.pop_back(); + printf("%c[2K", 27); // clear line + cout << (char)13; + print_prompt(); + cout << mystr; + } + + break; + + case'\n': // user hit enter + if (buf_ptr_write == CMD_BUFF_SIZE) { + buf_ptr_write = 0; + } + + string_buffer[buf_ptr_write] = mystr; + buf_ptr_write++; process_line(mystr, !daemon_mode); mystr = ""; + buf_ptr_read = buf_ptr_write; + break; + + case '\033': { // arrow keys + c = getchar(); // skip first one, does not have the info + c = getchar(); + + if (c == 'A') { + buf_ptr_read--; + + } else if (c == 'B') { + if (buf_ptr_read < buf_ptr_write) { + buf_ptr_read++; + } + + } else { + // TODO: Support editing current line + } + + if (buf_ptr_read < 0) { + buf_ptr_read = 0; + } + + string saved_cmd = string_buffer[buf_ptr_read]; + printf("%c[2K", 27); + cout << (char)13; + mystr = saved_cmd; + print_prompt(); + cout << mystr; + break; + } + + default: // any other input + cout << c; + mystr += c; + break; } } From a8ab52b444c711050f5abeb59c23cb568a367f5e Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 19 Nov 2015 09:13:44 +0100 Subject: [PATCH 124/136] topic_listener: allow listening to actuator control groups --- Tools/generate_listener.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index 625eda7037..e8cd1ae114 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -17,7 +17,7 @@ for index,m in enumerate(raw_messages): temp_list_floats = [] temp_list_uint64 = [] temp_list_bool = [] - if("actuator_control" not in m and "pwm_input" not in m and "position_setpoint" not in m): + if("pwm_input" not in m and "position_setpoint" not in m): temp_list = [] f = open(m,'r') for line in f.readlines(): @@ -55,9 +55,11 @@ for index,m in enumerate(raw_messages): f.close() (m_head, m_tail) = os.path.split(m) - messages.append(m_tail.split('.')[0]) + message = m_tail.split('.')[0] + if message != "actuator_controls": + messages.append(message) + message_elements.append(temp_list) #messages.append(m.split('/')[-1].split('.')[0]) - message_elements.append(temp_list) num_messages = len(messages); From 020844e9e9ca96e17f05f1e041cb2b42f5c8fe75 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Nov 2015 15:53:08 +0100 Subject: [PATCH 125/136] Gyro: also output to console --- src/modules/commander/gyro_calibration.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 54866d8cb3..c65779c051 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -122,7 +122,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) } if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) { - mavlink_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count); + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count); } } @@ -131,14 +131,14 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) } if (poll_errcount > 1000) { - mavlink_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG); + mavlink_and_console_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG); return calibrate_return_error; } } for (unsigned s = 0; s < max_gyros; s++) { if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) { - mavlink_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s) + mavlink_and_console_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s) return calibrate_return_error; } @@ -155,7 +155,7 @@ int do_gyro_calibration(int mavlink_fd) int res = OK; gyro_worker_data_t worker_data = {}; - mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); worker_data.mavlink_fd = mavlink_fd; @@ -179,7 +179,7 @@ int do_gyro_calibration(int mavlink_fd) (void)sprintf(str, "CAL_GYRO%u_ID", s); res = param_set_no_notification(param_find(str), &(worker_data.device_id[s])); if (res != OK) { - mavlink_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s); + mavlink_and_console_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s); return ERROR; } @@ -193,7 +193,7 @@ int do_gyro_calibration(int mavlink_fd) px4_close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s); return ERROR; } } @@ -302,7 +302,7 @@ int do_gyro_calibration(int mavlink_fd) px4_close(fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG); } } } @@ -325,7 +325,7 @@ int do_gyro_calibration(int mavlink_fd) res = param_save_default(); if (res != OK) { - mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG); } } @@ -333,9 +333,9 @@ int do_gyro_calibration(int mavlink_fd) usleep(200000); if (res == OK) { - mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name); } else { - mavlink_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name); } /* give this message enough time to propagate */ From 8b70bd248742e2ca291c17c98150d34980957ddb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Nov 2015 15:53:32 +0100 Subject: [PATCH 126/136] Commander: Increase stack for commandline calibration --- src/modules/commander/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index c1ec35b5ee..c8ede875b6 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -37,7 +37,7 @@ endif() px4_add_module( MODULE modules__commander MAIN commander - STACK 1200 + STACK 3000 COMPILE_FLAGS ${MODULE_CFLAGS} -Os From c77c0f927b279fe0fa5ef8cbb39762454bfdbb37 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Nov 2015 15:54:57 +0100 Subject: [PATCH 127/136] Gyro cal: be more forgiving in calibration offset --- src/modules/commander/gyro_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index c65779c051..2048ac03ea 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -238,7 +238,7 @@ int do_gyro_calibration(int mavlink_fd) float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset; /* maximum allowable calibration error in radians */ - const float maxoff = 0.0055f; + const float maxoff = 0.01f; if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) || !PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) || From ca125bccbd5cefc6682e8d988d3927806faeae91 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Nov 2015 16:03:36 +0100 Subject: [PATCH 128/136] Update error message --- src/modules/commander/gyro_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 2048ac03ea..1fcd48b088 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -302,7 +302,7 @@ int do_gyro_calibration(int mavlink_fd) px4_close(fd); if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG); + mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, 1); } } } From 173edcef63b09fd0d334f57298368b0f24824cb8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Nov 2015 16:33:56 +0100 Subject: [PATCH 129/136] Commander: Fix reporting in presence of no telemetry link --- src/modules/commander/commander.cpp | 4 ++-- src/modules/commander/state_machine_helper.cpp | 6 +++--- src/modules/commander/state_machine_helper.h | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 83e79b77fc..d416358eeb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -382,9 +382,9 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); int checkres = 0; - checkres = preflight_check(&status, mavlink_fd_local, false); + checkres = preflight_check(&status, mavlink_fd_local, false, true); warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED"); - checkres = preflight_check(&status, mavlink_fd_local, true); + checkres = preflight_check(&status, mavlink_fd_local, true, true); warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED"); px4_close(mavlink_fd_local); return 0; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index bcdb5d3f9d..295ea3dcd2 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -743,12 +743,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en return status->nav_state != nav_state_old; } -int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm) +int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report) { /* */ - bool reportFailures = false; - reportFailures = !status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout; + bool reportFailures = force_report || (!status->data_link_lost && + !status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout); bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index bdb7b13f28..19b9394e3d 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); -int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm); +int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report=false); #endif /* STATE_MACHINE_HELPER_H_ */ From f2b988dcaa9781c2a397fbcfe8d2e2540891639a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Nov 2015 17:48:39 +0100 Subject: [PATCH 130/136] Fix error reporting logic --- src/modules/commander/commander.cpp | 10 +++------- src/modules/commander/state_machine_helper.cpp | 17 +++++++++++------ src/modules/commander/state_machine_helper.h | 2 +- 3 files changed, 15 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d416358eeb..e89626300e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2195,9 +2195,9 @@ int commander_thread_main(int argc, char *argv[]) /* report a regain */ if (telemetry_last_dl_loss[i] > 0) { mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i); - } else if (telemetry_last_dl_loss[i] == 0){ - /* do not report a new data link in order to not spam the user */ - status.data_link_found_new = true; + } else if (telemetry_last_dl_loss[i] == 0) { + /* new link */ + status.condition_system_prearm_error_reported = false; status_changed = true; } @@ -2208,10 +2208,6 @@ int commander_thread_main(int argc, char *argv[]) /* telemetry was healthy also in last iteration * we don't have to check a timeout */ have_link = true; - if(status.data_link_found_new) { - status.data_link_found_new = false; - status_changed = true; - } } } else { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 295ea3dcd2..c2f7126e85 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -123,6 +123,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* only perform the pre-arm check if we have to */ if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ ); } /* re-run the pre-flight check as long as sensors are failing */ @@ -130,6 +131,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */); status->condition_system_sensors_initialized = !prearm_ret; } @@ -272,10 +274,6 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s valid_transition) { status->condition_system_prearm_error_reported = false; } - if(status->data_link_found_new == true) - { - status->condition_system_prearm_error_reported = false; - } /* end of atomic state update */ #ifdef __PX4_NUTTX @@ -743,7 +741,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en return status->nav_state != nav_state_old; } -int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report) +int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report) { /* */ @@ -761,7 +759,14 @@ int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, if (!status->cb_usb && status->usb_connected && prearm) { preflight_ok = false; - if(reportFailures) mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); + } + } + + /* report once, then set the flag */ + if (mavlink_fd >= 0 && reportFailures && !preflight_ok) { + status->condition_system_prearm_error_reported = true; } return !preflight_ok; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 19b9394e3d..7c209cf07f 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); -int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report=false); +int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report=false); #endif /* STATE_MACHINE_HELPER_H_ */ From 7a5391a7233e8245d123ba966e79ba9202a1e789 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Nov 2015 18:28:16 +0100 Subject: [PATCH 131/136] Commander: Fix preflight check reporting and simplify logic --- src/modules/commander/PreflightCheck.cpp | 30 ++++++++++++++----- src/modules/commander/commander.cpp | 8 +++-- .../commander/state_machine_helper.cpp | 9 ++++-- 3 files changed, 33 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 8093ef56ba..54847fae28 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -124,8 +124,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in if (fd < 0) { if (!optional) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); + } } return false; @@ -134,8 +136,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id); if (ret) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); + } success = false; goto out; } @@ -143,8 +147,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in ret = px4_ioctl(fd, MAGIOCSELFTEST, 0); if (ret != OK) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance); + } success = false; goto out; } @@ -164,8 +170,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, if (fd < 0) { if (!optional) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); + } } return false; @@ -174,8 +182,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id); if (ret) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); + } success = false; goto out; } @@ -183,8 +193,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance); + } success = false; goto out; } @@ -200,7 +212,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - if(report_fail) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); + } /* this is frickin' fatal */ success = false; goto out; @@ -506,7 +520,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* ---- Global Navigation Satellite System receiver ---- */ if (checkGNSS) { - if(!gnssCheck(mavlink_fd, reportFailures)) { + if (!gnssCheck(mavlink_fd, reportFailures)) { failed = true; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e89626300e..02b919b524 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1034,7 +1034,7 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; - status.condition_system_prearm_error_reported = true; + status.condition_system_prearm_error_reported = false; status.condition_system_hotplug_timeout = false; status.counter++; @@ -2197,10 +2197,12 @@ int commander_thread_main(int argc, char *argv[]) mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i); } else if (telemetry_last_dl_loss[i] == 0) { /* new link */ - status.condition_system_prearm_error_reported = false; - status_changed = true; } + /* got link again or new */ + status.condition_system_prearm_error_reported = false; + status_changed = true; + telemetry_lost[i] = false; have_link = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c2f7126e85..478bad7595 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -271,6 +271,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* reset feedback state */ if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR && + status->arming_state != vehicle_status_s::ARMING_STATE_INIT && valid_transition) { status->condition_system_prearm_error_reported = false; } @@ -745,8 +746,8 @@ int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool { /* */ - bool reportFailures = force_report || (!status->data_link_lost && - !status->condition_system_prearm_error_reported && status->condition_system_hotplug_timeout); + bool reportFailures = force_report || (!status->condition_system_prearm_error_reported && + status->condition_system_hotplug_timeout); bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not @@ -755,7 +756,9 @@ int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool checkAirspeed = true; } - bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures); + bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, + checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), + !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures); if (!status->cb_usb && status->usb_connected && prearm) { preflight_ok = false; From e326e190cb85d142736aaa70dae66c48841322a9 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 19 Nov 2015 07:26:59 -1000 Subject: [PATCH 132/136] BUGFIX: Multiple PREFLIGHT FAIL: xxx messages - multiline macros not wrapped in do/while --- src/include/mavlink/mavlink_log.h | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index 13a6b16b34..f0141d8a17 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -106,10 +106,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ -#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \ - fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); +#define mavlink_and_console_log_emergency(_fd, _text, ...) do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); } while(0); /** * Send a mavlink critical message and print to console. @@ -117,10 +117,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ -#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \ - fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); +#define mavlink_and_console_log_critical(_fd, _text, ...) do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); } while(0); /** * Send a mavlink emergency message and print to console. @@ -128,10 +128,10 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ -#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \ - fprintf(stderr, "telem> "); \ - fprintf(stderr, _text, ##__VA_ARGS__); \ - fprintf(stderr, "\n"); +#define mavlink_and_console_log_info(_fd, _text, ...) do { mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); } while(0); struct mavlink_logmessage { char text[MAVLINK_LOG_MAXLEN + 1]; From 2ffccc5c9f1667aae5a2908e66e8a39df7faa15e Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 19 Nov 2015 07:50:45 -1000 Subject: [PATCH 133/136] BUGFIX:CLI "commander calibrate acel" resulted in hardfault --- src/modules/commander/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index c8ede875b6..4239797e0a 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -37,7 +37,7 @@ endif() px4_add_module( MODULE modules__commander MAIN commander - STACK 3000 + STACK 4096 COMPILE_FLAGS ${MODULE_CFLAGS} -Os From c51f414b222c95189e0e472fa4b49a8ad7d5096c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Nov 2015 19:09:42 +0100 Subject: [PATCH 134/136] Vehicle status remove unused field --- msg/vehicle_status.msg | 1 - 1 file changed, 1 deletion(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index c91672c0be..60ab34e90d 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -141,7 +141,6 @@ bool rc_signal_lost_cmd # true if RC lost mode is commanded bool rc_input_blocked # set if RC input should be ignored temporarily uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping. -bool data_link_found_new # new datalink to GCS found bool data_link_lost # datalink to GCS lost bool data_link_lost_cmd # datalink to GCS lost mode commanded uint8 data_link_lost_counter # counts unique data link lost events From d5c89c53923d0eacfcc1614d07887b7db77e383b Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 19 Nov 2015 11:43:11 -1000 Subject: [PATCH 135/136] BUGFIX:Take 2! CLI "commander calibrate acel" resulted in hardfault Processes: 20 total, 4 running, 16 sleeping CPU usage: 65.19% tasks, 0.56% sched, 34.24% idle Uptime: 387.573s total, 135.045s idle PID COMMAND CPU(ms) CPU(%) USED/STACK PRIO(BASE) STATE 0 Idle Task 135045 34.242 0/ 0 0 ( 0) READY 1 hpwork 7495 1.693 956/ 1592 192 (192) w:sig 2 lpwork 1576 0.376 572/ 1592 50 ( 50) w:sig 3 init 1655 0.000 1404/ 2496 100 (100) w:sem 170 top 9 0.000 1204/ 1696 100 (100) RUN 87 dataman 1 0.000 652/ 1192 90 ( 90) w:sem 105 sensors 31810 8.090 1644/ 1992 250 (250) w:sem 107 gps 1217 0.000 708/ 1192 220 (220) w:sem 109 commander 49016 17.027 3412/ 3596 215 (215) w:sig +100 184 head room 114 mavlink_if0 2607 0.564 2092/ 2392 100 (100) READY 115 mavlink_rcv_if0 27 0.000 804/ 2096 175 (175) w:sem 122 sdlog2 849 0.188 2068/ 2992 70 ( 70) READY 125 commander_low_prio 4678 0.000 2740/ 2872 50 ( 50) w:sem +200 132 head room 142 attitude_estimator_q 65555 16.933 1956/ 2096 250 (250) w:sem 144 position_estimator_inav 23877 6.208 4588/ 4992 250 (250) w:sem 148 mc_att_control 31210 7.902 1132/ 1496 250 (250) w:sem 150 mc_pos_control 2901 0.658 1044/ 1496 250 (250) w:sem 155 navigator 2219 0.470 828/ 1496 105 (105) w:sem 165 mavlink_if1 21195 5.079 2156/ 2392 100 (100) w:sig 167 mavlink_rcv_if1 116 0.000 1236/ 2096 175 (175) w:sem --- 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 02b919b524..49e6459b03 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -301,7 +301,7 @@ int commander_main(int argc, char *argv[]) daemon_task = px4_task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3500, + 3600, commander_thread_main, (char * const *)&argv[0]); @@ -1304,7 +1304,7 @@ int commander_thread_main(int argc, char *argv[]) /* initialize low priority thread */ pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2600); + pthread_attr_setstacksize(&commander_low_prio_attr, 2880); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); From 71320ac0fef7b5d9282a6b245adc14df2ce67725 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Nov 2015 09:00:44 +0100 Subject: [PATCH 136/136] Commander: Add decimal hints --- src/modules/commander/commander_params.c | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 1c4108017f..4b06ecbdf4 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -55,6 +55,7 @@ * @group Radio Calibration * @min -0.25 * @max 0.25 + * @decimal 2 */ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); @@ -69,6 +70,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); * @group Radio Calibration * @min -0.25 * @max 0.25 + * @decimal 2 */ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); @@ -83,6 +85,7 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); * @group Radio Calibration * @min -0.25 * @max 0.25 + * @decimal 2 */ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); @@ -93,6 +96,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); * * @group Battery Calibration * @unit V + * @decimal 2 */ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); @@ -103,6 +107,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); * * @group Battery Calibration * @unit V + * @decimal 2 */ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); @@ -115,6 +120,8 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); * @group Battery Calibration * @unit V * @min 0.0 + * @max 1.5 + * @decimal 2 */ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); @@ -137,6 +144,7 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); * * @group Battery Calibration * @unit mA + * @decimal 0 */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); @@ -184,6 +192,7 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); * @group Commander * @min 0.0 * @max 1.0 + * @decimal 1 */ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); @@ -196,6 +205,7 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); * @min 0.0 * @max 30.0 * @unit ampere + * @decimal 2 */ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); @@ -209,6 +219,7 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * @unit second * @min 0.0 * @max 60.0 + * @decimal 1 */ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); @@ -221,6 +232,7 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); * @unit second * @min 0 * @max 35 + * @decimal 1 */ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f); @@ -233,6 +245,7 @@ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f); * @unit meter * @min 2 * @max 15 + * @decimal 2 */ PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f); @@ -245,6 +258,7 @@ PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f); * @unit meter * @min 5 * @max 25 + * @decimal 2 */ PARAM_DEFINE_FLOAT(COM_HOME_V_T, 10.0f);