dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #2895 from PX4/master_uavcan_timesync_with_params 

Master uavcan timesync with command line UAVCAN parameter operations
This commit is contained in:
Lorenz Meier 2015-09-23 16:44:27 +02:00
parent 27bf924a40 2a36067cc7
commit 7f7fb7c0e3
6 changed files with 407 additions and 91 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

2
src/lib/uavcan

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

6
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_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;
}
}

360
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;
@ -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,27 @@ int UavcanNode::run()
memset(&_outputs, 0, sizeof(_outputs));
/*
* Set up the time synchronization
*/
const int slave_init_res = _time_sync_slave.start();
if (slave_init_res < 0) {
warnx("Failed to start time_sync_slave");
_task_should_exit = true;
}
/* When we have a system wide notion of time update (i.e the transition from the initial
* System RTC setting to the GPS) we would call uavcan_stm32::clock::setUtc() when that
* happens, but for now we use adjustUtc with a correction of 0
*/
uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0));
_master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync));
_master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000));
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
if (busevent_fd < 0) {
@ -870,7 +1172,7 @@ UavcanNode::print_info()
static void print_usage()
{
warnx("usage: \n"
"\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw}");
"\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]|reset nodeid}");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@ -956,6 +1258,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) {

43
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>
@ -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;
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);
};