diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 77ff77c6d7..9873d87fbe 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -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 \ diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 9daff4b075..96fca21ff1 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -55,6 +55,8 @@ #include "uavcan_main.hpp" #include +#include + //todo:The Inclusion of file_server_backend is killing // #include 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()); + + } 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(resp.value.to())); + + } 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()); + + } 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().c_str()); + } + + return -1; +} + +void UavcanNode::cb_opcode(const uavcan::ServiceCallResult &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 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 &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 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 &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 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(); + int64_t max = resp.max_value.to(); + + if (i >= min && i <= max) { + req.value.to() = 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(std::atof(value)); + float min = resp.min_value.to(); + float max = resp.max_value.to(); + + if (f >= min && f <= max) { + req.value.to() = f; + + } else { + std::printf("Invalid value for: %s must be between %.4f and %.4f\n", name, static_cast(min), + static_cast(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() = i; + + } else if (resp.value.is(uavcan::protocol::param::Value::Tag::string_value)) { + req.value.to() = 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 + * uavcan param save + * uavcan param get + * uavcan param set + * + */ + 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) { diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 098d8ea55d..23e9c89417 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -38,6 +38,9 @@ #include #include #include +#include +#include +#include #include #include @@ -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 TimerCallback; uavcan::TimerEventForwarder _master_timer; + bool _callback_success; + uavcan::protocol::param::GetSet::Response *_setget_response; + typedef uavcan::MethodBinder &)> GetSetCallback; + typedef uavcan::MethodBinder &)> ExecuteOpcodeCallback; + typedef uavcan::MethodBinder &)> RestartNodeCallback; + void cb_setget(const uavcan::ServiceCallResult &result); + void cb_opcode(const uavcan::ServiceCallResult &result); + void cb_restart(const uavcan::ServiceCallResult &result); + };