Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 17:35:22 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Uavcan parameter and reset command line operations

Browse Source
This commit is contained in:
David Sidrane
2015-09-21 18:18:24 -10:00
parent 05ec165ed5
commit 1996d2b55a
3 changed files with 339 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/uavcan/module.mk
+1 -1
View File
@@ -40,7 +40,7 @@ MODULE_COMMAND = uavcan
MAXOPTIMIZATION = -O3
MODULE_STACKSIZE = 3200
WFRAME_LARGER_THAN = 1400
WFRAME_LARGER_THAN = 1416
# Main
SRCS += uavcan_main.cpp \
src/modules/uavcan/uavcan_main.cpp
+308 -18
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
@@ -80,7 +82,8 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
_esc_controller(_node),
_time_sync_master(_node),
_time_sync_slave(_node),
_master_timer(_node)
_master_timer(_node),
_setget_response(0)
{
_task_should_exit = false;
_fw_server_action = None;
@@ -189,6 +192,239 @@ int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
return rv;
}
int UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp)
{
if (resp.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
return std::printf("name: %s %lld\n", resp.name.c_str(),
resp.value.to<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;
@@ -517,24 +753,24 @@ int UavcanNode::run()
memset(&_outputs, 0, sizeof(_outputs));
/*
* Set up the time synchronization
*/
/*
* Set up the time synchronization
*/
const int slave_init_res = _time_sync_slave.start();
if (slave_init_res < 0)
{
warnx("Failed to start time_sync_slave");
_task_should_exit = true;
}
const int slave_init_res = _time_sync_slave.start();
/* When we have a system wide notion of time update (i.e the transition from the initial
* System RTC setting to the GPS) we would call uavcan_stm32::clock::setUtc() when that
* happens, but for now we use adjustUtc with a correction of 0
*/
uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0));
_master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync));
_master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000));
if (slave_init_res < 0) {
warnx("Failed to start time_sync_slave");
_task_should_exit = true;
}
/* When we have a system wide notion of time update (i.e the transition from the initial
* System RTC setting to the GPS) we would call uavcan_stm32::clock::setUtc() when that
* happens, but for now we use adjustUtc with a correction of 0
*/
uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0));
_master_timer.setCallback(TimerCallback(this, &UavcanNode::handle_time_sync));
_master_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000));
@@ -936,7 +1172,7 @@ UavcanNode::print_info()
static void print_usage()
{
warnx("usage: \n"
"\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw}");
"\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw|param [set|get|list|save] nodeid [name] [value]}");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@@ -1022,6 +1258,60 @@ int uavcan_main(int argc, char *argv[])
::exit(0);
}
/*
* Parameter setting commands
*
* uavcan param list <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) {
src/modules/uavcan/uavcan_main.hpp
+30 -1
View File
@@ -38,6 +38,9 @@
#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>
@@ -122,7 +125,11 @@ public:
static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
int fw_server(eServerAction action);
void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
int list_params(int remote_node_id);
int save_params(int remote_node_id);
int set_param(int remote_node_id, const char *name, char *value);
int get_param(int remote_node_id, const char *name);
int reset_node(int remote_node_id);
private:
void fill_node_info();
@@ -133,6 +140,16 @@ private:
int start_fw_server();
int stop_fw_server();
int request_fw_check();
int print_params(uavcan::protocol::param::GetSet::Response &resp);
int get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req);
void set_setget_response(uavcan::protocol::param::GetSet::Response *resp)
{
_setget_response = resp;
}
void free_setget_response(void)
{
_setget_response = nullptr;
}
int _task = -1; ///< handle to the OS task
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
@@ -187,4 +204,16 @@ private:
typedef uavcan::MethodBinder<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);
};