fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:PX4/Firmware into cmake-2

Browse Source
This commit is contained in:
Lorenz Meier 2015-09-23 21:17:15 +02:00
commit 0da9b4d715
10 changed files with 431 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
NuttX

@ -1 +1 @@
Subproject commit 8891d035df45c8be570cfbd9419b438679faf7ee
Subproject commit aa15c32058acbda4cc2ee51e97d31e9e49225193

8
README.md
View File

@ -16,7 +16,7 @@ This repository contains the PX4 Flight Core, with the main applications located
* [Downloads](http://px4.io/firmware/downloads)
* Releases
* [Downloads](https://github.com/PX4/Firmware/releases)
* Mailing list: [Google Groups](http://groups.google.com/group/px4users)
* Forum / Mailing list: [Google Groups](http://groups.google.com/group/px4users)
### Users ###
@ -29,7 +29,7 @@ Contributing guide:
* [PX4 Contribution Guide](http://px4.io/dev/contributing)
Software in the Loop guide:
Use software in the loop [to get started with the codebase](https://github.com/PX4/Firmware/tree/master/posix-configs/SITL)
Use software in the loop [to get started with the codebase](https://pixhawk.org/dev/simulation/native_sitl).
Developer guide:
http://px4.io/dev/
@ -38,8 +38,10 @@ Testing guide:
http://px4.io/dev/unit_tests
This repository contains code supporting these boards:
* [Snapdragon Flight](https://www.intrinsyc.com/qualcomm-snapdragon-flight/)
* FMUv1.x
* FMUv2.x
* FMUv2.x (Pixhawk)
* FMUv3.x (Pixhawk 2)
* AeroCore (v1 and v2)
* STM32F4Discovery (basic support) [Tutorial](https://pixhawk.org/modules/stm32f4discovery)

2
src/lib/uavcan

@ -1 +1 @@
Subproject commit 3ae5400aa5ead18139106d30f730114d5e9b65dd
Subproject commit 50dc08663af2d9b55eae0ccf5f07c63db0ee8907

6
src/modules/mavlink/mavlink_main.cpp
View File

@ -1506,9 +1506,11 @@ Mavlink::task_main(int argc, char *argv[])
} else if (strcmp(myoptarg, "onboard") == 0) {
_mode = MAVLINK_MODE_ONBOARD;
} else if (strcmp(optarg, "osd") == 0) {
} else if (strcmp(myoptarg, "osd") == 0) {
_mode = MAVLINK_MODE_OSD;
} else if (strcmp(optarg, "config") == 0) {
} else if (strcmp(myoptarg, "config") == 0) {
_mode = MAVLINK_MODE_CONFIG;
}

8
src/modules/uavcan/module.mk
View File

@ -40,12 +40,11 @@ MODULE_COMMAND = uavcan
MAXOPTIMIZATION = -O3
MODULE_STACKSIZE = 3200
WFRAME_LARGER_THAN = 1400
WFRAME_LARGER_THAN = 1416
# Main
SRCS += uavcan_main.cpp \
uavcan_servers.cpp \
uavcan_clock.cpp \
uavcan_servers.cpp \
uavcan_params.c
# Actuators
@ -70,7 +69,8 @@ override EXTRADEFINES := $(EXTRADEFINES) \
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \
-DUAVCAN_NO_ASSERTIONS \
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48 \
-DUAVCAN_MAX_NETWORK_SIZE_HINT=16
-DUAVCAN_MAX_NETWORK_SIZE_HINT=16 \
-DUAVCAN_STM32_TIMER_NUMBER=5
#
# libuavcan drivers for STM32

81
src/modules/uavcan/uavcan_clock.cpp
View File

@ -1,81 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <uavcan_stm32/uavcan_stm32.hpp>
#include <drivers/drv_hrt.h>
/**
* @file uavcan_clock.cpp
*
* Implements a clock for the CAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
namespace uavcan_stm32
{
namespace clock
{
uavcan::MonotonicTime getMonotonic()
{
return uavcan::MonotonicTime::fromUSec(hrt_absolute_time());
}
uavcan::UtcTime getUtc()
{
return uavcan::UtcTime();
}
void adjustUtc(uavcan::UtcDuration adjustment)
{
(void)adjustment;
}
uavcan::uint64_t getUtcUSecFromCanInterrupt();
uavcan::uint64_t getUtcUSecFromCanInterrupt()
{
return 0;
}
} // namespace clock
SystemClock &SystemClock::instance()
{
static SystemClock inst;
return inst;
}
}

363
src/modules/uavcan/uavcan_main.cpp
View File

@ -55,6 +55,8 @@
#include "uavcan_main.hpp"
#include <uavcan/util/templates.hpp>
#include <uavcan/protocol/param/ExecuteOpcode.hpp>
//todo:The Inclusion of file_server_backend is killing
// #include <sys/types.h> and leaving OK undefined
# define OK 0
@ -77,7 +79,11 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
CDev("uavcan", UAVCAN_DEVICE_PATH),
_node(can_driver, system_clock),
_node_mutex(),
_esc_controller(_node)
_esc_controller(_node),
_time_sync_master(_node),
_time_sync_slave(_node),
_master_timer(_node),
_setget_response(0)
{
_task_should_exit = false;
_fw_server_action = None;
@ -186,6 +192,239 @@ int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
return rv;
}
int UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp)
{
if (resp.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
return std::printf("name: %s %lld\n", resp.name.c_str(),
resp.value.to<uavcan::protocol::param::Value::Tag::integer_value>());
} else if (resp.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
return std::printf("name: %s %.4f\n", resp.name.c_str(),
static_cast<double>(resp.value.to<uavcan::protocol::param::Value::Tag::real_value>()));
} else if (resp.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
return std::printf("name: %s %d\n", resp.name.c_str(),
resp.value.to<uavcan::protocol::param::Value::Tag::boolean_value>());
} else if (resp.value.is(uavcan::protocol::param::Value::Tag::string_value)) {
return std::printf("name: %s '%s'\n", resp.name.c_str(),
resp.value.to<uavcan::protocol::param::Value::Tag::string_value>().c_str());
}
return -1;
}
void UavcanNode::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
{
uavcan::protocol::param::ExecuteOpcode::Response resp;
_callback_success = result.isSuccessful();
resp = result.getResponse();
_callback_success &= resp.ok;
}
int UavcanNode::save_params(int remote_node_id)
{
uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
opcode_req.opcode = opcode_req.OPCODE_SAVE;
uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> client(_node);
client.setCallback(ExecuteOpcodeCallback(this, &UavcanNode::cb_opcode));
_callback_success = false;
int call_res = client.call(remote_node_id, opcode_req);
if (call_res >= 0) {
while (client.hasPendingCalls()) {
usleep(10000);
}
}
if (!_callback_success) {
std::printf("Failed to save parameters: %d\n", call_res);
return -1;
}
return 0;
}
void UavcanNode::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
{
uavcan::protocol::RestartNode::Response resp;
_callback_success = result.isSuccessful();
resp = result.getResponse();
_callback_success &= resp.ok;
}
int UavcanNode::reset_node(int remote_node_id)
{
uavcan::protocol::RestartNode::Request restart_req;
restart_req.magic_number = restart_req.MAGIC_NUMBER;
uavcan::ServiceClient<uavcan::protocol::RestartNode, RestartNodeCallback> client(_node);
client.setCallback(RestartNodeCallback(this, &UavcanNode::cb_restart));
_callback_success = false;
int call_res = client.call(remote_node_id, restart_req);
if (call_res >= 0) {
while (client.hasPendingCalls()) {
usleep(10000);
}
}
if (!call_res) {
std::printf("Failed to reset node: %d\n", remote_node_id);
return -1;
}
return 0;
}
int UavcanNode::list_params(int remote_node_id)
{
int rv = 0;
int index = 0;
uavcan::protocol::param::GetSet::Response resp;
set_setget_response(&resp);
while (true) {
uavcan::protocol::param::GetSet::Request req;
req.index = index++;
_callback_success = false;
int call_res = get_set_param(remote_node_id, nullptr, req);
if (call_res < 0 || !_callback_success) {
std::printf("Failed to get param: %d\n", call_res);
rv = -1;
break;
}
if (resp.name.empty()) { // Empty name means no such param, which means we're finished
break;
}
print_params(resp);
}
free_setget_response();
return rv;
}
void UavcanNode::cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
{
_callback_success = result.isSuccessful();
*_setget_response = result.getResponse();
}
int UavcanNode::get_set_param(int remote_node_id, const char *name, uavcan::protocol::param::GetSet::Request &req)
{
if (name != nullptr) {
req.name = name;
}
uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> client(_node);
client.setCallback(GetSetCallback(this, &UavcanNode::cb_setget));
_callback_success = false;
int call_res = client.call(remote_node_id, req);
if (call_res >= 0) {
while (client.hasPendingCalls()) {
usleep(10000);
}
if (!_callback_success) {
call_res = -1;
}
}
return call_res;
}
int UavcanNode::set_param(int remote_node_id, const char *name, char *value)
{
uavcan::protocol::param::GetSet::Request req;
uavcan::protocol::param::GetSet::Response resp;
set_setget_response(&resp);
int rv = get_set_param(remote_node_id, name, req);
if (rv < 0 || resp.name.empty()) {
std::printf("Failed to retrieve param: %s\n", name);
rv = -1;
} else {
rv = 0;
req = {};
if (resp.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
int64_t i = std::strtoull(value, NULL, 10);
int64_t min = resp.min_value.to<uavcan::protocol::param::NumericValue::Tag::integer_value>();
int64_t max = resp.max_value.to<uavcan::protocol::param::NumericValue::Tag::integer_value>();
if (i >= min && i <= max) {
req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = i;
} else {
std::printf("Invalid value for: %s must be between %lld and %lld\n", name, min, max);
rv = -1;
}
} else if (resp.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
float f = static_cast<float>(std::atof(value));
float min = resp.min_value.to<uavcan::protocol::param::NumericValue::Tag::real_value>();
float max = resp.max_value.to<uavcan::protocol::param::NumericValue::Tag::real_value>();
if (f >= min && f <= max) {
req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = f;
} else {
std::printf("Invalid value for: %s must be between %.4f and %.4f\n", name, static_cast<double>(min),
static_cast<double>(max));
rv = -1;
}
} else if (resp.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
int8_t i = (value[0] == '1' || value[0] == 't') ? 1 : 0;
req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = i;
} else if (resp.value.is(uavcan::protocol::param::Value::Tag::string_value)) {
req.value.to<uavcan::protocol::param::Value::Tag::string_value>() = value;
}
if (rv == 0) {
rv = get_set_param(remote_node_id, name, req);
if (rv < 0 || resp.name.empty()) {
std::printf("Failed to set param: %s\n", name);
return -1;
}
return 0;
}
}
free_setget_response();
return rv;
}
int UavcanNode::get_param(int remote_node_id, const char *name)
{
uavcan::protocol::param::GetSet::Request req;
uavcan::protocol::param::GetSet::Response resp;
set_setget_response(&resp);
int rv = get_set_param(remote_node_id, name, req);
if (rv < 0 || resp.name.empty()) {
std::printf("Failed to get param: %s\n", name);
rv = -1;
} else {
print_params(resp);
rv = 0;
}
free_setget_response();
return rv;
}
int UavcanNode::start_fw_server()
{
int rv = -1;
@ -194,7 +433,7 @@ int UavcanNode::start_fw_server()
if (_servers == nullptr) {
rv = UavcanServers::start(2, _node);
rv = UavcanServers::start(_node);
if (rv >= 0) {
/*
@ -459,6 +698,48 @@ int UavcanNode::add_poll_fd(int fd)
}
void UavcanNode::handle_time_sync(const uavcan::TimerEvent &)
{
/*
* Check whether there are higher priority masters in the network.
* If there are, we need to activate the local slave in order to sync with them.
*/
if (_time_sync_slave.isActive()) { // "Active" means that the slave tracks at least one remote master in the network
if (_node.getNodeID() < _time_sync_slave.getMasterNodeID()) {
/*
* We're the highest priority master in the network.
* We need to suppress the slave now to prevent it from picking up unwanted sync messages from
* lower priority masters.
*/
_time_sync_slave.suppress(true); // SUPPRESS
} else {
/*
* There is at least one higher priority master in the network.
* We need to allow the slave to adjust our local clock in order to be in sync.
*/
_time_sync_slave.suppress(false); // UNSUPPRESS
}
} else {
/*
* There are no other time sync masters in the network, so we're the only time source.
* The slave must be suppressed anyway to prevent it from disrupting the local clock if a new
* lower priority master suddenly appears in the network.
*/
_time_sync_slave.suppress(true);
}
/*
* Publish the sync message now, even if we're not a higher priority master.
* Other nodes will be able to pick the right master anyway.
*/
_time_sync_master.publish();
}
int UavcanNode::run()
{
(void)pthread_mutex_lock(&_node_mutex);
@ -472,6 +753,28 @@ int UavcanNode::run()
memset(&_outputs, 0, sizeof(_outputs));
/*
* Set up the time synchronization
*/
const int slave_init_res = _time_sync_slave.start();
if (slave_init_res < 0) {
warnx("Failed to start time_sync_slave");
_task_should_exit = true;
}
/* When we have a system wide notion of time update (i.e the transition from the initial
* System RTC setting to the GPS) we would call uavcan_stm32::clock::setUtc() when that
* happens, but for now we use adjustUtc with a correction of the hrt so that the
* time bases are the same
*/
uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(hrt_absolute_time()));
_master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync));
_master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000));
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
if (busevent_fd < 0) {
@ -870,7 +1173,7 @@ UavcanNode::print_info()
static void print_usage()
{
warnx("usage: \n"
"\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw}");
"\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@ -956,6 +1259,60 @@ int uavcan_main(int argc, char *argv[])
::exit(0);
}
/*
* Parameter setting commands
*
* uavcan param list <node>
* uavcan param save <node>
* uavcan param get <node> <name>
* uavcan param set <node> <name> <value>
*
*/
int node_arg = !std::strcmp(argv[1], "reset") ? 2 : 3;
if (!std::strcmp(argv[1], "param") || node_arg == 2) {
if (argc < node_arg + 1) {
errx(1, "Node id required");
}
int nodeid = atoi(argv[node_arg]);
if (nodeid == 0 || nodeid > 127 || nodeid == inst->get_node().getNodeID().get()) {
errx(1, "Invalid Node id");
}
if (node_arg == 2) {
return inst->reset_node(nodeid);
} else if (!std::strcmp(argv[2], "list")) {
return inst->list_params(nodeid);
} else if (!std::strcmp(argv[2], "save")) {
return inst->save_params(nodeid);
} else if (!std::strcmp(argv[2], "get")) {
if (argc < 5) {
errx(1, "Name required");
}
return inst->get_param(nodeid, argv[4]);
} else if (!std::strcmp(argv[2], "set")) {
if (argc < 5) {
errx(1, "Name required");
}
if (argc < 6) {
errx(1, "Value required");
}
return inst->set_param(nodeid, argv[4], argv[5]);
}
}
if (!std::strcmp(argv[1], "stop")) {
if (fw) {

45
src/modules/uavcan/uavcan_main.hpp
View File

@ -36,6 +36,12 @@
#include <px4_config.h>
#include <uavcan_stm32/uavcan_stm32.hpp>
#include <uavcan/protocol/global_time_sync_master.hpp>
#include <uavcan/protocol/global_time_sync_slave.hpp>
#include <uavcan/protocol/param/GetSet.hpp>
#include <uavcan/protocol/param/ExecuteOpcode.hpp>
#include <uavcan/protocol/RestartNode.hpp>
#include <drivers/device/device.h>
#include <systemlib/perf_counter.h>
@ -48,7 +54,7 @@
#include "actuators/esc.hpp"
#include "sensors/sensor_bridge.hpp"
# include "uavcan_servers.hpp"
#include "uavcan_servers.hpp"
/**
* @file uavcan_main.hpp
@ -119,8 +125,13 @@ public:
static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
int fw_server(eServerAction action);
void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
int list_params(int remote_node_id);
int save_params(int remote_node_id);
int set_param(int remote_node_id, const char *name, char *value);
int get_param(int remote_node_id, const char *name);
int reset_node(int remote_node_id);
private:
void fill_node_info();
int init(uavcan::NodeID node_id);
void node_spin_once();
@ -129,6 +140,16 @@ private:
int start_fw_server();
int stop_fw_server();
int request_fw_check();
int print_params(uavcan::protocol::param::GetSet::Response &resp);
int get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req);
void set_setget_response(uavcan::protocol::param::GetSet::Response *resp)
{
_setget_response = resp;
}
void free_setget_response(void)
{
_setget_response = nullptr;
}
int _task = -1; ///< handle to the OS task
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
@ -150,6 +171,8 @@ private:
pthread_mutex_t _node_mutex;
px4_sem_t _server_command_sem;
UavcanEscController _esc_controller;
uavcan::GlobalTimeSyncMaster _time_sync_master;
uavcan::GlobalTimeSyncSlave _time_sync_slave;
List<IUavcanSensorBridge *> _sensor_bridges; ///< List of active sensor bridges
@ -175,4 +198,22 @@ private:
perf_counter_t _perfcnt_node_spin_elapsed = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed");
perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed");
perf_counter_t _perfcnt_esc_mixer_total_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed");
void handle_time_sync(const uavcan::TimerEvent &);
typedef uavcan::MethodBinder<UavcanNode *, void (UavcanNode::*)(const uavcan::TimerEvent &)> TimerCallback;
uavcan::TimerEventForwarder<TimerCallback> _master_timer;
bool _callback_success;
uavcan::protocol::param::GetSet::Response *_setget_response;
typedef uavcan::MethodBinder<UavcanNode *,
void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
typedef uavcan::MethodBinder<UavcanNode *,
void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)> ExecuteOpcodeCallback;
typedef uavcan::MethodBinder<UavcanNode *,
void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
void cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);
};

11
src/modules/uavcan/uavcan_servers.cpp
View File

@ -76,7 +76,7 @@
UavcanServers *UavcanServers::_instance;
UavcanServers::UavcanServers(uavcan::INode &main_node) :
_subnode_thread(-1),
_vdriver(UAVCAN_STM32_NUM_IFACES, uavcan_stm32::SystemClock::instance()),
_vdriver(NumIfaces, uavcan_stm32::SystemClock::instance()),
_subnode(_vdriver, uavcan_stm32::SystemClock::instance()),
_main_node(main_node),
_tracer(),
@ -101,7 +101,7 @@ UavcanServers::~UavcanServers()
_main_node.getDispatcher().removeRxFrameListener();
}
int UavcanServers::stop(void)
int UavcanServers::stop()
{
UavcanServers *server = instance();
@ -123,7 +123,7 @@ int UavcanServers::stop(void)
return 0;
}
int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node)
int UavcanServers::start(uavcan::INode &main_node)
{
if (_instance != nullptr) {
warnx("Already started");
@ -140,7 +140,7 @@ int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node)
return -2;
}
int rv = _instance->init(num_ifaces);
int rv = _instance->init();
if (rv < 0) {
warnx("Node init failed: %d", rv);
@ -174,7 +174,7 @@ int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node)
return rv;
}
int UavcanServers::init(unsigned num_ifaces)
int UavcanServers::init()
{
errno = 0;
@ -268,6 +268,7 @@ int UavcanServers::init(unsigned num_ifaces)
return OK;
}
__attribute__((optimize("-O0")))
pthread_addr_t UavcanServers::run(pthread_addr_t)
{

13
src/modules/uavcan/uavcan_servers.hpp
View File

@ -72,9 +72,11 @@
*/
class UavcanServers
{
static constexpr unsigned NumIfaces = 1; // UAVCAN_STM32_NUM_IFACES
static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
static constexpr unsigned MaxCanFramsPerTransfer = 63;
static constexpr unsigned MaxCanFramesPerTransfer = 63;
/**
* This number is based on the worst case max number of frames per interface. With
@ -84,7 +86,7 @@ class UavcanServers
* 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver.
*/
static constexpr unsigned QueuePoolSize =
(UAVCAN_STM32_NUM_IFACES * uavcan::MemPoolBlockSize *MaxCanFramsPerTransfer);
(NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer);
static constexpr unsigned StackSize = 3500;
static constexpr unsigned Priority = 120;
@ -96,8 +98,8 @@ public:
virtual ~UavcanServers();
static int start(unsigned num_ifaces, uavcan::INode &main_node);
static int stop(void);
static int start(uavcan::INode &main_node);
static int stop();
SubNode &get_node() { return _subnode; }
@ -116,8 +118,7 @@ private:
pthread_t _subnode_thread;
pthread_mutex_t _subnode_mutex;
int init(unsigned num_ifaces);
void deinit(void);
int init();
pthread_addr_t run(pthread_addr_t);