Lorenz Meier
10 years ago
25 changed files with 1384 additions and 237 deletions
@ -0,0 +1,50 @@
@@ -0,0 +1,50 @@
|
||||
#!nsh |
||||
# |
||||
# @name Generic AAERT tailplane airframe with Quad VTOL. |
||||
# |
||||
# @type Standard VTOL |
||||
# |
||||
# @maintainer Simon Wilks <simon@uaventure.com> |
||||
# |
||||
|
||||
sh /etc/init.d/rc.vtol_defaults |
||||
|
||||
if [ $AUTOCNF == yes ] |
||||
then |
||||
param set VT_TYPE 2 |
||||
param set VT_MOT_COUNT 4 |
||||
param set VT_TRANS_THR 0.75 |
||||
|
||||
param set MC_ROLL_P 7.0 |
||||
param set MC_ROLLRATE_P 0.15 |
||||
param set MC_ROLLRATE_I 0.002 |
||||
param set MC_ROLLRATE_D 0.003 |
||||
param set MC_ROLLRATE_FF 0.0 |
||||
param set MC_PITCH_P 7.0 |
||||
param set MC_PITCHRATE_P 0.12 |
||||
param set MC_PITCHRATE_I 0.002 |
||||
param set MC_PITCHRATE_D 0.003 |
||||
param set MC_PITCHRATE_FF 0.0 |
||||
param set MC_YAW_P 2.8 |
||||
param set MC_YAW_FF 0.5 |
||||
param set MC_YAWRATE_P 0.22 |
||||
param set MC_YAWRATE_I 0.02 |
||||
param set MC_YAWRATE_D 0.0 |
||||
param set MC_YAWRATE_FF 0.0 |
||||
fi |
||||
|
||||
set MIXER vtol_quad_x |
||||
set PWM_OUT 12345678 |
||||
|
||||
set MIXER_AUX vtol_AAERT |
||||
set PWM_AUX_RATE 50 |
||||
set PWM_AUX_OUT 1234 |
||||
set PWM_AUX_DISARMED 1000 |
||||
set PWM_AUX_MIN 1000 |
||||
set PWM_AUX_MAX 2000 |
||||
|
||||
set MAV_TYPE 22 |
||||
|
||||
param set VT_MOT_COUNT 4 |
||||
param set VT_IDLE_PWM_MC 1080 |
||||
param set VT_TYPE 2 |
||||
@ -1 +1 @@
@@ -1 +1 @@
|
||||
Subproject commit 988e404586900754e7e1748c87c0ec12b757cf87 |
||||
Subproject commit 3ae5400aa5ead18139106d30f730114d5e9b65dd |
||||
@ -0,0 +1,293 @@
@@ -0,0 +1,293 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2014 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include <nuttx/config.h> |
||||
|
||||
#include <cstdlib> |
||||
#include <cstring> |
||||
#include <fcntl.h> |
||||
#include <memory> |
||||
#include <pthread.h> |
||||
#include <systemlib/err.h> |
||||
#include <systemlib/systemlib.h> |
||||
#include <systemlib/param/param.h> |
||||
#include <systemlib/mixer/mixer.h> |
||||
#include <systemlib/board_serial.h> |
||||
#include <systemlib/scheduling_priorities.h> |
||||
#include <systemlib/git_version.h> |
||||
#include <version/version.h> |
||||
#include <arch/board/board.h> |
||||
#include <arch/chip/chip.h> |
||||
|
||||
#include "uavcan_main.hpp" |
||||
#include "uavcan_servers.hpp" |
||||
#include "uavcan_virtual_can_driver.hpp" |
||||
|
||||
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp> |
||||
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp> |
||||
# include <uavcan_posix/firmware_version_checker.hpp> |
||||
|
||||
//todo:The Inclusion of file_server_backend is killing
|
||||
// #include <sys/types.h> and leaving OK undefined
|
||||
# define OK 0 |
||||
|
||||
/**
|
||||
* @file uavcan_servers.cpp |
||||
* |
||||
* Implements basic functionality of UAVCAN node. |
||||
* |
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
* David Sidrane <david_s5@nscdg.com> |
||||
*/ |
||||
|
||||
/*
|
||||
* UavcanNode |
||||
*/ |
||||
UavcanServers *UavcanServers::_instance; |
||||
UavcanServers::UavcanServers(uavcan::INode &main_node) : |
||||
_subnode_thread(-1), |
||||
_vdriver(UAVCAN_STM32_NUM_IFACES, uavcan_stm32::SystemClock::instance()), |
||||
_subnode(_vdriver, uavcan_stm32::SystemClock::instance()), |
||||
_main_node(main_node), |
||||
_tracer(), |
||||
_storage_backend(), |
||||
_fw_version_checker(), |
||||
_server_instance(_subnode, _storage_backend, _tracer), |
||||
_fileserver_backend(_subnode), |
||||
_node_info_retriever(_subnode), |
||||
_fw_upgrade_trigger(_subnode, _fw_version_checker), |
||||
_fw_server(_subnode, _fileserver_backend), |
||||
_mutex_inited(false), |
||||
_check_fw(false) |
||||
|
||||
{ |
||||
} |
||||
|
||||
UavcanServers::~UavcanServers() |
||||
{ |
||||
if (_mutex_inited) { |
||||
(void)Lock::deinit(_subnode_mutex); |
||||
} |
||||
_main_node.getDispatcher().removeRxFrameListener(); |
||||
} |
||||
|
||||
int UavcanServers::stop(void) |
||||
{ |
||||
UavcanServers *server = instance(); |
||||
|
||||
if (server == nullptr) { |
||||
warnx("Already stopped"); |
||||
return -1; |
||||
} |
||||
|
||||
_instance = nullptr; |
||||
|
||||
if (server->_subnode_thread != -1) { |
||||
pthread_cancel(server->_subnode_thread); |
||||
pthread_join(server->_subnode_thread, NULL); |
||||
} |
||||
|
||||
server->_main_node.getDispatcher().removeRxFrameListener(); |
||||
|
||||
delete server; |
||||
return 0; |
||||
} |
||||
|
||||
int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node) |
||||
{ |
||||
if (_instance != nullptr) { |
||||
warnx("Already started"); |
||||
return -1; |
||||
} |
||||
|
||||
/*
|
||||
* Node init |
||||
*/ |
||||
_instance = new UavcanServers(main_node); |
||||
|
||||
if (_instance == nullptr) { |
||||
warnx("Out of memory"); |
||||
return -2; |
||||
} |
||||
|
||||
int rv = _instance->init(num_ifaces); |
||||
|
||||
if (rv < 0) { |
||||
warnx("Node init failed: %d", rv); |
||||
delete _instance; |
||||
_instance = nullptr; |
||||
return rv; |
||||
} |
||||
|
||||
/*
|
||||
* Start the thread. Normally it should never exit. |
||||
*/ |
||||
pthread_attr_t tattr; |
||||
struct sched_param param; |
||||
|
||||
pthread_attr_init(&tattr); |
||||
tattr.stacksize = StackSize; |
||||
param.sched_priority = Priority; |
||||
pthread_attr_setschedparam(&tattr, ¶m); |
||||
|
||||
static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);}; |
||||
|
||||
rv = pthread_create(&_instance->_subnode_thread, &tattr, static_cast<pthread_startroutine_t>(run_trampoline), NULL); |
||||
|
||||
if (rv < 0) { |
||||
warnx("pthread_create() failed: %d", errno); |
||||
rv = -errno; |
||||
delete _instance; |
||||
_instance = nullptr; |
||||
} |
||||
|
||||
return rv; |
||||
} |
||||
|
||||
int UavcanServers::init(unsigned num_ifaces) |
||||
{ |
||||
errno = 0; |
||||
|
||||
/*
|
||||
* Initialize the mutex. |
||||
* giving it its path |
||||
*/ |
||||
|
||||
int ret = Lock::init(_subnode_mutex); |
||||
|
||||
if (ret < 0) { |
||||
warnx("Lock init: %d", errno); |
||||
return ret; |
||||
} |
||||
|
||||
_mutex_inited = true; |
||||
|
||||
_subnode.setNodeID(_main_node.getNodeID()); |
||||
_main_node.getDispatcher().installRxFrameListener(&_vdriver); |
||||
|
||||
|
||||
/*
|
||||
* Initialize the fw version checker. |
||||
* giving it its path |
||||
*/ |
||||
ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH); |
||||
|
||||
if (ret < 0) { |
||||
warnx("FirmwareVersionChecker init: %d, errno: %d", ret, errno); |
||||
return ret; |
||||
} |
||||
|
||||
/* Start fw file server back */ |
||||
|
||||
ret = _fw_server.start(); |
||||
|
||||
if (ret < 0) { |
||||
warnx("BasicFileServer init: %d, errno: %d", ret, errno); |
||||
return ret; |
||||
} |
||||
|
||||
/* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */ |
||||
|
||||
ret = _storage_backend.init(UAVCAN_NODE_DB_PATH); |
||||
|
||||
if (ret < 0) { |
||||
warnx("FileStorageBackend init: %d, errno: %d", ret, errno); |
||||
return ret; |
||||
} |
||||
|
||||
/* Initialize trace in the UAVCAN_NODE_DB_PATH directory */ |
||||
|
||||
ret = _tracer.init(UAVCAN_LOG_FILE); |
||||
|
||||
if (ret < 0) { |
||||
warnx("FileEventTracer init: %d, errno: %d", ret, errno); |
||||
return ret; |
||||
} |
||||
|
||||
/* hardware version */ |
||||
uavcan::protocol::HardwareVersion hwver; |
||||
UavcanNode::getHardwareVersion(hwver); |
||||
|
||||
/* Initialize the dynamic node id server */ |
||||
ret = _server_instance.init(hwver.unique_id); |
||||
|
||||
if (ret < 0) { |
||||
warnx("CentralizedServer init: %d", ret); |
||||
return ret; |
||||
} |
||||
|
||||
/* Start node info retriever to fetch node info from new nodes */ |
||||
|
||||
ret = _node_info_retriever.start(); |
||||
|
||||
if (ret < 0) { |
||||
warnx("NodeInfoRetriever init: %d", ret); |
||||
return ret; |
||||
} |
||||
|
||||
/* Start the fw version checker */ |
||||
|
||||
ret = _fw_upgrade_trigger.start(_node_info_retriever, _fw_version_checker.getFirmwarePath()); |
||||
|
||||
if (ret < 0) { |
||||
warnx("FirmwareUpdateTrigger init: %d", ret); |
||||
return ret; |
||||
} |
||||
|
||||
/* Start the Node */ |
||||
|
||||
return OK; |
||||
} |
||||
__attribute__((optimize("-O0"))) |
||||
pthread_addr_t UavcanServers::run(pthread_addr_t) |
||||
{ |
||||
Lock lock(_subnode_mutex); |
||||
|
||||
while (1) { |
||||
|
||||
if (_check_fw == true) { |
||||
_check_fw = false; |
||||
_node_info_retriever.invalidateAll(); |
||||
} |
||||
|
||||
const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(100)); |
||||
|
||||
if (spin_res < 0) { |
||||
warnx("node spin error %i", spin_res); |
||||
} |
||||
} |
||||
|
||||
warnx("exiting."); |
||||
return (pthread_addr_t) 0; |
||||
} |
||||
|
||||
@ -0,0 +1,145 @@
@@ -0,0 +1,145 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2014 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <nuttx/config.h> |
||||
|
||||
#include <uavcan_stm32/uavcan_stm32.hpp> |
||||
#include <drivers/device/device.h> |
||||
#include <systemlib/perf_counter.h> |
||||
|
||||
#include <uavcan/node/sub_node.hpp> |
||||
#include <uavcan/protocol/node_status_monitor.hpp> |
||||
|
||||
# include <uavcan/protocol/dynamic_node_id_server/centralized.hpp> |
||||
# include <uavcan/protocol/node_info_retriever.hpp> |
||||
# include <uavcan_posix/basic_file_server_backend.hpp> |
||||
# include <uavcan/protocol/firmware_update_trigger.hpp> |
||||
# include <uavcan/protocol/file_server.hpp> |
||||
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp> |
||||
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp> |
||||
# include <uavcan_posix/firmware_version_checker.hpp> |
||||
|
||||
# include "uavcan_virtual_can_driver.hpp" |
||||
|
||||
/**
|
||||
* @file uavcan_servers.hpp |
||||
* |
||||
* Defines basic functionality of UAVCAN node. |
||||
* |
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
* @author David Sidrane <david_s5@nscdg.com> |
||||
*/ |
||||
|
||||
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" |
||||
#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db" |
||||
#define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw" |
||||
#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log" |
||||
|
||||
/**
|
||||
* A UAVCAN Server Sub node. |
||||
*/ |
||||
class UavcanServers |
||||
{ |
||||
static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize; |
||||
|
||||
static constexpr unsigned MaxCanFramsPerTransfer = 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 = |
||||
(UAVCAN_STM32_NUM_IFACES * uavcan::MemPoolBlockSize *MaxCanFramsPerTransfer); |
||||
|
||||
static constexpr unsigned StackSize = 3500; |
||||
static constexpr unsigned Priority = 120; |
||||
|
||||
typedef uavcan::SubNode<MemPoolSize> SubNode; |
||||
|
||||
public: |
||||
UavcanServers(uavcan::INode &main_node); |
||||
|
||||
virtual ~UavcanServers(); |
||||
|
||||
static int start(unsigned num_ifaces, uavcan::INode &main_node); |
||||
static int stop(void); |
||||
|
||||
SubNode &get_node() { return _subnode; } |
||||
|
||||
static UavcanServers *instance() { return _instance; } |
||||
|
||||
/*
|
||||
* Set the main node's pointer to to the injector |
||||
* This is a work around as main_node.getDispatcher().remeveRxFrameListener(); |
||||
* would require a dynamic cast and rtti is not enabled. |
||||
*/ |
||||
void attachITxQueueInjector(ITxQueueInjector **injector) {*injector = &_vdriver;} |
||||
|
||||
void requestCheckAllNodesFirmwareAndUpdate() { _check_fw = true; } |
||||
|
||||
private: |
||||
pthread_t _subnode_thread; |
||||
pthread_mutex_t _subnode_mutex; |
||||
|
||||
int init(unsigned num_ifaces); |
||||
void deinit(void); |
||||
|
||||
pthread_addr_t run(pthread_addr_t); |
||||
|
||||
static UavcanServers *_instance; ///< singleton pointer
|
||||
|
||||
typedef VirtualCanDriver<QueuePoolSize> vCanDriver; |
||||
|
||||
vCanDriver _vdriver; |
||||
|
||||
uavcan::SubNode<MemPoolSize> _subnode; ///< library instance
|
||||
uavcan::INode &_main_node; ///< library instance
|
||||
|
||||
uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer; |
||||
uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend; |
||||
uavcan_posix::FirmwareVersionChecker _fw_version_checker; |
||||
uavcan::dynamic_node_id_server::CentralizedServer _server_instance; ///< server singleton pointer
|
||||
uavcan_posix::BasicFileSeverBackend _fileserver_backend; |
||||
uavcan::NodeInfoRetriever _node_info_retriever; |
||||
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; |
||||
uavcan::BasicFileServer _fw_server; |
||||
|
||||
bool _mutex_inited; |
||||
volatile bool _check_fw; |
||||
|
||||
}; |
||||
@ -0,0 +1,506 @@
@@ -0,0 +1,506 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved. |
||||
* Pavel Kirienko <pavel.kirienko@zubax.com> |
||||
* David Sidrane <david_s5@nscdg.com> |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <nuttx/config.h> |
||||
|
||||
#include <cstdlib> |
||||
#include <cstdint> |
||||
#include <cstring> |
||||
#include <cstdio> |
||||
#include <fcntl.h> |
||||
#include <pthread.h> |
||||
#include <semaphore.h> |
||||
#include <debug.h> |
||||
|
||||
#include <uavcan_stm32/clock.hpp> |
||||
#include <uavcan/node/sub_node.hpp> |
||||
#include <uavcan/protocol/node_status_monitor.hpp> |
||||
|
||||
/*
|
||||
* General purpose wrapper around os's mutual exclusion |
||||
* mechanism. |
||||
* |
||||
* It supports the |
||||
* |
||||
* Note the naming of thier_mutex_ implies that the underlying |
||||
* mutex is owned by class using the Lock. |
||||
* and this wrapper provides service to initialize and de initialize |
||||
* the mutex. |
||||
*/ |
||||
class Lock |
||||
{ |
||||
pthread_mutex_t &thier_mutex_; |
||||
|
||||
public: |
||||
|
||||
Lock(pthread_mutex_t &m) : |
||||
thier_mutex_(m) |
||||
{ |
||||
(void)pthread_mutex_lock(&m); |
||||
} |
||||
|
||||
~Lock() |
||||
{ |
||||
(void)pthread_mutex_unlock(&thier_mutex_); |
||||
} |
||||
|
||||
static int init(pthread_mutex_t &thier_mutex_) |
||||
{ |
||||
return pthread_mutex_init(&thier_mutex_, NULL); |
||||
} |
||||
|
||||
static int deinit(pthread_mutex_t &thier_mutex_) |
||||
{ |
||||
return pthread_mutex_destroy(&thier_mutex_); |
||||
} |
||||
|
||||
}; |
||||
|
||||
/**
|
||||
* Generic queue based on the linked list class defined in libuavcan. |
||||
* This class does not use heap memory. |
||||
*/ |
||||
template <typename T> |
||||
class Queue |
||||
{ |
||||
struct Item : public uavcan::LinkedListNode<Item> { |
||||
T payload; |
||||
|
||||
template <typename... Args> |
||||
Item(Args... args) : payload(args...) { } |
||||
}; |
||||
|
||||
uavcan::LimitedPoolAllocator allocator_; |
||||
uavcan::LinkedListRoot<Item> list_; |
||||
|
||||
public: |
||||
Queue(uavcan::IPoolAllocator &arg_allocator, std::size_t block_allocation_quota) : |
||||
allocator_(arg_allocator, block_allocation_quota) |
||||
{ |
||||
uavcan::IsDynamicallyAllocatable<Item>::check(); |
||||
} |
||||
|
||||
bool isEmpty() const { return list_.isEmpty(); } |
||||
|
||||
/**
|
||||
* Creates one item in-place at the end of the list. |
||||
* Returns true if the item was appended successfully, false if there's not enough memory. |
||||
* Complexity is O(N) where N is queue length. |
||||
*/ |
||||
template <typename... Args> |
||||
bool tryEmplace(Args... args) |
||||
{ |
||||
// Allocating memory
|
||||
void *const ptr = allocator_.allocate(sizeof(Item)); |
||||
|
||||
if (ptr == nullptr) { |
||||
return false; |
||||
} |
||||
|
||||
// Constructing the new item
|
||||
Item *const item = new(ptr) Item(args...); |
||||
assert(item != nullptr); |
||||
|
||||
// Inserting the new item at the end of the list
|
||||
Item *p = list_.get(); |
||||
|
||||
if (p == nullptr) { |
||||
list_.insert(item); |
||||
|
||||
} else { |
||||
while (p->getNextListNode() != nullptr) { |
||||
p = p->getNextListNode(); |
||||
} |
||||
|
||||
assert(p->getNextListNode() == nullptr); |
||||
p->setNextListNode(item); |
||||
assert(p->getNextListNode()->getNextListNode() == nullptr); |
||||
} |
||||
|
||||
return true; |
||||
} |
||||
|
||||
/**
|
||||
* Accesses the first element. |
||||
* Nullptr will be returned if the queue is empty. |
||||
* Complexity is O(1). |
||||
*/ |
||||
T *peek() { return isEmpty() ? nullptr : &list_.get()->payload; } |
||||
const T *peek() const { return isEmpty() ? nullptr : &list_.get()->payload; } |
||||
|
||||
/**
|
||||
* Removes the first element. |
||||
* If the queue is empty, nothing will be done and assertion failure will be triggered. |
||||
* Complexity is O(1). |
||||
*/ |
||||
void pop() |
||||
{ |
||||
Item *const item = list_.get(); |
||||
assert(item != nullptr); |
||||
|
||||
if (item != nullptr) { |
||||
list_.remove(item); |
||||
item->~Item(); |
||||
allocator_.deallocate(item); |
||||
} |
||||
} |
||||
}; |
||||
|
||||
/**
|
||||
* Objects of this class are owned by the sub-node thread. |
||||
* This class does not use heap memory. |
||||
*/ |
||||
class VirtualCanIface : public uavcan::ICanIface, |
||||
uavcan::Noncopyable |
||||
{ |
||||
/**
|
||||
* This class re-defines uavcan::RxCanFrame with flags. |
||||
* Simple inheritance or composition won't work here, because the 40 byte limit will be exceeded, |
||||
* rendering this class unusable with Queue<>. |
||||
*/ |
||||
struct RxItem: public uavcan::CanFrame { |
||||
const uavcan::MonotonicTime ts_mono; |
||||
const uavcan::UtcTime ts_utc; |
||||
const uavcan::CanIOFlags flags; |
||||
const uint8_t iface_index; |
||||
|
||||
RxItem(const uavcan::CanRxFrame &arg_frame, uavcan::CanIOFlags arg_flags) : |
||||
uavcan::CanFrame(arg_frame), |
||||
ts_mono(arg_frame.ts_mono), |
||||
ts_utc(arg_frame.ts_utc), |
||||
flags(arg_flags), |
||||
iface_index(arg_frame.iface_index) |
||||
{ |
||||
// Making sure it will fit into a pool block with a pointer prefix
|
||||
static_assert(sizeof(RxItem) <= (uavcan::MemPoolBlockSize - 8), "Bad coder, no coffee"); |
||||
} |
||||
}; |
||||
|
||||
pthread_mutex_t &common_driver_mutex_; |
||||
|
||||
uavcan::CanTxQueue prioritized_tx_queue_; |
||||
Queue<RxItem> rx_queue_; |
||||
|
||||
int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) override |
||||
{ |
||||
Lock lock(common_driver_mutex_); |
||||
prioritized_tx_queue_.push(frame, tx_deadline, uavcan::CanTxQueue::Volatile, flags); |
||||
return 1; |
||||
} |
||||
|
||||
int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic, |
||||
uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags) override |
||||
{ |
||||
Lock lock(common_driver_mutex_); |
||||
|
||||
if (rx_queue_.isEmpty()) { |
||||
return 0; |
||||
} |
||||
|
||||
const auto item = *rx_queue_.peek(); |
||||
rx_queue_.pop(); |
||||
|
||||
out_frame = item; |
||||
out_ts_monotonic = item.ts_mono; |
||||
out_ts_utc = item.ts_utc; |
||||
out_flags = item.flags; |
||||
|
||||
return 1; |
||||
} |
||||
|
||||
int16_t configureFilters(const uavcan::CanFilterConfig *, std::uint16_t) override { return -uavcan::ErrDriver; } |
||||
uint16_t getNumFilters() const override { return 0; } |
||||
uint64_t getErrorCount() const override { return 0; } |
||||
|
||||
public: |
||||
VirtualCanIface(uavcan::IPoolAllocator &allocator, uavcan::ISystemClock &clock, |
||||
pthread_mutex_t &arg_mutex, unsigned quota_per_queue) : |
||||
common_driver_mutex_(arg_mutex), |
||||
prioritized_tx_queue_(allocator, clock, quota_per_queue), |
||||
rx_queue_(allocator, quota_per_queue) |
||||
{ |
||||
} |
||||
|
||||
~VirtualCanIface() |
||||
{ |
||||
} |
||||
|
||||
/**
|
||||
* Note that RX queue overwrites oldest items when overflowed. |
||||
* Call this from the main thread only. |
||||
* No additional locking is required. |
||||
*/ |
||||
void addRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) |
||||
{ |
||||
Lock lock(common_driver_mutex_); |
||||
|
||||
if (!rx_queue_.tryEmplace(frame, flags) && !rx_queue_.isEmpty()) { |
||||
rx_queue_.pop(); |
||||
(void)rx_queue_.tryEmplace(frame, flags); |
||||
} |
||||
} |
||||
|
||||
/**
|
||||
* Call this from the main thread only. |
||||
* No additional locking is required. |
||||
*/ |
||||
void flushTxQueueTo(uavcan::INode &main_node, std::uint8_t iface_index) |
||||
{ |
||||
Lock lock(common_driver_mutex_); |
||||
const std::uint8_t iface_mask = static_cast<std::uint8_t>(1U << iface_index); |
||||
|
||||
while (auto e = prioritized_tx_queue_.peek()) { |
||||
UAVCAN_TRACE("VirtualCanIface", "TX injection [iface=0x%02x]: %s", |
||||
unsigned(iface_mask), e->toString().c_str()); |
||||
|
||||
const int res = main_node.injectTxFrame(e->frame, e->deadline, iface_mask, |
||||
uavcan::CanTxQueue::Qos(e->qos), e->flags); |
||||
|
||||
prioritized_tx_queue_.remove(e); |
||||
|
||||
if (res <= 0) { |
||||
break; |
||||
} |
||||
|
||||
} |
||||
} |
||||
|
||||
/**
|
||||
* Call this from the sub-node thread only. |
||||
* No additional locking is required. |
||||
*/ |
||||
bool hasDataInRxQueue() |
||||
{ |
||||
Lock lock(common_driver_mutex_); |
||||
return !rx_queue_.isEmpty(); |
||||
} |
||||
}; |
||||
|
||||
/**
|
||||
* This interface defines one method that will be called by the main node thread periodically in order to |
||||
* transfer contents of TX queue of the sub-node into the TX queue of the main node. |
||||
*/ |
||||
class ITxQueueInjector |
||||
{ |
||||
public: |
||||
virtual ~ITxQueueInjector() { } |
||||
|
||||
/**
|
||||
* Flush contents of TX queues into the main node. |
||||
* @param main_node Reference to the main node. |
||||
*/ |
||||
virtual void injectTxFramesInto(uavcan::INode &main_node) = 0; |
||||
}; |
||||
|
||||
/**
|
||||
* 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 <unsigned SharedMemoryPoolSize> |
||||
class VirtualCanDriver : public uavcan::ICanDriver, |
||||
public uavcan::IRxFrameListener, |
||||
public ITxQueueInjector, |
||||
uavcan::Noncopyable |
||||
{ |
||||
class Event |
||||
{ |
||||
FAR sem_t sem; |
||||
|
||||
|
||||
public: |
||||
|
||||
int init() |
||||
{ |
||||
return sem_init(&sem, 0, 0); |
||||
} |
||||
|
||||
int deinit() |
||||
{ |
||||
return sem_destroy(&sem); |
||||
} |
||||
|
||||
|
||||
Event() |
||||
{ |
||||
} |
||||
|
||||
~Event() |
||||
{ |
||||
} |
||||
|
||||
|
||||
/**
|
||||
*/ |
||||
|
||||
void waitFor(uavcan::MonotonicDuration duration) |
||||
{ |
||||
static const unsigned NsPerSec = 1000000000; |
||||
|
||||
if (duration.isPositive()) { |
||||
auto abstime = ::timespec(); |
||||
|
||||
if (clock_gettime(CLOCK_REALTIME, &abstime) >= 0) { |
||||
abstime.tv_nsec += duration.toUSec() * 1000; |
||||
|
||||
if (abstime.tv_nsec >= NsPerSec) { |
||||
abstime.tv_sec++; |
||||
abstime.tv_nsec -= NsPerSec; |
||||
} |
||||
|
||||
(void)sem_timedwait(&sem, &abstime); |
||||
} |
||||
} |
||||
} |
||||
|
||||
void signal() |
||||
{ |
||||
int count; |
||||
int rv = sem_getvalue(&sem, &count); |
||||
|
||||
if (rv > 0 && count <= 0) { |
||||
sem_post(&sem); |
||||
} |
||||
} |
||||
}; |
||||
|
||||
Event event_; ///< Used to unblock the select() call when IO happens.
|
||||
pthread_mutex_t driver_mutex_; ///< Shared across all ifaces
|
||||
uavcan::PoolAllocator<SharedMemoryPoolSize, uavcan::MemPoolBlockSize> allocator_; ///< Shared across all ifaces
|
||||
uavcan::LazyConstructor<VirtualCanIface> ifaces_[uavcan::MaxCanIfaces]; |
||||
const unsigned num_ifaces_; |
||||
uavcan::ISystemClock &clock_; |
||||
|
||||
uavcan::ICanIface *getIface(uint8_t iface_index) override |
||||
{ |
||||
return (iface_index < num_ifaces_) ? ifaces_[iface_index].operator VirtualCanIface * () : nullptr; |
||||
} |
||||
|
||||
uint8_t getNumIfaces() const override { return num_ifaces_; } |
||||
|
||||
/**
|
||||
* This and other methods of ICanDriver will be invoked by the sub-node thread. |
||||
*/ |
||||
int16_t select(uavcan::CanSelectMasks &inout_masks, |
||||
const uavcan::CanFrame * (&)[uavcan::MaxCanIfaces], |
||||
uavcan::MonotonicTime blocking_deadline) override |
||||
{ |
||||
bool need_block = (inout_masks.write == 0); // Write queue is infinite
|
||||
|
||||
for (unsigned i = 0; need_block && (i < num_ifaces_); i++) { |
||||
const bool need_read = inout_masks.read & (1U << i); |
||||
|
||||
if (need_read && ifaces_[i]->hasDataInRxQueue()) { |
||||
need_block = false; |
||||
} |
||||
} |
||||
|
||||
if (need_block) { |
||||
event_.waitFor(blocking_deadline - clock_.getMonotonic()); |
||||
} |
||||
|
||||
inout_masks = uavcan::CanSelectMasks(); |
||||
|
||||
for (unsigned i = 0; i < num_ifaces_; i++) { |
||||
const std::uint8_t iface_mask = 1U << i; |
||||
inout_masks.write |= iface_mask; // Always ready to write
|
||||
|
||||
if (ifaces_[i]->hasDataInRxQueue()) { |
||||
inout_masks.read |= iface_mask; |
||||
} |
||||
} |
||||
|
||||
return num_ifaces_; // We're always ready to write, hence > 0.
|
||||
} |
||||
|
||||
/**
|
||||
* This handler will be invoked by the main node thread. |
||||
*/ |
||||
void handleRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) override |
||||
{ |
||||
UAVCAN_TRACE("VirtualCanDriver", "RX [flags=%u]: %s", unsigned(flags), frame.toString().c_str()); |
||||
|
||||
if (frame.iface_index < num_ifaces_) { |
||||
ifaces_[frame.iface_index]->addRxFrame(frame, flags); |
||||
event_.signal(); |
||||
|
||||
} |
||||
} |
||||
|
||||
/**
|
||||
* This method will be invoked by the main node thread. |
||||
*/ |
||||
void injectTxFramesInto(uavcan::INode &main_node) override |
||||
{ |
||||
for (unsigned i = 0; i < num_ifaces_; i++) { |
||||
ifaces_[i]->flushTxQueueTo(main_node, i); |
||||
} |
||||
|
||||
event_.signal(); |
||||
} |
||||
|
||||
public: |
||||
VirtualCanDriver(unsigned arg_num_ifaces, uavcan::ISystemClock &system_clock) : |
||||
num_ifaces_(arg_num_ifaces), |
||||
clock_(system_clock) |
||||
{ |
||||
Lock::init(driver_mutex_); |
||||
event_.init(); |
||||
|
||||
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)); |
||||
|
||||
for (unsigned i = 0; i < num_ifaces_; i++) { |
||||
ifaces_[i].template construct<uavcan::IPoolAllocator &, uavcan::ISystemClock &, |
||||
pthread_mutex_t &, unsigned>(allocator_, clock_, driver_mutex_, quota_per_queue); |
||||
} |
||||
} |
||||
|
||||
~VirtualCanDriver() |
||||
{ |
||||
Lock::deinit(driver_mutex_); |
||||
event_.deinit(); |
||||
} |
||||
|
||||
}; |
||||
Loading…
Reference in new issue